Geometric Nonlinear Filtering with Almost Global Convergence for Attitude and Bias Estimation on the Special Orthogonal Group

摘要

This paper proposes a novel geometric nonlinear filter for attitude and bias estimation on the Special Orthogonal Group $SO(3)$ using matrix measurements. The structure of the proposed filter is similar to that of the continuous-time deterministic multiplicative extended Kalman filter (MEKF). The main difference with the MEKF is the inclusion of curvature correction terms in both the filter gain and gain update equations. These terms ensure that the proposed filter, named the Generalized $SO(3)$-MEKF, renders the desired equilibrium of the estimation error system to be almost globally uniformly asymptotically stable (AGUAS). More precisely, the attitude and bias estimation errors converge uniformly asymptotically to zero for almost all initial conditions except those where the initial angular estimation error equals $π$ radians. Moreover, in the case of small estimation errors, the proposed generalized $SO(3)$-MEKF simplifies to the standard $SO(3)$-MEKF with matrix measurements. Simulation results indicate that the proposed filter has similar performance compared to the latter. Thus, the main advantage of the proposed filter over the MEKF is the guarantee of (almost) global uniform asymptotic stability.