navlie: A Python Package for State Estimation on Lie Groups

Robotic state estimation has evolved from simple Euclidean filters to sophisticated manifold-based algorithms. Early Kalman filters excelled in linear spaces but struggled with rotations and poses represented on Lie groups like SO(3) and SE(3). Recent developments incorporate Lie theory, enabling algorithms such as invariant EKF and equivariant filters, which improve consistency and robustness. Existing software tools like GTSAM and Ceres support manifold optimization but are primarily C++-based, limiting accessibility and rapid development. The need for flexible, high-level tools in Python has become evident, especially for research prototyping and multi-sensor fusion. This work aims to fill this gap by providing a comprehensive, easy-to-use Python library supporting multiple Lie group estimation algorithms.

In robotics, states such as orientation and pose naturally reside on nonlinear manifolds. Traditional filters operating in Euclidean space often produce inconsistent estimates when applied directly to these states, leading to bias and divergence. Existing tools lack flexibility, requiring extensive custom coding for each model and algorithm. Moreover, real-time applications demand efficient implementations that can handle high data rates and complex models. The core challenge is to develop a unified, modular framework that ensures geometric consistency, supports multiple algorithms, and is accessible for rapid testing and deployment in diverse robotic scenarios.

This work introduces a Python-based library supporting multiple Lie group filters—EKF, iterated EKF, invariant EKF, sigma-point filters—within a unified interface. Key innovations include: 1) abstract model classes with flexible definitions of ⊕ and ⊖ operators, 2) leveraging Lie algebra exponential and logarithmic maps for accurate manifold operations, 3) integrated preintegration modules for IMU and wheel odometry, 4) utilities for numerical differentiation and batch optimization, and 5) support for multi-model and multi-sensor fusion. These features enable rapid prototyping, easy model customization, and robust estimation in complex scenarios, bridging the gap between theoretical advances and practical robotics applications.

• �� Define abstract classes for states, process, and measurement models, implementing ⊕ and ⊖ operators compatible with Lie groups.
• �� Implement multiple estimation algorithms (EKF, iterated EKF, invariant EKF, sigma-point filters) that operate directly on these models, using exponential/logarithmic maps for manifold consistency.
• �� Incorporate process models such as IMU preintegration, wheel odometry, and linear motion, with automatic Jacobian and covariance computation.
• �� Use numerical differentiation (finite difference, complex step) for models lacking analytical Jacobians.
• �� Support batch optimization via nonlinear least squares, utilizing Gauss-Newton and Levenberg-Marquardt algorithms.
• �� Provide utilities for Monte Carlo simulations, error analysis, and sensor data visualization.
• �� Enable flexible model composition, including composite states and hierarchical structures, for complex robotic systems.

在KITTI和IMU-视觉SLAM数据集上，验证了navlie的滤波算法性能。通过不同噪声水平和参数设置，比较EKF、UKF和批量优化的定位误差。误差指标包括平均位置误差、NEES和收敛速度。多次仿真验证了算法在高噪声和模型误差条件下的鲁棒性。结果显示，navlie的滤波器在定位精度上优于传统欧几里得空间滤波器，误差降低20%以上，收敛速度提升30%。多模型滤波在动态场景中表现出更强的适应性，误差保持在预期范围内，验证了其实用性。

实验结果表明，navlie实现的滤波器在KITTI数据集上的平均位置误差低于1.5米，比传统方法低20%。在IMU-视觉SLAM中，误差标准差降低15%，鲁棒性增强。批量优化在大规模SLAM中实现了30%的时间缩短，收敛更快。多模型滤波器在动态环境中表现出更好的适应性，误差保持在预期范围内，验证了其在复杂场景中的实用性。

navlie适用于自主导航、SLAM、多传感器融合等场景。用户只需定义模型，即可快速部署滤波器，满足机器人、无人机等平台的实时定位需求。其Python实现降低了科研验证和工业应用的门槛，便于快速验证新算法和多传感器方案。

目前在高频率大规模SLAM任务中，性能仍需优化，特别是在GPU加速方面。模型库虽丰富，但在极端复杂环境（如多机器人、多源异构传感器）中的适应性尚待验证。部分算法对参数敏感，调参复杂。未来将加强算法的实时性和扩展性，提升在实际应用中的鲁棒性。