Smoother-based 3D Attitude Estimation for Mobile Robot Localization
Stergios I. Roumeliotis, Gaurav S. Sukhatme and George A. Bekey
The mobile robot localization problem is decomposed and treated as a two -stage iterative process. The attitude is estimated first and is then used for position estimation. The innovation of our method presented in the sequel is the incorporation of a smoother, in the attitude estimation loop that outperforms i n estimate accuracy any other Kalman fiter based technique. The smoother is ideally suited for this application because it can exploit the special nature of the data fused. These data are either provided by inertial sensors and, in general, appear with high frequency, or result from absolute orientation measuring devic es and arrive at lower rates. 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). Then, the second filter propagates the recently renewed estimate backwards this time. Combining the outcome of the two filters, the smoother optimally exploits the limited (only at certain time instances) observability of the system. The ba ckward filter propagation is particularly beneficial for those parts of the traj ectory that exhibit lower levels of accuracy as these were determined during the forward filter propagation. This way the estimates across each time interval, a re smoothed and thus a more uniform and better quality orientation estimate is a chieved. Another prominent feature of this new approach is that the frequency of the absolute orientation sensors can be used as a design parameter. The desired level of accuracy for the attitude estimation and subsequently for the three-di mensional position estimation can be preset. At the system model level, gyro mod eling is used which relies on the integration of the kinematic equations to prop agate the attitude estimates. Thus, the Indirect (error state) form of the Kalman filter is developed for both the forward and the backward parts of the smoother. The main benefit of this ch oice is that the tedious and highly sensitive dynamic modeling of the mobile rob ot and its environment is avoided. The proposed implementation is independent of the structure of the vehicle or the morphology of the ground. It can easily be transfered to another mobile platform provided that it carries an equivalent set of sensors. Finally, instead of Euler angles mostly found in the relevant robotics literature, quaternions are selected for the three-dimensional attitude representation. T he quaternion parameterization was chosen mainly for three practical reasons. Fi rst, the prediction equations are treated linearly, then the representation is f ree of singularities and finally the attitude matrix is algebraic in the quatern ion components, thus eliminating the need for transcendental functions. The prop osed innovative algorithm is tested in simulation and the overall improvement in position estimation is demonstrated.