Maximum likelihood estimation-assisted ASVSF through state covariance-based 2D SLAM algorithm

The smooth variable structure ﬁlter (ASVSF) has been relatively considered as a new robust predictor-corrector method for estimating the state. In order to effectively utilize it, an SVSF requires the accurate system model, and exact prior knowledge includes both the process and measurement noise statistic. Unfortunately, the system model is always inaccurate because of some considerations avoided at the beginning. More-over, the small addictive noises are partially known or even unknown. Of course, this limitation can degrade the performance of SVSF or also lead to divergence condition. For this reason, it is proposed through this paper an adaptive smooth variable structure ﬁlter (ASVSF) by conditioning the probability density function of a measurement to the unknown parameters at one iteration. This proposed method is assumed to ac-complish the localization and direct point-based observation task of a wheeled mobile robot, TurtleBot2. Finally, by realistically simulating it and comparing to a conven-tional method, the proposed method has been showing a better accuracy and stability in term of root mean square error (RMSE) of the estimated map coordinate (EMC) and estimated path coordinate (EPC). This is an open access article under the CC BY-SA license.


INTRODUCTION
The presence of a map can be useful for a mobile robot to perform its navigation task. Unfortunately, the consistent map is unavailable at the beginning [1]. Consequently, the system should be able to track the robot path and to build a map based on the robot measurement [2][3][4]. It can be done by knowing the current pose when sensing a feature and locating the pose based on the around features [5]. Since these tasks should be addressed at the same time, the definition of simultaneous localization and mapping (SLAM) is stated [6][7][8][9][10][11][12]. The SLAM-based mobile robot navigation has intensively received attention because of some challenging factors that need to be solved such as wide uncertainty, system complexity, inaccurate system model, limited prior information, noise statistics of the process and measurement, computational cost and filter divergence [13,14]. Additionally, in the mobile robot application, the successful of solving SLAM problem can be validated root mean square error (RMSE) [15][16][17] calculated based on the different of the estimated and true value. The continuosly grown uncertainty makes the estimated values deviates from its base. For this reason, the probability-based filtering method has been intensively used. It has been frequently adopted to effectively represent all the possibility related to the system [18]. The most popular one is extended kalman filter which has the basic task to update the state and covariance with an assumption all the related variable comply with Gaussian distribution. Generally, extended kalman filter [11,[19][20][21][22][23] is known as an nonlinear version of its predecessor named kalman filter [18][19][20][24][25][26][27][28]. The easiness to apply extended kalman filter makes it has been widely used to solve in many different fields such as for the state and parameter estimation including SLAM, signal processing, fault detection and diagnosis and target tracking [29]. Nevertheless, it has many incompatibilities and difficulties such as the deviant solution from the state trajectory, less optimal state estimation and large estimation error due to the linearization process and computational cost [14,15,17]. These limitations make its practical application becomes limited nowadays.
Furthermore, many researcher have switched to use the similar method that might offered better robustness rather than EKF such as unscented kalman filter (UKF) [20,[30][31][32], cubature kalman filter (CKF) [15,16,26] smooth variable structure filter (SVSF) [15][16][17]33], etc. Their recorded successes have been proving to have significant improvement of EKF. Among of them, SVSF has been rapidly developed. The SVSF is a relative new predictor-estimator, which is first invented in 2007 [15,16]. Firstly, it was derived from a variable structure filter (VSF) [34] and extended variable structure filter (EVSF) [13,17,35]. Then proceed with a presence of new form by completing it with the error covariance matrix without affecting its accuracy and stability [35,36]. As a common filtering technique, it was then enhanced by involving the time-varying boundary layer width to replace its previous characteristic [36,37]. Due to these developments, SVSF becomes the popular method to against the uncertainty which is not only suitable for the linear but also nonlinear system [15][16][17]. Additionally, based on its characteristic, the SVSF can be combined with different methods in obtaining the optimal solution [15-17, 33, 34]. However, like the other traditional filter methods, to apply SVSF the noise statistic is often predefined to be fixed and constant for the whole estimation process. It often leads to divergence and degradation performance. Thus, traditional SVSF should at least be enhanced to partially estimating the noise statistic. Therefore, through this paper, SVSF is adaptively improved. Firstly, the SVSF's error covariance matrix is mathematically derived via maximum a likelihood estimator [38]. It aims to recursively update the process covariance matrix Q as well as the measurement error covariance matrix R. Since the prediction step of SVSF [15,16,33,34] is totally same as EKF, so that, the innovation error can be similarly computed. Consequentially, the derivation process of finding the compact formulation of Q and R is easy to be evaluable [31,38]. Additionally, since this algorithm is used to solve SLAM problem, henceforth it is termed as ASVSF-SLAM algorithm in this paper. In case of knowing the performance of this algorithm, the proposed algorithm is realistic simulated and compared with traditional SLAM algorithm. Based on the comparative results, it can be declared that the ASVSF-SLAM algorithm can significantly solve the SLAM problem of wheeled mobile robot in term of RMSE for both estimated path coordinate and estimated map coordinate.
The rest parts of this paper are arranged as follows. Section 2 presents a discussion of the original SVSF. Section 3 sequentially presents the mathematical derivation conducted to find the recursive error covariance matrix of both the process and measurement noise statistic. Section 4 discuss an algorithm named ASVSF-SLAM algorithm with expansion of kinematic configuration and motion model, direct point-based observation and, inverse point-based observation. Finally, Section 5 presents a conclusion according to the result discussed in previous section

CLASSICAL SVSF
Considering that the dynamic model of Gaussian nonlinear system is given as follows where k is discrete time index, x ∈ R n is the state vector, u is the control vector and z ∈ R m is the measurement vector. While, ω ∈ R n and ν ∈ R m are small process, and measurement noise, respectively. Whereas, f (.) and h (.) are the nonlinear function and measurement model, respectively. The statistical characteristic of this dynamic model is given as follows and Cov [, ] are mean and covariance term, respectively. Then the complete formulation of SVSF can be chainned as followŝ x k|k =x k|k−1 + K SV SF k e z,k|k−1 (10)

ESTIMATING THE NOISE STATISTIC
Traditionally, SVSF has no ability to approximate the responsive noise statistic. Consequently, the possibility of divergence still high in case of either the realistic simulation or real application. Moreover, inaccurate modelled system might also increase the uncertainty that is precisely lead to degradation of filter performance. For this reason, an effort to complete SVSF with recursive noise statistic is proposed in this paper. This process can be done by estimating error matrix of the noise statistic by deriving the predicted error covariance matrix of the state using MLE [31,38].
First, by recalling the innovation sequence of SVSF and considering that the nonlinear system (1) is Gaussian. According to [38,39] and [6], S k can also be alternatively calculated aŝ where d k is innovation sequence error as shown in (5),Ĉ k is alternative form of S k , and the addition of k j=k−N +1 d j d T j is the moving window [40] for N refers to the window size. Now, by recalling as shown in (2) and assuming that the unknown parameter α = {Q, R}, the adaptation process is done with assumptions that State vector x is not depend on α i.e ∂x ∂α , F and H are independent of α and time variant, k is white and ergodic sequence within the estimation window, and S k is regarded as the main key of adaption on depend parameters. Therefore, the probability density function of the measurements conditioned on the parameter α at time k can be described as follows [31,38] Then by taking its algorithm, and ignoring all the constants, the compact formulation of shown in (15) is Next, by addopting two relations of matrix differential calculus [38], (16) is derived as follows At this point, it is clear that the adaptive SVSF lies on the determination of C and its partial derivative with respect to α. Additionally, the main interest is in R k and Q k instead of C [31,41,42] . Therefore, by sequentially referring to (6), first derivative of C with respect to α, and a condition of P k−1|k−1 in the steady state, (17) can be compactly expressed as follows k j=k−N +1 At this point, the process of obtaining both the adaptive estimation of the process noise and measurement noise covariance are presented. First, Q is assumed to be known and independent of α to obtain the explicit expression for R. Similarly, the process to gain the expression for Q will also involve the assumption that R is fixed and independent of α at the previous process. Both processes can be sequentially described at the following subsection

Adaptive covariance matrix of process noise statistic
Given R then (18) becomes Then by referring to (7) after replacing S k by C k , the alternative formulation of C −1 k is obtained as follows Since, ψ −1 AH + is gain of SVSF on (9), and the saturation function on (8) is satisfied at the previous step, then it is clear to have the following form Since (21) is formed then by substituting (21) where P j is P k|k−1 . By definition, it should be at least semi-definite positive matrix, thus P −1 j can be vanished.
As well-known that the alternative formulation of (11) is P k|k = P k|k−1 − HK SV SF k P k|k−1 , thus Now, substituting (10) and (24) into (23), it yields k j=k−N +1 Note that P + and P − refer to P k|k and P k|k−1 , respectively. Meanwhile,x + andx − refer tox k|k andx k|k−1 , respectively. Then by substituting (4) into (25), it yields k j=k−N +1 Finally, the covariance matrix of process noise statistic is obtained Suppose that (24) alternatively represents the updated covariance of SVSF. It is totally same with Kalman Filter, then (26) can be approximately reformed aŝ

Adaptive covariance matrix of measurement noise statistic
Similarly, since Q is fixed and independent of α, then (18) becomes k j=k−N +1 It can also be simplified as then by deriving (30), it yields Now since 1 N k j=k−N +1 d j d T j is the way to calculate the expectation of d j d T j , which is also as shown in (2), then the recursive measurement error covariance matrix iŝ Mathematical derivation above showing that the adaptive covariance matrix of measurement noise statistic seems able to be adopted from the value used at the previous iteration. Finally, both the process and measurement noise statistic are obtained already when their corresponding mean are considered to be zero.

ADAPTIVE SVSF-BASED FEATURE 2D SLAM ALGORITHM
The proposed method is approached to address a feature-based SLAM problem of the wheeled mobile robot [14,17]. The objective is concurrently estimating the robot pose and feature in certain environment. It is assumed that by using the motion model the robot moves after executing the control command, and by using laser scanner-based measurement, it senses the features with distance and bearing as the gathered data [3,4,43,44]. It is noted that all the prerequisites for a feature-based SLAM algorithm in [14] and [17], therefore,

RESULTS AND DISCUSSION
The proposed algorithm discussed above is realistically simulated and applied for solving SLAM problem of wheeled mobile robot. Initially, the robot pose and covariance as well as all the completeness parameters for ASVSF are initialized as followŝ Note that all the parameters shown herein are adopted from the real robot platform, Turtlebot2, which equipped with laser scanner [4,43,14] in displacement d ls = 14cm. By realistically measuring the distance between two separately powered wheels, it is obtained W r = 33cm. It assumed that this mobile robot is operated in planar indoor environment with the size and random point as described in Figure 1 that is served as the reference path and map in this experiment. The validation is conducted based on RMSE of different algorithm in estimating the robot path and map. The validation is conducted based on RMSE of different algorithm in estimating the robot path and map. There are two different condition of the initial noise statistic in order to see the consistency of the proposed algorithm as shown in Table 1. All the parameters presented in Table 1 aim to know the performance of the proposed algorithm when the initial process and measurement noise statistic are increased in type of additive white Gaussian noise. All the parameters presented above aim to know the performance of the proposed algorithm when the initial process and measurement noise statistic are increased in type of additive white Gaussian noise. The scenario is involved to validate the stability of the proposed ASVSF-SLAM algorithm compared to conventional SVSF-SLAM algorithm. Regarding to these parameters and the reference path depicted in Figure 1 the following results were obtained. It analogized that the robot moves based on all the command send to the odometers. It aims to track the reference path as well as locating new detected landmark in the environment then by applying two different algorithm which are SVSF-SLAM and Adaptive SVSF-SLAM algorithm, the performance of the robot can be graphically expressed as shown in Figure 2.  Figure 2 represents the performance of different SLAM algorithm. They are attempted to estimate the path and map coordinate by respecting to the reference trajectory. According to different simulations, both SVSF and ASVSF-based SLAM algorithm show the convergence to track the reference. Additionally, the proposed method shows a smoother performance with a guaranteed stability when the noise statistic is increased. However,it is quite difficult to know the detail diversity of the SLAM algorithms through the graphical representation only.
Therefore, to ease the validation and analysis, their estimated path and estimated map coordinate are compared in term of root mean square error. Figure 3 depicts the difference RMSE values for SVSF and ASVSF-SLAM algorithms. According to two different simulation, it is clear to see that the adaptive SVSF gives smaller RMSE for the estimated robot path in which there is no much diversity to its reference. By using the same method and term, the estimated map of SVSF and ASVSF-SLAM algorithm is also compared. According to Figure 4 the stability and accuracy of ASVSF-SLAM is validated. There is no significant effect after increasing the initial noise statistic. Therefore, it can be stated that since no degradation detected, ASVSF-SLAM algorithm is better than SVSF-SLAM algorithm. Referring to all the graphical result above, it might be difficult to see the difference. For this reason, the following Tables 2 and 3 are presented. In which, these tables are intended to give detail values for all test as the way to validate the accuracy for each algorithm.

CONCLUSIONS
The main contribution of this paper is to equip the traditional SVSF with an approach termed as adaptive filtering strategy. It utilizes the maximum likelihood estimation (MLE) used to approximate the error covariance matrices of noise statistic by conditioning the probability density function of measurement to an unknown parameter at one iteration. The output of this project is the enhancement of SVSF. It can recursively calculate the covariance of the noise statistic based on the uncertainty condition of the previous process. In other words, the predetermined covariance of the noise statistic is executed once at the first iteration and the system continuously generates new covariances for the rest iteration. The proposed method is applied to solve the feature-based SLAM problem of a wheeled mobile robot named ASVSF-based SLAM algorithm. The presence of adaptive strategy prevent the SVSF from degradation and divergence condition under time integration when it is applied for the real case. Regarding to all the comparative results, the accuracy, stability, and robustness of the proposed algorithm is guaranteed and satisfied. 7.