On State Estimation in Multi-Sensor Fusion Navigation: Optimization and Filtering

Feng Zhu, Zhuo Xu, Xveqing Zhang, Yuantai Zhang, Weijie Chen, Xiaohong Zhang

Abstract

The essential of navigation, perception, and decision-making which are basic tasks for intelligent robots, is to estimate necessary system states. Among them, navigation is fundamental for other upper applications, providing precise position and orientation, by integrating measurements from multiple sensors. With observations of each sensor appropriately modelled, multi-sensor fusion tasks for navigation are reduced to the state estimation problem which can be solved by two approaches: optimization and filtering. Recent research has shown that optimization-based frameworks outperform filtering-based ones in terms of accuracy. However, both methods are based on maximum likelihood estimation (MLE) and should be theoretically equivalent with the same linearization points, observation model, measurements, and Gaussian noise assumption. In this paper, we deeply dig into the theories and existing strategies utilized in both optimization-based and filtering-based approaches. It is demonstrated that the two methods are equal theoretically, but this equivalence corrupts due to different strategies applied in real-time operation. By adjusting existing strategies of the filtering-based approaches, the Monte-Carlo simulation and vehicular ablation experiments based on visual odometry (VO) indicate that the strategy adjusted filtering strictly equals to optimization. Therefore, future research on sensor-fusion problems should concentrate on their own algorithms and strategies rather than state estimation approaches.

Introduction

In the development of autonomous systems such as UAVs and self-driving cars, state estimation—the process of fusing noisy sensor data (cameras, IMUs, GNSS) to determine a robot's pose—is a critical challenge. Historically, the robotics community has been divided between two primary methodologies: Filtering (e.g., Extended Kalman Filter) and Optimization (e.g., Factor Graphs).

While recent trends in SLAM and computer vision have favored optimization for its perceived superior accuracy, both methods are fundamentally rooted in Maximum Likelihood Estimation (MLE). This paper addresses the apparent contradiction: if the underlying mathematics are identical, why do practical results diverge? We identify the specific implementation strategies that cause this divergence and demonstrate how filtering can achieve the same accuracy as optimization.

Methodology: Identifying the Divergence

The research identifies three primary "corruptions" in real-time filtering that lead to accuracy degradation compared to optimization:

  1. Prior Constraints: Optimization typically marginalizes only the oldest states, whereas filtering often converts the entire window into a prior, losing the ability to refine older measurements.
  2. Measurement Utilization: Optimization allows for the re-linearization of all measurements within a window. Standard filters often use a measurement once and discard it.
  3. Linearization Consistency: Filters frequently suffer from "Jacobian inconsistency," where different states are linearized at different points in time, introducing spurious information into the system.
Comparison of Sliding Window Optimization (SWO) and Sliding Window Filtering (SWF) strategies, illustrating how measurements are converted to constraints and removed differently.

To resolve these issues, we propose a 2-step update strategy for filtering. The first step updates states using only measurements of marginalized states, while the second step treats remaining measurements as a "virtual" update. This ensures the filter maintains the same linearization points and constraints as a batch optimizer.

Key Insights: The Role of Linearization

The perceived superiority of optimization is not due to the algorithm itself, but rather better "bookkeeping" regarding linearization points. In a perfectly linear system, the two methods are identical. The divergence in non-linear real-world systems occurs because optimization-based frameworks act as iterative filters that maintain consistency across the state window.

Comparison of batch estimation (a) versus sequential estimation (b). The color gradient indicates how linearization accuracy evolves, highlighting the potential for Jacobian inconsistency in sequential filters.

By adjusting the filtering strategy to match the optimization logic, the choice of estimator becomes a matter of computational preference—balancing the number of landmarks versus poses—rather than a trade-off in accuracy.

Experimental Results

The proposed Strategy Adjusted Filter (SWF-SA) was validated through both simulation and real-world data:

  • Numerical Equivalence: In Monte-Carlo trials, the discrepancy between SWF-SA and Sliding Window Optimization (SWO) was negligible, reaching $10^{-6}$ meters for position and $10^{-7}$ degrees for attitude.
  • KITTI Dataset Validation: Using sequences 00-10 of the KITTI benchmark with ORB-SLAM2 as a frontend, the adjusted filter achieved accuracy strictly equal to the optimizer across all scenarios. This proves that the 2-step update effectively mitigates the linearization errors typical of standard EKFs.

Conclusion and Future Work

This study demonstrates that the debate between "Filter vs. Optimizer" is largely a matter of implementation strategy. When strategies are aligned, the methods are equivalent. Future research should focus on refining sensor models and fusion strategies rather than the choice of state estimator.

Limitations of this work include the reliance on the Gaussian noise assumption. Future efforts will explore non-Gaussian noise environments and apply these strategy adjustments to more complex sensor suites, including high-rate IMUs and LiDAR-based systems.

Summary updated

Abstract

The essential of navigation, perception, and decision-making which are basic tasks for intelligent robots, is to estimate necessary system states. Among them, navigation is fundamental for other upper applications, providing precise position and orientation, by integrating measurements from multiple sensors. With observations of each sensor appropriately modelled, multi-sensor fusion tasks for navigation are reduced to the state estimation problem which can be solved by two approaches: optimization and filtering. Recent research has shown that optimization-based frameworks outperform filtering-based ones in terms of accuracy. However, both methods are based on maximum likelihood estimation (MLE) and should be theoretically equivalent with the same linearization points, observation model, measurements, and Gaussian noise assumption. In this paper, we deeply dig into the theories and existing strategies utilized in both optimization-based and filtering-based approaches. It is demonstrated that the two methods are equal theoretically, but this equivalence corrupts due to different strategies applied in real-time operation. By adjusting existing strategies of the filtering-based approaches, the Monte-Carlo simulation and vehicular ablation experiments based on visual odometry (VO) indicate that the strategy adjusted filtering strictly equals to optimization. Therefore, future research on sensor-fusion problems should concentrate on their own algorithms and strategies rather than state estimation approaches.
BibTeX
@misc{zhu2024state,
  author = {Feng Zhu and Zhuo Xu and Xveqing Zhang and Yuantai Zhang and Weijie Chen and Xiaohong Zhang},
  title = {{On State Estimation in Multi-Sensor Fusion Navigation: Optimization and Filtering}},
  year = {2024},
  eprint = {2401.05836},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2401.05836},
}
Figure