ADAPTIVE DEVELOPMENT OF SVSF FOR A FEATURE- BASED SLAM ALGORITHM USING MAXIMUM LIKELIHOOD ESTIMATION AND EXPECTATION MAXIMIZATION

The smooth variable structure filter (SVSF) has been considered as the robust estimator. Like other filters, the SVSF needs an accurate system model and known noise statistics to approximate the posterior state. Unfortunately, the system cannot be accurately modeled, and the noise statistic is unknown in the real application. For these reasons, the performance of SVSF might be decreased or even led to divergence. Therefore, the enhancement of SVSF is required. This paper presents an Adaptive SVSF. Initially, SVSF is smoothed. To provide the ability to estimate the noise statistic, ASVSF is then derived based on maximum likelihood estimation (MLE) and expectation-maximization (EM). Additionally, the unbiased noise statistic is also approached. However, its covariance is complicatedly formulated. It might cause a negative definite symmetric matrix. Therefore, it is tuned based on the innovation covariance estimator (ICE). The ASVSF is designed to solve the online problem of Simultaneous Localization and Mapping (SLAM). Henceforth, it is termed as the ASVSF-SLAM algorithm. The proposed algorithm showed better accuracy and stability compared to the conventional algorithm in terms of root mean square error (RMSE) for both Estimated Path Coordinate (EPC) and Estimated Map Coordinate (EMC). ABSTRAK: Penapis struktur bolehubah lembut (SVSF) telah dianggap sebagai penganggar teguh. Seperti penapis lain, SVSF memerlukan model sistem yang tepat dan statistik hingar yang diketahui bagi menganggar keadaan posterior. Malangnya, sistem tidak dapat dimodelkan dengan tepat dan statistik hingar tidak diketahui dalam aplikasi sebenar. Atas sebab-sebab ini, prestasi SVSF mungkin berkurangan, bahkan berbeza. Oleh itu, memperbaharui SVSF adalah perlu. Kajian ini adalah mengenai SVSF Mudah Suai. Pada awalnya, SVSF dilembutkan. Bagi menyediakan keupayaan anggaran statistik hinggar, ASVSF dihasilkan terlebih dahulu berdasarkan anggaran kemungkinan maksimum (MLE) dan maksimum-harapan (EM). Tambahan, statistik hinggar yang tidak berat sebelah juga dibuat. Walau bagaimanapun, rumusan formula kovarians ini adalah kompleks. Ini mungkin menyebabkan matriks simetri menjadi negatif. Oleh itu, ia diselaraskan berdasarkan penganggar kovarians inovasi (ICE). ASVSF dibina bagi menyelesaikan masalah dalam talian Penempatan dan Pemetaan Serentak (SLAM) dalam talian. Oleh itu, ia disebut sebagai


INTRODUCTION
It has been stated that the role of a consistent map of a complex environment can significantly help the robot to conduct the navigation task [1], [2]. However, the robot is commonly blind from this environment. Therefore, the robot should be completed to construct the map used to navigate itself and concurrently locate the position of the robot used [1] for the initial base of the mapping task. Theoretically, it is known as Simultaneous Localization and Mapping (SLAM) problem) [3]- [6]. Due to this widespread problem's challenges, the SLAM has been attracting much attention from different researchers. As the manner to fulfill the objective to estimate the robot path and static map, the estimation based on the probability principle has been frequently proposed, such as Extended Kalman Filter (EKF) [5]- [9]. The use of EKF has been limited because of the consistency issue [10].
For this reason, the Smooth Variable Structure Filter (SVSF) [10]- [13] is often chosen as the alternative filtering method of EKF. The SVSF is relatively considered a new predictorestimator that adopts the sliding mode [13]- [15]. The switching gain is utilized as a way to reach the convergence by forcing the estimated values to always on the boundary of the truth estimates. Similar to the former implementation of EKF, which keeps the noise statistic to be invariant under the step increment, SVSF has been proven to successfully solving the featurebased online SLAM problem [16]- [19]. Fundamentally, the SVSF requires the accurate system model and known characteristic of the noise statistic. Unfortunately, these orders are often unavailable. Therefore, it still poses a risk of divergence and filter degradation quality [18], [20]. In order to cover this possibility, its conventional form needs to modify and enhance [21]- [24].
The rest parts of this paper are organized as follows. Section II presents the formulation of SVSF. Section III presents the adaptive SVSF with mathematical derivation. The first solution calculated using the MLE and EM, the enhanced SVSF, the unbiased MLE-estimator, its time- varying, and the addition of ICE. Section IV presents all the prerequisites to build the SLAM algorithm as well as presents the feature-based online SLAM algorithm referring to the proposed method. Section V presents a discussion about the results. Section VI presents the conclusion.

CLASSICAL SMOOTH VARIABLE STRUCTURE FILTER
Given a nonlinear Gaussian system as shown below, where is the discrete time index, ∈ ℝ refers to the state vector, is the control vector, and ∈ ℝ is the measurement vector. Meanwhile, ω ∈ ℝ is the additive noise following the process and ν ∈ ℝ is the additive noise of the measurement. Therefore, once (. ) and ℎ(. ) are assumed as the transition and measurement function, respectively, the characteristic noise of Eq. (1) is expressed as follows where is well-known as the Kronecker delta function. Meanwhile, [. ] is term used to indicate the expectation or mean and [, ] is used to indicate the covariance term.
̿̿̿̿̿̿ refers to the diagonal term, γ refers to the constant indicating the convergence rate 0 < γ ≤ 1, ψ refers to the boundary layer width, ∈ ℝ correspondences to the covariance matrix of the noise statistic relative to the process and ∈ ℝ correspondences to the covariance matrix of the noise statistic relative to the measurement and is Jacobian matrix of the measurement. Meanwhile, the sign of . + and ∘ are used to indicate the function of the pseudo-invers and Schur matrix multiplication, respectively. The stability of SVSF and its convergence to the existence of the subspace are satisfied if the absolute error about the posterior is smaller than the prior one [11], [12], [47]- [49]. Mathematically, it can be described as follows

ADAPTIVE SMOOTH VARIABLE STRUCTURE FILTER
Assuming Eq. (1) contains the unknown parameters θ = ( , , , ); then, using the Maximum Likelihood Estimation, its estimates θ can be calculated as follows Note that the likelihood function of θ is ( , , , | , ), which can be expanded as Then by taking logarithm Eq. (16), it yields

Expectation-Maximization (EM) Estimation
According to Eq. (1) -Eq. (17) and aiming to estimate noise statistics about the process and measurement together with the corresponding covariances, the principle of Expectation-Maximization is involved. Estimating particular function based on EM are done by taking the expectation of the objective function, and sequentially maximizing its result [7], [40]- [43].

Expectation Process (E-Step)
The expectation process can be done by first taking the conditional expectation and sequentially equating the result to zero, as shown below.

Maximization Process (M-Step)
Up to this point, the unknown parameters relative to the noise can be approximated by maximizing [ln[ ( , , , | , )]]. It can be done by calculating its partial derivative concerning all elements of the unknown parameter and equating it to zero. Correspondingly, the suboptimal of are determined as follows Suwoyo According to Eq. (22) -Eq. (25), it can be evaluated that the multistep smoothing term of | and −1| seems unobservable. Therefore, as the effort to proceed derivation process and prevent solution from inefficiency, | and −1| are respectively replaced by | and −1| . Thus, the new forms of suboptimal solutions are obtained as Although the suboptimal formulation relative to the noise statistics are calculated already, they contain the lack of estimates values, −1| , which is unavailable from the original form of SVSF. For this reason, the SVSF is smoothed and improved using a one-step smoothing point [7], [15], [20]. Using Eq.

Unbiased Estimate Noise Statistic
Up to this point, it seems to have clear derived formulation, but the simplification shown in Eq. (26) -Eq. (29) might reduce the quality as well as leads to bias condition. For this reason, the unbias estimation is approached to guarantee the optimality of Eq. (26) -Eq. (29). This stage can be sequentially conducted as follows; By definition, it is known that Now, by substituting Eq.
then the equivalent form of Eq. (26) -Eq. (29) are respectively presented as follows Since the innovation , | −1 and its covariance , | −1 , | −1 are the parts on the process and measurement noise estimator, it is clear to have In order to obtain the expectation form of the error covariance noise statistic for both the process and measurement, the Joseph covariance form is used.
It is clear, once the unbiased estimates are also satisfiying , | −1 , | −1 = | −1 + , therefore, the following forms are determined Then all expectation values of relative to the error matrices are

Time-Varying Unbiased Noise Statistic
Once, the unbiased form are calculated, the time-varying forms can also be calculated. According to Eq. (50) -Eq. (53), they are respectively expressed as follows where ̂, , ̂, and ̂ are −1 , −1 , −1 , and −1 , respectively. Moreover, as an effort to keep its stability, the exponential 1 in Eq. (54) -Eq. (57) are replaced with the weighting coefficient [7], [15], [18], [22], [30], [37]. Additionally, the use of Innovation Covariance Estimator (ICE) is involved to depress the posibility of negative definite matrices Eq. (56) -Eq. (57). Then, the final formulation for covariance matrices relative to the noise statistic of the process and measurement are expressed as follows Once, the adaptive SVSF is designed with addition of the smoothing formulation (see Eq. (1) -Eq. (12) and Eq. (32) -Eq. (33)) and the time-varying of the noise statistic Eq. (54) -Eq. (55) and Eq. (58) -Eq. (59), the adaptive SVSF-based SLAM algorihtm can also be designed. The design involves all the configuration and completeness that have been introduced in [15]. It is noted that this involvement includes the motion model, direct point-based observation, and inverse point-based observation. Accordingly, the compact pseduo-code of this algorithm can be described as follows: Algorithm ASVSF-Based SLAM Require : Initial State Estimate, Covariance, Convergence Rate, and Initial Error 1: loop 2: Prediction Step: If proprioceptive data is available 3: Propagate the state estimate 4: Compute the Jacobian of f(.) 5: Propagate the covariance relative to the state 6: Update: If the observation data is available 7: Compute the innovation sequence error 8: Calculate Gain 9: Update the State, and Covariance 10: Compute the noise statistic 11: endloop

RESULT AND DISCUSSION
In order to validate its stability, the proposed method is realistically simulated before it is compared to the conventional one. All the parameters relative to the robot configuration are adopted from Turtlebot2, as stated below. It is also assumed, the robot is also completed with a 2D-LIDAR used to get the measurement data, distance and bearing. According to the initial noise statistics, there would be two different simulation cases presented in this paper.
• First Test The initial process and measurement noise are considered as follows These noises will be executed once initially, and for the next iteration, they will be recursively provided by the proposed algorithm. To validate the effectiveness and robustness of the proposed method some comparative results are presented as follows. Graphically, it is hard to evaluate the difference, so that Fig. 2 and Fig. 3 are presented. Fig. 2 shows that the proposed method has been successfully presenting the better-estimated path than then SVSF-SLAM algorithm. Although the small difference between ISVSF-SLAM, ASVSF-MLE-EM cannot be observed from this figure. So that to ease our analysis, Table 1 is presented.
Additionally, the algorithms mentioned above are also compared in terms of RMSE for the estimated map. Fig. 3 demonstrates that the proposed method has been reducing the RMSE given by the SVSF-SLAM algorithm. Thus, it can be noted that the ASVSF-SLAM based on MLE-EM has been successful in upgrading the quality of the classical method.
Moreover, according to Fig. 3, the effectiveness of one-step smoothing has been proven to stably the SVSF-SLAM algorithm.  In the second case, the initial noise statistics both for process and measurement were increased. It aims to examine the stability of the proposed algorithm under an uncertain predetermined noise statistic. Similarly, Fig. 4 also demonstrates different filter performances in solving the SLAM problem of wheeled mobile robots. It also verifies that the proposed method's properness is satisfied with locating the robot position and mapping the features even there is an increment of small additive noise to both the process and measurement.
According to Fig. 5, it can be noted that the increment of the defined noise statistic has no effect on the proposed method stability. It is shown by its ability to reduce RMSE of the SVSF-SLAM algorithm. At this point, it can be stated that the proposed method can guarantees stability better than the classical one. Next, to validate the proposed algorithm stability, the different approaches in estimating map are compared (see Fig. 6). Fig. 6 illustrates that the stability of the proposed method in mapping the feature is satisfied even after increasing the statistics of the initial noise. According to Fig. 6, the proposed method also presents better quality compared with the SVSF-SLAM algorithm. The effectiveness of one-smoothing is again proven, referring to its best achievement in estimating the map. Similarly, to clearance the differences in the simulation results discussed above, Table 2 11.1995 According to Table 1 and Table 2, the smallest RMSE is shown by the ISVSF-Based SLAM algorithm. It verifies that the one-step smoothing technique has been successfully enhancing the classical SVSF without losing the SVSF characteristic. However, since it can estimate the noise statistic, its performance cannot guarantee effectiveness when applied in the real application. Regarding these differences, the proposed method has been presenting excellent performance.
• Third Test ASVSF-SLAM Algorithm Based on MLE-EM with and without ICE In order to evaluate the existence of Innovation Covariance Estimation, the following figures (see Fig. 7 and Fig. 8) are presented. These results refer to the initial noise statistic as defined in the Second Test above.   (Third Test) At this point, it can be noted that the differences depicted by Fig. 7 and Fig. 8 verify that the Innovation Covariance Estimator (ICE) has been successfully reducing RMSE of the original ASVFV-SLAM algorithm. To ease our analysis, Table 3 is also presented  11.1995 According to Table 3, the proposed method with ICE has been showing better performance compared to other algorithm in term of RMSE of EPC and EMC

CONCLUSION
This paper presents a proposed method termed as ASVSF-SLAM Based on MLE-EM creation. The contributions can be summarized as follows; improving the normal SVSF based on a one-step smoothing method, providing an ability to estimate the noise statistic to normal SVSF, providing the time-varying unbiased noise statistic; completing the recursive unbiased noise statistic with innovation covariance estimation. Following all the stages and validating the ASVSF-SLAM algorithm's performance in terms of RMSE has been proving the robustness and effectiveness of the proposed method. Additionally, by referring to the convergence result, which is shown by its performance in different and increment of the initially predetermined noise statistic, the proposed method guarantees the stability compared to the conventional one.