SINERGI Vol.
No.
February 2020: 37-48 http://publikasi.
id/index.
php/sinergi http://doi.
org/10.
22441/sinergi.
A MAPAEKF-SLAM ALGORITHM WITH RECURSIVE MEAN AND
COVARIANCE OF PROCESS AND MEASUREMENT NOISE
STATISTIC
Heru Suwoyo1,2*.
Yingzhong Tian1,3.
Wenbin Wang4.
Md Musabbir Hossain1.
Long Li1,3 1 Mechatronic Engineering.
School of Mechatronic Engineering and Automation.
Shanghai University.
Baoshan, 200444.
Shanghai.
China 2Department of Electrical Engineering.
Universitas Mercu Buana Jakarta 11650.
Indonesia 3Shanghai Key Laboratory of Intelligent Manufacturing and Robotics.
Shanghai, 200444.
China 4Mechanical and Electrical Engineering School.
Shenzhen Polytechnic.
Guangdong, 518055.
China *Corresponding Author Email: heru.
suwoyo@mercubuana.
Abstract -- The most popular filtering method used for solving a Simultaneous Localization and Mapping is the Extended Kalman Filter.
Essentially, it requires prior stochastic knowledge both the process and measurement noise statistic.
In order to avoid this requirement, these noise statistics have been defined at the beginning and kept to be fixed for the whole process.
Indeed, it will satisfy the desired robustness in the case of simulation.
Oppositely, due to the continuous uncertainty affected by the dynamic system under time integration, this manner is strongly not recommended.
The reason is, improperly defined noise will not only degrade the filter performance but also might lead the filter to divergence condition.
For this reason, there has been a strong manner well-termed as an adaptivebased strategy that commonly used to equip the classical filter for having an ability to approximate the noise statistic.
Of course, by knowing the closely responsive noise statistic, the robustness and accuracy of an EKF can increase.
However, most of the existed Adaptive-EKF only considered that the process and measurement noise statistic are characteristically zero-mean and responsive Accordingly, the robustness of EKF can still be enhanced.
This paper presents a proposed method named as a MAPAEKF-SLAM algorithm used for solving the SLAM problem of a mobile robot.
Turtlebot2.
Sequentially, a classical EKF was estimated using Maximum a Posteriori.
However, due to the existence of unobserved value.
EKF was also smoothed one time based on the fixed-interval smoothing method.
This smoothing step aims to keep-up the derivation process under MAP creation.
Realistically, this proposed method was simulated and compared to the conventional Finally, it has been showing better accuracy in terms of Root Mean Square Error (RMSE) of both Estimated Map Coordinate (EMC) and Estimated Path Coordinate (EPC).
Keywords: Simultaneous Localization and Mapping.
Adaptive Extended Kalman Filter.
Maximum a Posteriori.
Fixed-Interval Smoothing Method.
Root Mean Square Error Copyright A 2019 Universitas Mercu Buana.
All right reserved.
Received: July 14, 2019
Revised: November 27, 2019
INTRODUCTION
In a complex environment, a map is useful for autonomous robot navigation.
However, the robot has no knowledge about the environment at the beginning.
Therefore, to appropriately perform the navigation tasks, the autonomous robot should have the ability to build its map and simultaneously locate its current position.
It is well-known as simultaneous localization and mapping (SLAM), which was first introduced in 1988 .
Recently, the SLAM-based mobile robot navigation has intensively received much Accepted: December 3, 2019 attention because of some challenging factors such as continuous uncertainty, system complexity, inaccurate system model, limited prior information, noise statistics of the process and measurement, computational cost and filter divergence which are required to be addressed .
, 4, .
AA.
An effort familiarly used to obtain an effective solution for the SLAM problem is proposed the probability-based that has been effectively and commonly used by others researchers such as Extended Kalman Filter .
, 7, .
Suwoyo et al.
A MAPAEKF-SLAM Algorithm with Recursive Mean and Covariance of A SINERGI Vol.
No.
February 2020: 37-48 Extended Kalman Filter (EKF) has become a popular choice to solve SLAM Weingarten and Siegwart .
used EKF for solving the full SLAM problem of AUV in especially estimating the local path traveled by the robot while forming the scan as well as its uncertainty and keeping the scans pose.
Weingarten and Siegwart utilized an EKF as the SLAM-algorithm to track the robot when it moves and to incrementally update the Symmetric and Perturbation model (SPmode.
of the 3D reconstructed stochastic map .
Lemaire.
Lacroix, and Soly adopted EKF to solve the bearing only 3D SLAM by estimating the parameters used for landmark initialization process such as the visual point features tracked in the sequence of the acquired images .
Moreover, regarding survey consequences.
Dissanayake.
Newman.
Clark.
Durrant-Whyte, and Csorba have proven the SLAM problems with the performance of EKF by developing zero uncertainty of estimated map and absolute accuracy of the map .
Huang.
Mourikis, and Roumeliotis improvement by analyzing the issue of filter consistency of extended Kalman filter based SLAM, from an observability perspective .
examining the observability properties of the nonlinear SLAM system model with the linearized error-state model employed in the EKF.
The observable subspace of the standard EKF is constantly of higher dimension than the observable subspace of the underlying nonlinear Ahmad and Namerikawa declared that EKF based mobile robot localization with intermittent measurements is examined by characteristics .
The uncertainties bound the estimation by analyzing the measurement innovation to preserve good estimations, although sometimes measurement data are They also have proposed the theoretical analysis of the EKF to show the situations during the problem that occurred.
Besides, the Jacobian transformation is considered one of the main factors to affect the estimation performance.
In addition, initial state covariance, process, and measurement noises must be less to execute better estimation results.
Wang.
Wu.
Zhou, and He have used State Transformation Extended Kalman Filter (STEKF) mechanization method to resolve the inconsistency problem of EKF, which is improving the propagation rates of the system matrix and the error covariance matrix .
However, these proposed methods cannot estimate the noise statistics.
Besides that, an EKF has complications of a slow convergence rate, low accuracy, and poor numerical stability as mentioned by Gadsdenin 2011 .
, 5, .
For this reason, the adaptive-based approaches have been popularly attempted and utilized Akhlaghi.
Zhou, and Huan have proposed the AEKF, which can approximate covariance matrices of the process and measurement noise It was conducted by referring to innovation and residual for improving the dynamic state estimation accuracy of EKF .
Jetto.
Longhi, and Venturini have proposed an adaptive EKF for optimizing the linearization between KF and EKF .
They believed that it could be conducted by adjusting the input and measurement noise covariance matrices.
Chatterjee and Matsuno have proposed the AEKF based on neuro-fuzzy to estimate the elements of covariance matrices .
It aims to reduce the mismatch between the theoretical and actual covariance of the innovation Moreover.
Yuzhen.
Quande, and Benfa have proposed AEKF by using the SageHusa time-varying noise estimator and Taylor series of sampling time in AEKF to estimate observation noise in real-time .
It aims to overcome the linearization error and enhance environmental adaptability.
Similarly, to provide proper filtering method-based solution for the SLAM problem, this paper also presents a proposed method named as A MAPAEKF based SLAM algorithm (MAPAEKF-SLAM Algorith.
Initially, the classical EKF was estimated by utilizing the creation of Maximum a Posterior (MAP) aims to approximate the posterior values of both the process and measurement noise statistic with their corresponding covariances.
A smoothed EKF estimate value continuously tuned the suboptimal estimated values.
It is obtained based on a fixed-point smoothing method as utilized by Caballero.
Hermoso.
Jimynez, and Linares in 2003 .
A as well as by Gao.
Li.
Zhou, and Li in 2015 .
AA.
Next, the estimated values were mathematically derived to obtain the timevarying noise statistic.
However, due to the complexity of deriving these parameters, some certain approach was also involved.
Moreover, the proposed method is approached to address the SLAM problem of the wheeled mobile robot.
Then it was utilized and Adaptively, the role of Maximum a Posterior was used to estimate the unknown parameter of a classical EKF for both the process and measurement noise statistic with their corresponding covariances.
Henceforth, it Suwoyo et al.
A MAPAEKF-SLAM Algorithm with Recursive Mean and Covariance of A p-ISSN: 1410-2331 e-ISSN: 2460-1217 is termed as MAPAEKF-SLAM.
It was simulated and compared with the classical algorithm in terms of RMSE of estimating both path and map namely.
EPC and EMC, respectively.
The comparative simulated result has been realistically proving that the proposed method has good and better accuracy, stability and The rest parts of this paper are organized as follows.
Section II contains a classical EKF.
Section i presents the adaptive EKF with process derivation of the first solution given by MAP, improved/modified EKF, weight exponent Section IV presents the MAPAEKFSLAM algorithm which is expanded with the discussion of the motion model, direct pointbased observation, and inverse point-based Section V presents comparative results and discussion.
Section VI presents the conclusion.
METHOD
Classical Extended Kalman Filter (EKF) Through this paper.
EKF is considered as the main filtering method used to estimate the path traveled by the robot when it moves.
Besides that, it is utilized to locate all the features on the environment at the same time.
Since the localization of the robot is only for recording the current pose of the robot and single-location of the feature/landmark, it can declare that this work is focused on solving the online SLAM problem.
Considering the nonlinear system has a model as shown below where refers to discrete-time index, state vector, is control vector, measurement vector, small adaptive process and measurement noise.
While, are the nonlinear function and measurement model, respectively.
The characteristic of this dynamic model .
is described as follows.
refer to the process and measurement covariance matrix, respectively.
While refers to a Jacobian matrix of the transition function with respect to prior state and refers to a Jacobian matrix of the measurement function predicted state vector MAP based Adaptive EKF (MAPAEKF) Classical EKF requires the known noise statistic and an accurate system model.
Unfortunately, the noise is partially known, and the model is might change because of continued uncertainty in time integration.
Thus, an improvement of EKF is required as presented in this paper.
An adaptive filter strategy approached to the classical EKF aims to provide the ability to approximate the noise statistic under time It can be described as follows A Suboptimal MAP of Adaptive EKF Initially, a classical EKF was estimated by using MAP creation, as mentioned on .
, 22, 23, 24, .
Assuming that, the unknown parameters are the process and measurement statistics with their covariance Moreover, since considered be a positive definite symmetric matrix, then the estimated value of , , , and can be obtained by calculating the maximum value of the following objective function described below ] and Referring to Bayes rule, .
can be reformulated as follows .
where is Kronecker delta function.
Whereas, are mean and covariance term, .
illustrates that the mean values of the process and measurement noise are nonzero mean but instead and , respectively.
At this point, the classical of EKF can be presented as follows.
Since plays no role in optimization, we have .
Assuming that can be obtained from the prior information means it can be regarded as being a constant.
Then the posteriori distribution can be calculated by Suwoyo Et Al.
A MAPAEKF-SLAM Algorithm with Recursive Mean and Covariance of A SINERGI Vol.
No.
February 2020: 37-48 as derived below Note that and refer to and , respectively.
Whereas, refer to Since .
is the first-order Markov Process, can be reformulated as .
Next, by supposing that then by assuming that .
is normally distributed then it yields .
Then .
can be simplified as follows .
At this point, the estimated unknown parameter can be calculated by taking the logarithm of the objective function , calculating the first derivative of logarithm with respect to , , , and equating its derived to be zero.
These steps can be derived as follows.
Since the logarithm of is .
Similarly, for p[Z_.
X_k,q,r,Q,R] we have .
Then assuming that .
is Gaussian distribution , , ,and .
Then by multiplying .
, .
we have the following equation .
Suwoyo et al.
A MAPAEKF-SLAM Algorithm with Recursive Mean and Covariance of A p-ISSN: 1410-2331 e-ISSN: 2460-1217 The complicated multistep smoothing term in .
- .
might cause inefficiency of the MAP estimate.
Therefore, to find the conventional and efficient recursive form the simplification is needed.
Note that the recursive update process only utilizes the estimated value at time k-1 and k, hence the simplification can be conducted by replacing in .
and in .
- .
Therefore, the suboptimal of MAP noise estimator can be expressed as follows .
Now, the estimate value can be adopted from .
, respectively.
Time-Varying Noise Statistic of EKF Based on MAP As can be seen above that all the suboptimal values under the mathematical derivation of MAP creation are clear.
Now, both the mean of the process and measurement noise statistic and their corresponding covariance in .
, respectively, can be derived as follows.
First, by substituting .
, then .
As can be analyzed from the sequence equations above that the estimated value of is not provided obviously by classical EKF.
Therefore, modifying the original forms in .
is required aims to compute the noise statistics estimator .
At this point, it is obvious that the corresponding estimated covariance of is Modified and Improved EKF The process of modifying the EKF can be done by calculating the one-step smoothing of the EKF gain and its corresponding estimate value using the fixed point smoothing algorithm .
, .
, .
This process can be summarized as .
considering that the prior state in .
replaces the term of in the normal EKF, the rest part of modified EKF are chained as .
Now, by first substituting .
, it yields .
then by deriving and recalling the definition of .
, it is obtained Suwoyo Et Al.
A MAPAEKF-SLAM Algorithm with Recursive Mean and Covariance of A SINERGI Vol.
No.
February 2020: 37-48 Similarly, for the recursive covariance of the measurement noise statistic can be obtained as follows .
ee appendix B) .
Obviously, covariance of measurement noise is then .
Note that the existence of on the original function of can be ignored at this estimation process because it is known.
Besides that, it does not play any role in the optimization as well.
Finally, the recursive noise statistic and their corresponding covariance can be presented as .
A MAP Based AEKF-SLAM Algorithm The proposed method is applied for solving the SLAM problem of wheeled mobile robots.
Henceforth it is termed as AEKF-SLAM Algorithm Based on MAP-WE.
Basically, it can be solved by locating the current robot position and gathering the information of features in a certain Therefore, the movement and observation method is required.
Both are discussed in the following subsection.
Motion Model By supposing that, the robot is simulated and initially located in a certain planar environment.
Then the kinematic configuration of the robot movement can be shown in Figure 1.
However, to obtain the corresponding recursive covariance of .
, some derivation is This process can be presented as Since the update covariance .
is originally adopted from the following Joseph form then the recursive of the form .
can be alternatively reformatted as follows .
ee appendix Figure 1.
Kinematic Configuration .
Now, considering and are the spatial position and is the orientation or the robot heading, then the state vector of the robot position can be expressed by The robot moves based on the odometry and differential steering system, then since the control vector contains Suwoyo et al.
A MAPAEKF-SLAM Algorithm with Recursive Mean and Covariance of A p-ISSN: 1410-2331 e-ISSN: 2460-1217 two-element vectors represented by for right wheel and left wheel velocity , then the motion model is expressed as follows .
where is discrete time index, the radius caused by the motion, is the width of the robot, and are linear and nonlinear motion model respect to the measured The motion is unpredictable since the small noise follows the motion control.
For this reason, the different types of motion models are introduced.
Thus, the measured rightwheel and left-wheel velocity can be regarded as follows with the existence of the small perturbation .
Figure 2 illustrates when the robotAos laser scanner detects the -th landmark.
Since refer to the position of the laser scanner, the direct point-based observation model can be calculated as follows .
is the moving factor and .
is the turning Direct Point-Based Observation Considering that, the state vectors are composed of the robot and landmark state Figure 2.
New Detected Landmark Therefore, a full state vector is represent the -th landmark position for is the displacement of laser scanner and are the distance and bearing sensed by laser scanner.
Next, by considering that the measurement is followed by small perturbation then we have .
Now it can be noted that since .
- .
are gained, both state transition in .
and measurement function in .
are Inverse Point-Based Observation A new observed landmark should be initialized and added to the state vector .
It can be conducted by using the inverse observation It initiates the mapping process by utilizing the information of the current robot and landmark position.
It can be written as follows .
Suwoyo Et Al.
A MAPAEKF-SLAM Algorithm with Recursive Mean and Covariance of A SINERGI Vol.
No.
February 2020: 37-48 At this point, the completeness of MAPAEKF and SLAM are fully derived.
Moreover, the flowchart of the whole process above can be graphically concluded as shown in Figure 3.
Then the result of the EKF-SLAM and MAPAEKFSLAM algorithm can be compared based on Figure 4.
Figure 4.
SLAM-Algorithm Performance .
st Tes.
Figure 4 illustrates the performance of EKF and MAPAEKF.
They are applied to an autonomous wheeled mobile robot for solving the SLAM problem.
It depicts that the MAPAEKFSLAM gives a better solution proven by the successful in following the reference path.
For more detail, it can be analyzed by the following Figure 3.
MAPAEKF-SLAM Algorithm
RESULTS AND DISCUSSION
In order to verify the effectiveness and accuracy, the proposed method was realistically It was compared with the classical algorithm.
EKF-SLAM algorithm, in terms of RMSE of EPC and EMC.
Initially, some parameters related to the robot were defined as Note that these parameters are adopted from the real robot platform that is Turtlebot2.
Furthermore, the initial state and its error covariance were also defined as follows According to the initial noise statistic, different SLAM-based algorithm was performed and They can be presented as follows Figure 5.
RMSE of Estimated Path .
st Tes.
Figure 5 shows the RMSE of different performed algorithm in estimating the path.
Comparing to the conventional approach.
MAPAEKF-SLAM algorithm has a better accuracy pointed by the smaller RMSE in almost all Furthermore, an effort to provide more comparative result, the different RMSE of estimated map is also presented as follows.
1st Test The initial process and measurement noise statistic are considered as follows Suwoyo et al.
A MAPAEKF-SLAM Algorithm with Recursive Mean and Covariance of A p-ISSN: 1410-2331 e-ISSN: 2460-1217 Figure 7.
SLAM-Algorithm Performance .
nd Tes.
Figure 6.
RMSE of Estimated MAP .
st Tes.
Figure 6 shows the different quality of EKF and MAPAEKF-SLAM algorithm in estimating the location of the landmark.
According to this figure, the proposed method shows better performance in the estimated map for x-coordinate and ycoordinate.
To confirm this statement.
Table 1 is Figure 7 depicts that the increment of the initial noise statistic does not affect the stability of MAPAEKF-SLAM.
Therefore, it can be noted that the proposed method provides a more stable filter compared with the EKF-SLAM algorithm.
For the 2nd Test, the EKF and MAPAEKF-SLAM are also performed and compared in terms of RMSE.
It is depicted in Figure 8.
Table 1.
Different SLAM Algorithm in Term of Root Mean Square Error of Estimated Path and Map
SLAM
Alg.
EKF
AEKF
RMSE of Estimated Path Coordinate RMSE of Estimated Map Coordinate 2nd Test Relatively, the small noise statistic both the process and measurement are unknown.
For this reason, their initial predetermined value might be either small or large.
Thus, the second case with the increment on those values was considered as It aims to validate the suitability of the proposed method for the dynamic system with the large uncertainty and unavoidable noise of the sensor and actuator.
The initial process and measurement noise are considered as follows Figure 8.
RMSE of Estimated Path .
nd Tes.
Figure 9.
RMSE of Estimated MAP .
nd Tes.
Like the previous experiment, the general performance of the EKF and MAPAEKF-SLAM algorithm is evaluated from the following graphical According to Figure 8, the MAPAEKFSLAM algorithm shows its effectiveness in locating the current robot position.
It is proven by the smaller RMSE for almost all the benchmark.
Then, it can be noted that the accuracy of the MAPAEKF-SLAM algorithm is guaranteed even though there exists a noise statistic increment.
Suwoyo Et Al.
A MAPAEKF-SLAM Algorithm with Recursive Mean and Covariance of A SINERGI Vol.
No.
February 2020: 37-48 Additionally, the result of the estimated map is also presented as shown in Figure 9.
Similarly.
Figure 8 shows that the MAPAEKF-SLAM has a better solution of estimating the map for x-coordinate and ycoordinate significantly.
Therefore, it can be noted that the MAPAEKF-SLAM algorithm performed well in estimating the landmark.
It is confirmed by Table 2.
Appendix A (A.
Alternatively, it can be rewritten as followed Table 2.
Different SLAM Algorithm in Term of Root Mean Square Error of Estimated Path and Map .
nd Tes.
SLAM Alg.
EKF
AEKF
RMSE of Estimated Path Coordinate RMSE of Estimated Map Coordinate Table.
2 shows clearly that the MAPAEKF-SLAM algorithm provides a stable filter.
Additionally, according to the result of the 1st and 2nd Test presented above, it can be noted that the MAPAEKF-SLAM has better quality in providing solutions under noise statistics increment.
CONCLUSION
This paper presents a proposed method termed as the MAPAEKF-SLAM algorithm.
The contributions can be summarized as follows:
estimating the noise statistic.
improving the normal EKF based on one-step smoothing deriving the suboptimal estimate values under the creation of Maximum a Posteriori (MAP), the proposed method has been regarded to be able to solve SLAM problem even when the unknown noise statistic is large.
Based on the compared result and discussion presented in the previous section, its robustness and effectiveness have been validated.
ACKNOWLEDGMENT