Globally exponentially stable Kalman filtering for SLAM with AHRS

The Simultaneous Localization And Mapping (SLAM) estimation problem using range or bearing angle sensors is an inherently nonlinear problem due to the nonlinear relationship between the measurements and the positions of the landmarks and the pose of the vehicle. Assuming an Attitude and Heading Reference System (AHRS) and that velocity is measured, it is shown that the nonlinear kinematic models in the inertial coordinate frame and the nonlinear measurement models can be re-formulated globally as a linear time-varying system. This allows a globally exponentially stable Kalman filter to be used for real-time recursive estimation, under natural assumptions on the trajectory of the vehicle relative to the landmarks. This ensures theoretical guarantees of robustness of the estimate of the vehicle and landmark positions to inaccuracies and arbitrary estimator initialization. Simulations are used to illustrate the results.