Landmark Based Nonlinear Observer for Rigid Body Attitude and Position Estimation
J.F. Vasconcelos, R. Cunha, C. Silvestre, and P. Oliveira 
Institute for Systems and Robotics (ISR) - Instituto Superior Tecnico (IST)
CDC 2007

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.