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.