Smoother based 3D attitude estimation for mobile robot localization

The mobile robot localization problem is decomposed into two stages; attitude estimation followed by position estimation. The innovation of our method is the use of a smoother, in the attitude estimation loop that outperforms other Kalman filter based techniques in estimate accuracy. The smoother exploits the special nature of the data fused; high frequency inertial sensor (gyroscope) data and low frequency absolute orientation data (from a compass or sun sensor). Two Kalman filters form the smoother. During each time interval one of them propagates the attitude estimate forward in time until it is updated by an absolute orientation sensor. At this time, the second filter propagates the recently renewed estimate back in time. The smoother optimally exploits the limited observability of the system by combining the outcome of the two filters. The system model uses gyro modeling which relies on integrating the kinematic equations to propagate the attitude estimates and obviates the need for complex dynamic modeling. The indirect (error state) form of the Kalman filter is developed for both parts of the smoother. The proposed approach is independent of the robot structure and the morphology of the ground. It can easily be transferred to another robot which has an equivalent set of sensors. Quaternions are used for the 3D attitude representation mainly for practical reasons discussed in the paper. Proposed innovative algorithm is tested in simulation and the overall improvement in position estimation is demonstrated.