A nonlinear observer for 6 DOF pose estimation from inertial and bearing measurements

This paper considers the problem of estimating pose from inertial and bearing-only vision measurements. We present a non-linear observer that evolves directly on the Special Euclidean group SE(3) from inertial measurements and bearing measurements, such as provided by a visual system tracking known landmarks. Local asymptotic convergence of the observer is proved. The observer is computationally simple and its gains are easy to tune. Simulation results demonstrate robustness to measurement noise and initial conditions