Dual Kalman filters for autonomous terrain aided navigation in unknown environments

In this paper, we address a method for terrain aided navigation of unmanned vehicles in unknown environments. The task is to simultaneously estimate the state of the vehicle (position and attitude) and a map of the surrounding environment given limited sensing capabilities. Possible available sensors include an on-board inertial measurement unit (IMU) and other simple "terrain sensors" such as altitude, temperature, or vision based features. Explicit positioning sensors such as GPS or a prior land map are not available. This problem is widely known as simultaneous localization and mapping (SLAM). A dual Kalman filter framework is proposed that works by alternating between using one Kalman filter to localize the vehicle given the current estimated map, and a second Kalman filter to update the estimate of the map given the position of the vehicle. Simulation results are generated for a two dimensional environment comparing the extended Kalman filter (EKF) and sigma point Kalman filter (SPKF) based implementations. It is shown that the SPKF based approach converges faster and also to a lower mean square error (MSE) than that of the EKF counterpart.