Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Invariant filtering for wheeled vehicle localization with unknown wheel radius and unknown GNSS lever arm

Paul Chauchat, Silvère Bonnabel, Axel Barrau

TL;DR

The paper addresses localization of a wheeled vehicle with unknown wheel-radius scale and unknown GNSS lever arm. It leverages the two-frame group framework to derive invariant filters, showing that Problems 1 and 2 fit naturally within this structure and delivering a novel change of variables to cast Problem 3 into the invariant-filtering framework, yielding an IEKF with autonomous error in transformed coordinates. The proposed approach uses a TFG on $SO(2)$ (and an extended scale group) and a variable change $X' = X/s$ to maintain output compatibility and autonomous error evolution, with an IEKF update based on the exponential map. Numerical experiments demonstrate that the resulting TFG-IEKF outperforms imperfect IEKF and standard EKF in convergence and accuracy, particularly for large initial attitude errors, underscoring the practical value of extending invariant filtering to joint scale and lever-arm estimation.

Abstract

We consider the problem of observer design for a nonholonomic car (more generally a wheeled robot) equipped with wheel speeds with unknown wheel radius, and whose position is measured via a GNSS antenna placed at an unknown position in the car. In a tutorial and unified exposition, we recall the recent theory of two-frame systems within the field of invariant Kalman filtering. We then show how to adapt it geometrically to address the considered problem, although it seems at first sight out of its scope. This yields an invariant extended Kalman filter having autonomous error equations, and state-independent Jacobians, which is shown to work remarkably well in simulations. The proposed novel construction thus extends the application scope of invariant filtering.

Invariant filtering for wheeled vehicle localization with unknown wheel radius and unknown GNSS lever arm

TL;DR

The paper addresses localization of a wheeled vehicle with unknown wheel-radius scale and unknown GNSS lever arm. It leverages the two-frame group framework to derive invariant filters, showing that Problems 1 and 2 fit naturally within this structure and delivering a novel change of variables to cast Problem 3 into the invariant-filtering framework, yielding an IEKF with autonomous error in transformed coordinates. The proposed approach uses a TFG on (and an extended scale group) and a variable change to maintain output compatibility and autonomous error evolution, with an IEKF update based on the exponential map. Numerical experiments demonstrate that the resulting TFG-IEKF outperforms imperfect IEKF and standard EKF in convergence and accuracy, particularly for large initial attitude errors, underscoring the practical value of extending invariant filtering to joint scale and lever-arm estimation.

Abstract

We consider the problem of observer design for a nonholonomic car (more generally a wheeled robot) equipped with wheel speeds with unknown wheel radius, and whose position is measured via a GNSS antenna placed at an unknown position in the car. In a tutorial and unified exposition, we recall the recent theory of two-frame systems within the field of invariant Kalman filtering. We then show how to adapt it geometrically to address the considered problem, although it seems at first sight out of its scope. This yields an invariant extended Kalman filter having autonomous error equations, and state-independent Jacobians, which is shown to work remarkably well in simulations. The proposed novel construction thus extends the application scope of invariant filtering.
Paper Structure (23 sections, 4 theorems, 40 equations, 1 figure, 1 table)

This paper contains 23 sections, 4 theorems, 40 equations, 1 figure, 1 table.

Table of Contents

  1. Introduction
  2. Three increasingly difficult problems
  3. First problem (basic problem)
  4. Second problem (adding one difficulty)
  5. Third problem (adding yet another difficulty)
  6. Casting the first problem into the framework of invariant filtering
  7. Group action and group law
  8. Error dynamics
  9. Invariant observers: compatible output maps
  10. Casting Problem 1 into the invariant filtering framework
  11. Casting the second problem into the framework of invariant filtering
  12. Definition of the TFG group structure and actions
  13. Results
  14. Casting the third problem into the framework of invariant filtering
  15. A preliminary subproblem as a first step
  16. ...and 8 more sections

Key Result

Proposition 1

The left-invariant error $E$ is autonomous for Problem 1. It evolves autonomously through eq::odo and its associated innovation $Z$ depends only upon itself.

Figures (1)

  • Figure 1: Results of the alignment experiments. Top: RMSE of the estimates for $\sigma_{att}^0 = 100^\circ$. Bottom: Yaw error for each MC run compared with the 3-$\sigma$ envelope for $\sigma_{att}^0 = 200^\circ$. Error curves are in blue if they stay below the envelope after 20s, and in red if they do not.

Theorems & Definitions (9)

  • Definition 1
  • Definition 2: Compatible output
  • Proposition 1
  • proof
  • Proposition 2
  • Proposition 3
  • proof
  • Proposition 4
  • proof