Improvement of the measurement update step of EKF-SLAM

In this study, the measurement update step of the Extended Kalman Filter (EKF)-based Simultaneous Localization and Mapping (SLAM) is improved. The computational complexity of the measurement uncertainty matrix inversion operation in the measurement update step is reduced via using Jacobi iteration method. It is observed that, the calculation of the measurement uncertainty matrix inverse by using Jacobi iteration method generates numerically more stable results than naive single and batch update operations. Moreover, it produces more accurate results than the results of Cholesky decomposition with less complexity.