Landmark based nonlinear observer for rigid body attitude and position estimation

This work proposes a nonlinear observer for position and attitude estimation on SE(3). Using a Lyapunov function based on the landmark measurement error, almost global exponential stability (GES) of the desired attitude and position equilibrium points is obtained. It is shown that the derived feedback law is an explicit function of the landmark measurements and velocity readings, and that the landmark geometry characterizes the asymptotic convergence of the closed loop system solution. Almost global exponential stabilization in the presence of biased velocity readings is also achieved. Simulation results for trajectories described by time-varying linear and angular velocities and for distinct initial conditions on SE(3) are presented to illustrate the stability and convergence properties of the observer.