The Difference between the Left and Right Invariant Extended Kalman Filter

TL;DR

The paper investigates whether left- and right-invariant IEKFs are fundamentally different when a reset step is included. By formulating state uncertainty with concentrated Gaussian distributions on a Lie group $\mathbf{G}$ and deriving the left- and right-IEKF with reset, it proves that the two filters are equivalent: the log-likelihoods satisfy $\mathcal{L}_{\mathrm{L}}=\mathcal{L}_{\mathrm{R}}$ and the corresponding parameters relate via $\mu_{\mathrm{R}}=\mathrm{Ad}_{\hat{X}_{\mathrm{L}}}\mu_{\mathrm{L}}$, $\hat{X}_{\mathrm{R}}=\hat{X}_{\mathrm{L}}$, $\Sigma_{\mathrm{R}}=\mathrm{Ad}_{\hat{X}_{\mathrm{L}}}^{\vee} \Sigma_{\mathrm{L}} (\mathrm{Ad}_{\hat{X}_{\mathrm{L}}}^{\vee})^{\top}$. The authors extend the analysis to discrete-time systems and show that discretisation can introduce differences unless propagation is handled consistently; the reset step is shown to improve asymptotic performance, with transient behavior depending on handedness. GNSS-aided INS experiments corroborate the theory and provide practical guidance: implement the IEKF with reset, rendering the choice of handedness effectively inconsequential for performance. Overall, the work clarifies the role of reset in invariant filtering and offers a robust blueprint for applying IEKFs to robotics problems on Lie groups.

Abstract

The extended Kalman filter (EKF) has been the industry standard for state estimation problems over the past sixty years. The Invariant Extended Kalman Filter (IEKF) is a recent development of the EKF for the class of group-affine systems on Lie groups that has shown superior performance for inertial navigation problems. The IEKF comes in two versions, left- and right- handed respectively, and there is a perception in the robotics community that these filters are different and one should choose the handedness of the IEKF to match handedness of the measurement model for a given filtering problem. In this paper, we revisit these algorithms and demonstrate that the left- and right- IEKF algorithms (with reset step) are identical, that is, the choice of the handedness does not affect the IEKF's performance when the reset step is properly implemented. The reset step was not originally proposed as part of the IEKF, however, we provide simulations to show that the reset step improves asymptotic performance of all versions of the the filter, and should be included in all high performance algorithms. The GNSS-aided inertial navigation system (INS) is used as a motivating example to demonstrate the equivalence of the two filters.

Figures (4)

• Figure 1: The RMSE of each state variable and ANEES of the L-IEKF ($$) and R-IEKF ($\,$) with reset. Note that the two filters are indistinguishable in terms of performance at the current scale. This figure is used to demonstrate that the filters converge to the correct solution over time.
• Figure 2: The difference in the estimates of each state variable and the covariance matrices between the L-IEKF and R-IEKF. The shaded area represents the standard deviation of the data across all trials.
• Figure 3: The difference between L-IEKF and R-IEKF with different $N$-substep Euler counts in the prediction step. Shaded areas represent the standard deviation of the data across all trials. Log scale is used for better visibility of the difference.
• Figure 4: The RMSE and ANEES of the L-IEKF with reset ($$) and without reset ($$), R-IEKF with reset ($\,$) and without reset ($\,$).

Theorems & Definitions (35)

• Remark 3.1
• Remark 4.1
• Definition 4.2: L-IEKF prediction dynamics
• Definition 4.3: R-IEKF prediction dynamics
• Definition 4.4: L-IEKF measurement update
• Definition 4.5: R-IEKF measurement update
• Remark 4.6
• Definition 4.7: L-IEKF reset
• Definition 4.8: R-IEKF reset
• Remark 4.9