Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Invariant Kalman Filtering with Noise-Free Pseudo-Measurements

Sven Goffin, Silvère Bonnabel, Olivier Brüls, Pierre Sacré

TL;DR

This work develops an Invariant Extended Kalman Filter (IEKF) framework for estimating extended pose under deterministic, noise-free constraints treated as pseudo-measurements. It derives a Kalman gain limit for rank-deficient covariances via the Moore–Penrose pseudoinverse and embeds the problem in the SE2(3) Lie group, ensuring covariance updates align with the constrained subspace (Property 2) while acknowledging that the mean may not exactly satisfy the nonlinear constraint (Property 1). To address nonlinear residuals, an iterative update using the same noise-free measurement is proposed. A crane-hook estimation case demonstrates that the Noise-free IEKF outperforms both EKF and standard IEKF in convergence speed and robustness, highlighting practical benefits for lightweight IMU-based pose estimation with deterministic side information.

Abstract

In this paper, we focus on developing an Invariant Extended Kalman Filter (IEKF) for extended pose estimation for a noisy system with state equality constraints. We treat those constraints as noise-free pseudo-measurements. To this aim, we provide a formula for the Kalman gain in the limit of noise-free measurements and rank-deficient covariance matrix. We relate the constraints to group-theoretic properties and study the behavior of the IEKF in the presence of such noise-free measurements. We illustrate this perspective on the estimation of the motion of the load of an overhead crane, when a wireless inertial measurement unit is mounted on the hook.

Invariant Kalman Filtering with Noise-Free Pseudo-Measurements

TL;DR

This work develops an Invariant Extended Kalman Filter (IEKF) framework for estimating extended pose under deterministic, noise-free constraints treated as pseudo-measurements. It derives a Kalman gain limit for rank-deficient covariances via the Moore–Penrose pseudoinverse and embeds the problem in the SE2(3) Lie group, ensuring covariance updates align with the constrained subspace (Property 2) while acknowledging that the mean may not exactly satisfy the nonlinear constraint (Property 1). To address nonlinear residuals, an iterative update using the same noise-free measurement is proposed. A crane-hook estimation case demonstrates that the Noise-free IEKF outperforms both EKF and standard IEKF in convergence speed and robustness, highlighting practical benefits for lightweight IMU-based pose estimation with deterministic side information.

Abstract

In this paper, we focus on developing an Invariant Extended Kalman Filter (IEKF) for extended pose estimation for a noisy system with state equality constraints. We treat those constraints as noise-free pseudo-measurements. To this aim, we provide a formula for the Kalman gain in the limit of noise-free measurements and rank-deficient covariance matrix. We relate the constraints to group-theoretic properties and study the behavior of the IEKF in the presence of such noise-free measurements. We illustrate this perspective on the estimation of the motion of the load of an overhead crane, when a wireless inertial measurement unit is mounted on the hook.
Paper Structure (21 sections, 4 theorems, 27 equations, 3 figures, 1 algorithm)

This paper contains 21 sections, 4 theorems, 27 equations, 3 figures, 1 algorithm.

Table of Contents

  1. Introduction
  2. Motivating problem
  3. Mathematical formulation
  4. Application examples
  5. Crane state estimation
  6. Robotic arm state estimation
  7. Kalman filtering with noise-free pseudo-measurements
  8. The linear case
  9. The nonlinear case
  10. Handling rank deficiency in the linear case
  11. Solving for the Kalman gain in the noise-free limit
  12. Properties of the Riccati update (Property 2)
  13. Invariant Kalman filtering with noise-free pseudo-measurements
  14. The Invariant EKF for the considered problem
  15. What about desirable Property 2?
  16. ...and 6 more sections

Key Result

Theorem 1

Assume $P_{k \mid k-1}$ to be of rank $l\leq n$ and write it as $P_{k \mid k-1} = L_kL_k^T$ where $L_k \in \mathbb{R}^{n\times l}$ has linearly independent columns. The following Kalman gain is the limit of $P_{k \mid k-1}H_k^T(H_kP_{k \mid k-1}H_k^T+N_k)^{-1}$ as the measurement noise covariance matrix $N_k$ shrinks to $0_{m \times m}$, where $A^\dagger$ denotes the Moore-Penrose pseudo-inverse

Figures (3)

  • Figure 1: Crane with a wireless IMU mounted on its hook.
  • Figure 2: Evolution of the length of the crane cable $l_k$ as a function of the time index.
  • Figure 3: Mean norm of the error function $\xi_k$ as a function of the time index, computed over $30$ simulations. The standard deviation of the norm is displayed in light colors.

Theorems & Definitions (10)

  • Theorem 1
  • proof
  • Theorem 2
  • proof
  • Definition 1
  • Theorem 3
  • proof
  • Proposition 1
  • Remark 1
  • Remark 2