Natural Gradient Gaussian Approximation Filter on Lie Groups for Robot State Estimation

2026-04-11

AI Summary

This work addresses the issue of state estimation error accumulation on Lie group manifolds caused by linearization of nonlinear observation models in tangent spaces. To circumvent linearization altogether, the authors propose a natural gradient Gaussian approximation filtering framework. The approach reformulates manifold-based filtering as a parameter optimization problem over Gaussian incremental variables, where increments are mapped onto the prior state via the exponential map and iteratively refined using natural gradients. Under invariant observation models, a closed-form covariance update is derived, achieving a favorable balance between accuracy and computational efficiency. Experimental validation on the Unitree GO2 quadruped robot across diverse terrains demonstrates approximately 40% reduction in estimation error compared to existing filters, with comparable computational overhead.

Technology Category

• Intelligent Robots: State Estimation
• Machine Learning: Learning with Manifolds
• Natural Language Processing: Learning & Optimization for NLP

Application Category

• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs
• User Modeling, Personalization and Recommendation: On-Device user modeling, personalization, and recommendation
• Search and Retrieval-Augmented AI: Web learning to rank, online learning, and counterfactual learning for ranking

Abstract

Accurate state estimation for robotic systems evolving on Lie group manifolds, such as legged robots, is a prerequisite for achieving agile control. However, this task is challenged by nonlinear observation models defined on curved manifolds, where existing filters rely on local linearization in the tangent space to handle such nonlinearity, leading to accumulated estimation errors. To address this limitation, we reformulate manifold filtering as a parameter optimization problem over a Gaussian-distributed increment variable, thereby avoiding linearization. Under this formulation, the increment can be mapped to the Lie group through the exponential operator, where it acts multiplicatively on the prior estimate to yield the posterior state. We further propose a natural gradient optimization scheme for solving this problem, whose iteration process leverages the Fisher information matrix of the increment variable to account for the curvature of the tangent space. This results in an iterative algorithm named the Natural Gradient Gaussian Approximation on Lie Groups (NANO-L) filter. Leveraging the perturbation model in Lie derivative, we prove that for the invariant observation model widely adopted in robotic localization tasks, the covariance update in NANO-L admits an exact closed-form solution, eliminating the need for iterative updates thus improving computational efficiency. Hardware experiments on a Unitree GO2 legged robot operating across different terrains demonstrate that NANO-L achieves approximately 40% lower estimation error than commonly used filters at a comparable computational cost.

Problem

• state estimation
• Lie groups
• nonlinear observation models
• manifold filtering
• estimation error

Innovation

• Natural Gradient
• Gaussian Approximation
• Lie Groups
• State Estimation
• Manifold Filtering