On the Fast Nonlinear Filtering with Matrix Fisher Distributions on SO(3)

TL;DR

The paper addresses fast nonlinear attitude filtering on $SO(3)$ using matrix Fisher distributions (MFDs). It reveals two intrinsic properties that explain why MFD-based Bayesian filters outperform CGD-based counterparts and proposes two fast filters (FNF-R and FNF-L) that preserve these properties while linearizing error dynamics for efficiency. The authors derive closed-form posterior updates under right- and left-invariant error models, analyze posterior concentration and mean-angle evolution, and demonstrate through extensive simulations that the proposed filters achieve near state-of-the-art accuracy with orders-of-magnitude lower computation time than previous MFD-based methods, and superior robustness to non-isotropic noise compared with IEKF. These results indicate that MFD-based filters can provide fast, reliable attitude estimation on $SO(3)$ suitable for resource-constrained platforms and challenging sensing conditions.

Abstract

This paper addresses two interrelated problems: 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 from a theoretical viewpoint. Benefiting from the new understanding of the filtering mechanism associated with MFDs, two closed-form filters with MFDs are then proposed. The filters avoids the burdensome computations in previous MFD-based filters by introducing linearized error systems with invariant errors but retaining the two advantageous properties. Numerical simulations demonstrate that the proposed filters are more accurate than the classic invariant Kalman filter. Besides, it is also as accurate as recent MFD-based Bayesian filters in challenging circumstances with large initial error and measurement uncertainty, but it consumes far less computation time (about 1/5 to 1/100 of previous MFD-based attitude filters).

Figures (7)

• Figure 1: Matrix Fisher distributions with different parameters. The marginal distribution for each column of $R$ is shown on the unit sphere as red, green and blue shades, as presented in leeBayesianAttitudeEstimation2018. The red, green and blue lines represent the first, second, and third columns of the mean attitude, respectively, and the black line represents the axis $w_0$. (a) $\mathcal{M}(F_1)$: $\kappa_1 = 100,\ \bar{\theta}_1=0$; (b) $\mathcal{M}(F_2)$: $\kappa_2 = 4,\ \bar{\theta}_2=0$; (c) $\mathcal{M}(F_3)$: $\kappa_3 = 100,\ \bar{\theta}_3=\pi/4$
• Figure 2: The trend of $\kappa^+$ as a function of $\Delta \bar{\theta}$. Blue: $\kappa^+$ given by MFD-based Bayesian filters. Red: $\kappa^+$ given by CGD-based Bayesian filters
• Figure 3: The trends of $\bar{\theta}^+$ on $\kappa^m/\kappa^-$ with different $\Delta{\bar{\theta}}$. Blue: $\bar{\theta}^+$ given by MFD-based Bayesian filters. Red: $\bar{\theta}^+$ given by CGD-based Bayesian filters. Green: the mean angle with higher confidence in $\bar{\theta}^-$ and $\bar{\theta}^m$
• Figure 4: The posterior distribution with several direct attitude measurements.
• Figure 5: Average error for the case of large initial errors with non-isotropic measurement errors. The shadow represents an envelope of 95% confidence. The attitude uncertainty is calculated as the square root of the first diagonal term of the attitude covariance matrix in the inertial frame as wang2020matrix.

Theorems & Definitions (38)

• Lemma II.1
• proof
• Proposition II.1
• proof
• Definition III.1
• Proposition III.1
• proof
• Theorem III.1
• proof
• Definition III.2