Learning Covariances for Estimation with Constrained Bilevel Optimization

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.

Authors

• Mohamad Qadri
• Zachary Manchester
• Michael Kaess

Keywords

• Vectors
• Stability analysis
• Trajectory
• Covariance matrices
• Task analysis
• State estimation
• Robots
• Bilevel Optimization
• Noise Model
• Tracking Accuracy
• Model Tuning
• Error Covariance Matrix
• Robot State
• Factor Graph
• Bilevel Optimization Problem
• Training Set
• Diagonal Matrix
• Network Output
• Nonlinear Least Squares
• Positive Definite Matrix
• Tracking Error
• Sensor Measurements
• Numerical Stability
• Lie Algebra
• Maximum A Posteriori
• Minimum Eigenvalue
• Numerical Differentiation
• Hard Constraints
• Training Trajectories
• Implicit Function Theorem
• Lie Group
• Ground Truth Trajectory
• Graph Optimization
• Output Trajectory
• Positive Definite
• Manual Tuning
• Mathematical Statistics