On Fast Attitude Filtering Using Matrix Fisher Distributions with Stability Guarantee

摘要

This paper addresses two interrelated problems of the nonlinear filtering mechanism and fast attitude filtering with the matrix Fisher distribution (MFD) on the special orthogonal group. By analyzing the distribution evolution along Bayes' rule, we reveal two essential properties that enhance the performance of Bayesian attitude filters with MFDs, particularly in challenging conditions. Benefiting from the new understanding of the filtering mechanism associated with MFDs, two closed-form filters with MFDs are then proposed. These filters avoid the burdensome computations in previous MFD-based filters by introducing linearized error systems with right-invariant errors but retaining the two advantageous properties. The proposed filter with right-invariant error is proven to be almost globally asymptotically stable for any trajectory on $SO(3)$ leveraging its closed-form iteration and global uncertainty representation with MFDs. Moreover, we further prove the local exponential stability of the filter for single-axis rotations to reveal the effect of the two properties on the convergence rate. These stability results support the performance of the proposed filter with large initial error from a theoretical viewpoint, which to our knowledge, is not achieved by existing directional statistics-based filters. Numerical simulations demonstrate that proposed filters are as accurate as recent MFD-based Bayesian filters in challenging circumstances but consume far less computation time (about 1/5 to 1/100 of previous MFD-based attitude filters).