Learning Covariances for Estimation with Constrained Bilevel Optimization

TL;DR

This paper introduces a gradient-based approach to learn well-conditioned error covariance matrices for robotic state estimation, improving stability and accuracy in both simulated and real-world tasks.

Contribution

It formulates covariance learning as a constrained bilevel optimization problem over factor graphs, a novel approach for tuning noise models in state estimation.

Findings

Improved tracking accuracy on unseen trajectories.

Converges to better covariance estimates.

Effective in both simulated and real-world scenarios.

Abstract

We consider the problem of learning error covariance matrices for robotic state estimation. The convergence of a state estimator to the correct belief over the robot state is dependent on the proper tuning of noise models. During inference, these models are used to weigh different blocks of the Jacobian and error vector resulting from linearization and hence, additionally affect the stability and convergence of the non-linear system. We propose a gradient-based method to estimate well-conditioned covariance matrices by formulating the learning process as a constrained bilevel optimization problem over factor graphs. We evaluate our method against baselines across a range of simulated and real-world tasks and demonstrate that our technique converges to model estimates that lead to better solutions as evidenced by the improved tracking accuracy on unseen test trajectories.