Observability Analysis for Improving EKF-SLAM Algorithm by Using Simulation

As for state estimation inconsistency in standard EKF-SLAM (Extended Kalman Filter, Simultaneous Localization and Mapping), a new algorithm that increasing observability constraint condition is represented in order to improve the consistency of standard EKF-SLAM. The observability constraint condition is first added and the compensation matrix $\boldsymbol{U}$ is constructed for standard EKF-SLAM algorithm in order to keep the rank of local observability matrix of EKF-SLAM consistent with non-linear SLAM system. Then we use data fusion based state delay estimation to improve the accuracy of the initial landmark position. Next, a new linear point is calculated through solving the constrained optimization problem and computing Jacobi matrix of state model and observation model of the linear error status system for improving system observation matrix. Finally, the simulation results illustrate that the proposed algorithm is superior to the standard EKF-SLAM algorithm in terms of state estimation accuracy and consistency.