A Landmark Based Nonlinear Observer for Attitude and Position Estimation with Bias Compensation

This paper addresses the problem of estimating position and attitude of a rigid body based on landmark coordinate readings and biased velocity measurements. Using a Lyapunov function conveniently defined by the landmark measurement error, a nonlinear observer on SE(3) is derived. The resulting position, attitude, and biases estimation errors are shown to converge exponentially fast to the desired equilibrium points. The observer terms are explicit functions of the landmark measurements and velocity readings, exploiting the sensor information directly. Simulation results for trajectories described by time-varying linear and angular velocities are presented to illustrate the stability and convergence properties of the observer, supporting the application of the algorithm to autonomous air vehicles and other robotic platforms.