Nonlinear Observer for 3D Rigid Body Motion Estimation Using Doppler Measurements

This work presents a nonlinear observer that estimates the translational and rotational motion of a rigid body based on measurements of configuration (pose), angular velocity, and radial velocity as well as modeled forces and torques. The radial velocity measurements are provided by a single direction Doppler sensor. Using a conveniently defined Lyapunov function, a nonlinear observer on the Special Euclidean Group (SE(3)) is derived and almost globally stability is guaranteed. The resulting estimation error is almost globally exponentially convergent for sufficiently rich motion. Numerical simulation results are presented that illustrate the performance of the proposed solution.