[Paper Review] Notes on Kalman Filter (KF, EKF, ESKF, IEKF, IESKF)

Gyubeom Im|arXiv (Cornell University)|Jun 10, 2024

Inertial Sensor and Navigation4 citations

TL;DR

This paper provides a comprehensive, mathematically rigorous derivation and comparative analysis of the Kalman Filter (KF), Extended Kalman Filter (EKF), Error-State Kalman Filter (ESKF), Iterated Extended Kalman Filter (IEKF), and Iterated Error-State Kalman Filter (IESKF). It unifies these filters under a Bayesian optimization framework, deriving each using maximum a posteriori (MAP) estimation and Gauss-Newton optimization, with explicit derivations of prediction and correction steps, highlighting improvements in convergence and accuracy through iterative refinement.

ABSTRACT

The Kalman Filter (KF) is a powerful mathematical tool widely used for state estimation in various domains, including Simultaneous Localization and Mapping (SLAM). This paper presents an in-depth introduction to the Kalman Filter and explores its several extensions: the Extended Kalman Filter (EKF), the Error-State Kalman Filter (ESKF), the Iterated Extended Kalman Filter (IEKF), and the Iterated Error-State Kalman Filter (IESKF). Each variant is meticulously examined, with detailed derivations of their mathematical formulations and discussions on their respective advantages and limitations. By providing a comprehensive overview of these techniques, this paper aims to offer valuable insights into their applications in SLAM and enhance the understanding of state estimation methodologies in complex environments.

Motivation & Objective

• To provide a unified, mathematically rigorous derivation of KF, EKF, ESKF, IEKF, and IESKF from a Bayesian and optimization perspective.
• To clarify the relationships between MAP estimation, MLE, and Gauss-Newton optimization in the context of nonlinear filtering.
• To derive the update steps of IEKF and IESKF using iterative refinement, improving convergence and accuracy over standard EKF and ESKF.
• To present a systematic comparison of the five filters, highlighting their similarities, differences, and algorithmic structures.
• To serve as a foundational reference for researchers implementing or analyzing nonlinear state estimation in robotics, SLAM, and autonomous systems.

Proposed method

• Derives the Kalman filter prediction and update steps using Bayesian inference and minimum mean square error (MMSE) estimation.
• Applies the extended Kalman filter (EKF) by linearizing the nonlinear motion and observation models using first-order Taylor expansions.
• Introduces the error-state Kalman filter (ESKF) by propagating state errors relative to a nominal trajectory, improving numerical stability in nonlinear systems.
• Develops the iterated extended Kalman filter (IEKF) by iteratively re-linearizing the observation model around updated estimates to improve convergence.
• Derives the iterated error-state Kalman filter (IESKF) by combining ESKF structure with iterative refinement of the innovation term using Gauss-Newton optimization.
• Uses MAP-based derivation to unify EKF and IEKF, showing that iterative updates correspond to Gauss-Newton steps on the likelihood function.

Experimental results

Research questions

• RQ1 How do the KF, EKF, ESKF, IEKF, and IESKF differ in their mathematical formulation and algorithmic structure?
• RQ2 What is the relationship between the EKF and MAP estimation, and how does Gauss-Newton optimization improve its performance?
• RQ3 How does the iterative refinement in IEKF and IESKF enhance estimation accuracy compared to standard EKF and ESKF?
• RQ4 Why is the error-state formulation (ESKF/IESKF) more numerically stable than standard EKF in nonlinear systems?
• RQ5 What is the role of the Hessian and Fisher information in the derivation of the IESKF update step?

Key findings

• The IEKF and IESKF achieve improved convergence and accuracy by iteratively re-linearizing the observation model around updated state estimates, reducing linearization errors.
• The IESKF update step is derived as a Gauss-Newton step on the MAP objective, showing that iterative refinement corresponds to minimizing the negative log-likelihood via Newton-type optimization.
• The ESKF and IESKF frameworks are shown to be more numerically stable than standard EKF due to error propagation instead of absolute state propagation.
• The derivation reveals that the Kalman gain in KF is optimal under Gaussian assumptions, while in EKF and IEKF, it is derived from linearized models.
• The IESKF update equation is explicitly derived as δx̂_t|t,j = K_t,j(z_t - h( x̂_t|t,j,0,0) + H_t,j J_t,j^{-1}(x̂_t|t,j - x̂_t|t-1)) - J_t,j^{-1}(x̂_t|t,j - x̂_t|t-1), showing the correction term's dependence on the Jacobian and innovation.
• The paper establishes that the IESKF is equivalent to performing multiple Gauss-Newton iterations on the observation model, leading to faster convergence and better performance in nonlinear systems.