Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Supervisory Measurement-Guided Noise Covariance Estimation

Haoying Li, Yifan Peng, Xinghan Li, Junfeng Wu

TL;DR

This work tackles the problem of unknown sensor noise covariances in robotic state estimation by formulating covariance learning as a bilevel optimization that factorizes the joint likelihood into odometry and supervisory components. A chain-structured Bayesian model enables parallel execution of a State Filter (invariant EKF with state augmentation) and a Derivative Filter to produce analytical gradients for the upper-level optimization over covariance parameters. The methodology accommodates supervisory measurements like loop closures to enrich information without prohibitive complexity, yielding more accurate covariance estimates and improved long-horizon performance. Validation on synthetic and real-world datasets demonstrates both higher efficiency and better covariance tuning compared to baselines, highlighting practical impact for SLAM and robust navigation.

Abstract

Reliable state estimation hinges on accurate specification of sensor noise covariances, which weigh heterogeneous measurements. In practice, these covariances are difficult to identify due to environmental variability, front-end preprocessing, and other reasons. We address this by formulating noise covariance estimation as a bilevel optimization that, from a Bayesian perspective, factorizes the joint likelihood of so-called odometry and supervisory measurements, thereby balancing information utilization with computational efficiency. The factorization converts the nested Bayesian dependency into a chain structure, enabling efficient parallel computation: at the lower level, an invariant extended Kalman filter with state augmentation estimates trajectories, while a derivative filter computes analytical gradients in parallel for upper-level gradient updates. The upper level refines the covariance to guide the lower-level estimation. Experiments on synthetic and real-world datasets show that our method achieves higher efficiency over existing baselines.

Supervisory Measurement-Guided Noise Covariance Estimation

TL;DR

This work tackles the problem of unknown sensor noise covariances in robotic state estimation by formulating covariance learning as a bilevel optimization that factorizes the joint likelihood into odometry and supervisory components. A chain-structured Bayesian model enables parallel execution of a State Filter (invariant EKF with state augmentation) and a Derivative Filter to produce analytical gradients for the upper-level optimization over covariance parameters. The methodology accommodates supervisory measurements like loop closures to enrich information without prohibitive complexity, yielding more accurate covariance estimates and improved long-horizon performance. Validation on synthetic and real-world datasets demonstrates both higher efficiency and better covariance tuning compared to baselines, highlighting practical impact for SLAM and robust navigation.

Abstract

Reliable state estimation hinges on accurate specification of sensor noise covariances, which weigh heterogeneous measurements. In practice, these covariances are difficult to identify due to environmental variability, front-end preprocessing, and other reasons. We address this by formulating noise covariance estimation as a bilevel optimization that, from a Bayesian perspective, factorizes the joint likelihood of so-called odometry and supervisory measurements, thereby balancing information utilization with computational efficiency. The factorization converts the nested Bayesian dependency into a chain structure, enabling efficient parallel computation: at the lower level, an invariant extended Kalman filter with state augmentation estimates trajectories, while a derivative filter computes analytical gradients in parallel for upper-level gradient updates. The upper level refines the covariance to guide the lower-level estimation. Experiments on synthetic and real-world datasets show that our method achieves higher efficiency over existing baselines.

Paper Structure

This paper contains 14 sections, 2 theorems, 38 equations, 5 figures, 1 table, 1 algorithm.

Table of Contents

  1. INTRODUCTION
  2. Related Works
  3. Preliminary on Robotic Lie Groups
  4. Problem Statement
  5. Methodology
  6. Problem Formulation
  7. State Filter
  8. Derivatives of lo and ls w.r.t. theta
  9. Methodology Summary and More Discussions
  10. Simulation and Experiment
  11. Baselines
  12. Simulation
  13. Dataset Experiment
  14. Conclusion and Future Work

Key Result

Theorem 1

The derivative of $\ell^o({\theta})$ with respect to each component $\theta_j$ of $\theta$ is the sum of its derivatives from each time step: Each constituent term $\frac{\partial l_k^o({\theta})}{\partial \theta_j}$ has the form with $S_k({\theta})$ and $r_k({\theta})$ abbreviated as $S_k$ and $r_k$. The computation of $\partial S_k / \partial \theta_j$ and $\partial r_k / \partial \theta_j$ fo

Figures (5)

  • Figure 1: System framework.
  • Figure 2: Logical flow for solving the bilevel problem.
  • Figure 3: (a) Loop-to-Open dataset (Synthetic). Left: trajectory with calibration results; Right: pose estimation error over time. The lowest-MSE methods are highlighted during two stages. (b) Visualization of the Garage dataset. (c) Trajectory comparison using only IMU or binary measurements, and trajectories before and after parameter tuning.
  • Figure 4: Comparison of the numerical and analytical gradients. The lower panel reports the normalized error $((\partial \mathcal{L}/\partial \theta_j)_{\mathrm{ana}} - (\partial \mathcal{L}/\partial \theta_j)_{\mathrm{num}}) / ((\partial \mathcal{L}/\partial \theta_j)_{\mathrm{num}})$.
  • Figure 5: Test MSE and Wasserstein error across $\alpha$.

Theorems & Definitions (4)

  • Remark 1
  • Remark 2: Dimension Control.
  • Theorem 1: Derivative of $\ell^o$
  • Theorem 2: Derivative of $\ell^s$