Skip to main content
SpringerLink
Log in

An AEKF-SLAM Algorithm with Recursive Noise Statistic Based on MLE and EM

  • Published:
Journal of Intelligent & Robotic Systems Aims and scope Submit manuscript

Abstract

Extended Kalman Filter (EKF) has been popularly utilized for solving Simultaneous Localization and Mapping (SLAM) problem. Essentially, it requires the accurate system model and known noise statistic. Nevertheless, this condition can be satisfied in simulation case. Hence, EKF has to be enhanced when it is applied in the real-application. Mainly, this improvement is known as adaptive-based approach. In many different cases, it is indicated by some manners of estimating for either part or full noise statistic. This paper present a proposed method based on the adaptive-based solution used for improving classical EKF namely An Adaptive Extended Kalman Filter. Initially, the classical EKF was improved based on Maximum Likelihood Estimation (MLE) and Expectation-Maximization (EM) Creation. It aims to equips the conventional EKF with ability of approximating noise statistic and its covariance matrices recursively. Moreover, EKF was modified and improved to tune the estimated values given by MLE and EM creation. Besides that, the recursive noise statistic estimators were also estimated based on the unbiased estimation. Although it results high quality solution but it is followed with some risks of non-positive definite matrices of the process and measurement noise statistic covariances. Thus, an addition of Innovation Covariance Estimation (ICE) was also utilized to depress this possibilities. The proposed method is applied for solving SLAM problem of autonomous wheeled mobile robot. Henceforth, it is termed as AEKF-SLAM Algorithm. In order to validate the effectiveness of proposed method, some different SLAM-Based algorithm were compared and analyzed. The different simulation has been showing that the proposed method has better stability and accuracy compared to the conventional filter in term of Root Mean Square Error (RMSE) of Estimated Map Coordinate (EMC) and Estimated Path Coordinate (EPC).

This is a preview of subscription content, log in via an institution to check access.

Access this article

Log in via an institution

Price excludes VAT (USA)
Tax calculation will be finalised during checkout.

Instant access to the full article PDF.

Institutional subscriptions

Similar content being viewed by others

References

  1. Ahmad, H., Namerikawa, T.: Extended Kalman filter-based mobile robot localization with intermittent measurements. Syst. Sci. Control Eng.: An Open Access Journal 1(1), 113–126 (2013)

    Article  Google Scholar 

  2. Bailey, T., Durrant-Whyte, H.: Simultaneous localization and mapping (slam): Part ii. IEEE Robot. Autom. Mag. 13(3), 108–117 (2006)

    Article  Google Scholar 

  3. Caballero, R., Hermoso, A., Jiménez, J., Linares, J.: Filtering and fixed-point smoothing from an innovation approach in systems with uncertainty. Extracta Math. 18(1), 119–128 (2003)

    MathSciNet  MATH  Google Scholar 

  4. Dissanayake, M.G., Newman, P., Clark, S., Durrant-Whyte, H.F., Csorba, M.: A solution to the simultaneous localization and map building (slam) problem. IEEE Trans. Robot. Autom. 17(3), 229–241 (2001)

    Article  Google Scholar 

  5. Durrant-Whyte, H., Bailey, T.: Simultaneous localization and mapping: part i. IEEE Robot. Autom. Mag. 13(2), 99–110 (2006)

    Article  Google Scholar 

  6. Gao, W., Li, J., Zhou, G., Li, Q.: Adaptive Kalman filtering with recursive noise estimator for integrated sins/dvl systems. J. Navig. 68(1), 142–161 (2015)

    Article  Google Scholar 

  7. Gao, B., Gao, S., Hu, G., Zhong, Y., Gu, C.: Maximum likelihood principle and moving horizon estimation based adaptive unscented kalman filter. Aerosp. Sci. Technol. 73, 184–196 (2018)

    Article  Google Scholar 

  8. Gustafsson, F.: Adaptive filtering and change detection, vol. 1. Citeseer (2000)

  9. He, J., Zhang, Q., Hu, Q., Sun, G.: A hybrid adaptive unscented kalman filter algorithm. Preprints 2017, 2017030127. https://doi.org/10.20944/preprints201703.0127.v1 (2017)

  10. Huang, G.P., Mourikis, A.I., Roumeliotis, S.I.: Analysis and improvement of the consistency of extended kalman filter based slam. In: 2008 IEEE International Conference on Robotics and Automation, pp 473–479. IEEE (2008)

  11. Huang, Y., Zhang, Y., Xu, B., Wu, Z., Chambers, J.A.: A new adaptive extended kalman filter for cooperative localization. IEEE Trans. Aerosp. Electron. Syst. 54(1), 353–368 (2018)

    Article  Google Scholar 

  12. Kim, K.H., Lee, J.G., Park, C.G.: Adaptive two-stage extended kalman filter for a fault-tolerant ins-gps loosely coupled system. IEEE Trans. Aerosp. Electron. Syst. 45(1), 125–137 (2009)

    Article  Google Scholar 

  13. Kurt-Yavuz, Z., Yavuz, S.: A comparison of ekf, ukf, fastslam2. 0, and ukf-based fastslam algorithms. In: 2012 IEEE 16th International Conference on Intelligent Engineering Systems (INES), pp 37–43. IEEE (2012)

  14. Logothetis, A., Krishnamurthy, V.: Expectation maximization algorithms for map estimation of jump Markov linear systems. IEEE Trans. Signal Process. 47(8), 2139–2156 (1999)

    Article  Google Scholar 

  15. Mahmoudi, Z., Poulsen, N.K., Madsen, H.: Jørgensen, J.B.: Adaptive unscented Kalman filter using maximum likelihood estimation. IFAC-PapersOnLine 50(1), 3859–3864 (2017)

    Article  Google Scholar 

  16. Parra-Tsunekawa, I., Ruiz-del Solar, J., Vallejos, P.: A Kalman-filtering-based approach for improving terrain mapping in off-road autonomous vehicles. J. Intell. Robot. Syst. 78(3–4), 577–591 (2015)

    Article  Google Scholar 

  17. Rullán-Lara, J.L., Salazar, S., Lozano, R.: Real-time localization of an UAV using Kalman filter and a wireless sensor network. J. Intell. Robot. Syst. 65(1-4), 283–293 (2012)

    Article  Google Scholar 

  18. Shao, X., He, B., Guo, J., Yan, T.: The application of auv navigation based on adaptive extended Kalman filter. In: Oceans 2016-Shanghai, pp 1–4. IEEE (2016)

  19. Thrun, S., Burgard, W., Fox, D.: Probabilistic robotics. MIT Press, Cambridge (2005)

    MATH  Google Scholar 

  20. Wang, H., Fu, G., Li, J., Yan, Z., Bian, X.: An adaptive ukf based slam method for unmanned underwater vehicle. In: Mathematical Problems in Engineering 2013 (2013)

  21. Yuzhen, P., Quande, Y., Benfa, Z.: The application of adaptive extended kalman filter in mobile robot localization. In: Control and Decision Conference (CCDC), 2016 Chinese, pp 5337–5342. IEEE (2016)

Download references

Acknowledgements

Research was supported by Special Plan of Major Scientific Instruments and Equipment of the State (Grant No.2018YFF01013101), National Natural Science Foundation of China (51775322, 91748122, 61603237), the IIOT Innovation and Development Special Foundation of Shanghai (2017-GYHLW01037) and Project named ”Key Technology Research and Demonstration Line Construction of Advanced Laser Intelligent Manufacturing Equipment” from Shanghai Lingang Area Development Administration

Author information

Authors and Affiliations

  1. School of Mechatronic Engineering and Automation, Shanghai University, Shanghai, 200072, China

    Yingzhong Tian, Heru Suwoyo, Dziki Mbemba & Long Li

  2. Shanghai Key Laboratory of Intelligent Manufacturing and Robotics, Shanghai, 200072, China

    Yingzhong Tian & Long Li

  3. Department of Electrical Engineering, Universitas Mercu Buana, Jakarta, 11650, Indonesia

    Heru Suwoyo

  4. Mechanical and Electrical Engineering School, Shenzhen Polytechnic, Guangdong, 518055, China

    Wenbin Wang

Authors
  1. Yingzhong Tian
    View author publications

    You can also search for this author in PubMed Google Scholar

  2. Heru Suwoyo
    View author publications

    You can also search for this author in PubMed Google Scholar

  3. Wenbin Wang
    View author publications

    You can also search for this author in PubMed Google Scholar

  4. Dziki Mbemba
    View author publications

    You can also search for this author in PubMed Google Scholar

  5. Long Li
    View author publications

    You can also search for this author in PubMed Google Scholar

Corresponding author

Correspondence to Long Li.

Additional information

Publisher’s Note

Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.

Appendix: Derivation of Covariance Matrix of Measurement Noise Statistic

Appendix: Derivation of Covariance Matrix of Measurement Noise Statistic

Assuming Eq. 87 is satisfied

$$ e_{z,i}e_{z,i}^{T} = S_{i} $$
(1)

Then Eq. 43 can be derived as follows

$$ \begin{array}{@{}rcl@{}} K_{i} = P_{i|i-1}H^{T}(S_{i})^{-1}\\ K_{i} = P_{i|i-1}H^{T}(e_{z,i}e_{z,i}^{T})^{-1}\\ K_{i}e_{z,i}e_{z,i}^{T} = P_{i|i-1}H^{T} \end{array} $$
(2)
$$ e_{z,i}e_{z,i}^{T}{K_{i}^{T}} = (K_{i}e_{z,i}e_{z,i}^{T})^{T} = HP_{i|i-1}^{T} = HP_{i|i-1} $$
(3)
$$ K_{i}e_{z,i}e_{z,i}^{T}{K_{i}^{T}} = K_{i}HP_{i|i-1}^{T} = K_{i}HP_{i|i-1} $$
(4)

Equation 45 is derived as follows

$$ \begin{array}{@{}rcl@{}} P_{i|i} = (I-K_{i}H)P_{i|i-1}\\ P_{i|i} = P_{i|i-1} - K_{i}HP_{i|i-1}\\ K_{i}HP_{i|i-1}=P_{i|i-1} - P_{i|i} \end{array} $$
(5)

Substituting Eq. 33 into Eq. 91 then Eq. 90 becomes

$$ \begin{array}{@{}rcl@{}} K_{i}e_{z,i}e_{z,i}^{T}{K_{i}^{T}} & = & P_{i|i-1}-P_{i|i} = F_{i-1}P_{i-1|i-1}F_{i-1|i-1}^{T}\\ &&+Q_{i-1} - P_{i|i} \end{array} $$
(6)

Then the formulation of covariance matrix of measurement noise statistic R can be derived as follows

$$ \hat{R}_{k} = \frac{1}{k}\sum\limits_{i=1}^{k}(I-HK_{i})e_{z,i}e_{z,i}^{T}(I-HK_{i})^{T} $$
(7)

where

$$ \begin{array}{@{}rcl@{}} &&(I-HK_{i})e_{z,i}e_{z,i}^{T}(I-HK_{i})^{T}\\ &=&e_{z,i}e_{z,i}^{T} - HKe_{z,i}e_{z,i}^{T} - e_{z,i}e_{z,i}^{T}K^{T}H^{T} + HKe_{z,i}e_{z,i}^{T}K^{T}H^{T} \end{array} $$
(8)

then by substituting Eqs. 87–93 into Eq. 94

$$ \begin{array}{@{}rcl@{}} &&(I-HK_{i})e_{z,i}e_{z,i}^{T}(I-HK_{i})^{T}\\ &=&(HP_{i|i-1}H^{T} + R_{i}) - 2HP_{i|i-1}H^{T} + HK_{i}HP_{i|i-1}H^{T}\\ &=&R_{i} - HP_{i|i-1}H^{T} + HK_{i}HP_{i|i-1}H^{T}\\ &=&R_{i} - (I-HK_{i})HP_{i|i-1}H^{T} \end{array} $$
(9)
$$ \begin{array}{@{}rcl@{}} E{[\hat{R}_{k}]} &=& E\left\{\frac{1}{k}\sum\limits_{i=1}^{k}(I-HK_{i})e_{z,i}e_{z,i}^{T}(I-HK_{i})^{T}\right\}\\ &=& E\left\{\frac{1}{k}\sum\limits_{i=1}^{k}R_{i}-(I-HK_{i})HP_{i|i-1}H^{T}\right\}\\ &=&R_{k} + E\left\{\frac{1}{k}\sum\limits_{i=1}^{k}-(HP_{i|i-1}H^{T} - HK_{i}HP_{i|i-1}H^{T})\right\}\\ &=&R_{k} + E\left\{\frac{1}{k}\sum\limits_{i=1}^{k}HK_{i}HP_{i|i-1}H^{T} - HP_{i|i-1}H^{T}\right\} \end{array} $$
(10)

where Rk refers to Eq. 51 then Eq. 96 can be derived as follows

$$ \begin{array}{@{}rcl@{}} \hat{R}_{k} &=& \frac{1}{k}\sum\limits_{i=1}^{k}(I-HK_{i})e_{z,i}e_{z,i}^{T}(I-HK_{i})^{T} \\ &&+HK_{i}HP_{i|i-1}H^{T} - HP_{i|i-1}H^{T} \end{array} $$
(11)

where

$$ \begin{array}{@{}rcl@{}} &&(I-HK_{i})e_{z,i}e_{z,i}^{T}(I-HK_{i})^{T} + HK_{i}HP_{i|i-1}H^{T} \\ &&- HP_{i|i-1}H^{T}\\ &=&e_{z,i}e_{z,i}^{T} - HK_{i}e_{z,i}e_{z,i}^{T} - e_{z,i}e_{z,i}^{T}{K_{i}^{T}}H^{T} \\ &&+ HK_{i}e_{z,i}e_{z,i}^{T}{K_{i}^{T}}H^{T} + HK_{i}HP_{i|i-1}H^{T} - HP_{i|i-1}H^{T}\\ &=&e_{z,i}e_{z,i}^{T} - HK_{i}e_{z,i}e_{z,i}^{T} - e_{z,i}e_{z,i}^{T}{K_{i}^{T}}H^{T} \\ &&+ HK_{i}e_{z,i}e_{z,i}^{T}{K_{i}^{T}}H^{T} + (I-HK_{i})(HP_{i|i-1}H^{T})\\ &=&(I-HK_{i})[e_{z,i}e_{z,i}^{T}(I-HK_{i})^{T} + HP_{i|i-1}H^{T}] \end{array} $$
(12)

Then the equivalent estimated value of R is obtained as follows

$$ \hat{R}_{k} = \frac{1}{k}\sum\limits_{i=1}^{k}(I-K_{i}H){[e_{z,i}e_{z,i}^{T}(I-K_{i}H)+HP_{i|i-1}H^{T}]} $$
(13)

Rights and permissions

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Tian, Y., Suwoyo, H., Wang, W. et al. An AEKF-SLAM Algorithm with Recursive Noise Statistic Based on MLE and EM. J Intell Robot Syst 97, 339–355 (2020). https://doi.org/10.1007/s10846-019-01044-8

Download citation

  • Received:

  • Accepted:

  • Published:

  • Issue Date:

  • DOI: https://doi.org/10.1007/s10846-019-01044-8

Keywords

Search

Navigation