Nonlinear observer for 3D rigid body motion

Observer design for rigid body translational and rotational motion has important applications to unmanned or manned vehicles operating in air, underwater, or in space. An observer design for pose and velocity estimation for three-dimensional rigid body motion, in the framework of geometric mechanics, is presented here. Resorting to convenient defined Lyapunov function, a nonlinear observer on the Special Euclidean Group (SE(3)) is derived. This observer is based on the exponential coordinates, which are used to represent the group of rigid body motions. Exponential convergence of the estimation errors is shown and boundedness of the estimation error under bounded unmodeled torques and forces is established. Since exponential coordinates can describe uniquely almost the entire group of rigid body motions, the resulting observer design is almost globally exponentially convergent. The observer is then applied to the free dynamics of a rigid vehicle. Numerical simulation results are presented to show the performance of this observer, both in the absence and with unmodeled forces and torques.