Posterior linearisation smoothing with robust iterations

Jakob Lindqvist, Simo Särkkä, Ángel F. García‐Fernández, Matti Raitoharju, Lennart SvenssonarXiv 2021 · DOI · arXiv

TL;DR

This paper introduces robust versions of the Iterated Posterior Linearisation Smoother (IPLS) by integrating Levenberg–Marquardt regularization and line-search methods. By framing smoothing as an optimization problem, the authors create algorithms that are significantly more stable and accurate than standard smoothers in highly nonlinear tracking scenarios.

What Problem Does This Paper Address?

In state estimation, "smoothing" involves estimating the past states of a system (like a vehicle's trajectory) based on a sequence of noisy measurements. When the system's movement or the sensors are highly nonlinear (e.g., tracking a turn using only angles/bearings), standard tools like the Extended Kalman Smoother (EKS) often struggle because they rely on simple linear approximations that can be wildly inaccurate.

Iterative smoothers (like IEKS and IPLS) try to fix this by repeatedly refining their estimates. However, these iterations aren't guaranteed to converge; they can diverge or "blow up," leading to useless results. The authors tackle this instability by applying classic optimization techniques—Levenberg–Marquardt (LM) and Line-Search (LS)—to ensure that each iteration actually moves the estimate closer to the truth.

Methods

The authors' primary contribution is bridging the gap between Bayesian smoothing and mathematical optimization.

  1. Gauss-Newton (GN) Interpretation: They prove that one iteration of the IPLS is mathematically equivalent to a single step of the Gauss-Newton optimization method. This allows them to treat smoothing as a cost-minimization problem.
  2. LM–IPLS: They introduce Levenberg–Marquardt regularization. A clever insight here is that LM regularization can be implemented as a "pseudo-measurement" of the state. This means engineers can add robustness to their existing smoothing code with minimal architectural changes.
  3. LS–IPLS: They derive the Armijo–Wolfe conditions specifically for the IPLS cost function. This enables an "inexact line-search" that prevents the algorithm from taking steps that are too large or that increase the error.
  4. Statistical Linear Regression (SLR): Unlike the IEKS, which uses Jacobians (slopes at a single point), the IPLS uses SLR. This method linearizes the model by looking at a distribution of points (often using sigma points), making it inherently more robust to local nonlinearities.

Datasets

The paper relies on simulated benchmarks rather than real-world datasets to maintain controlled conditions for testing nonlinearity:

  • Coordinated Turn (CT) Model: A standard benchmark for target tracking where a vehicle moves at a constant speed and turn rate.
  • Bearings-Only Measurements: The sensors only provide the angle to the target, not the distance. This is a classic "hard" problem in estimation because the relationship between the state (position) and the measurement (angle) is highly nonlinear.
  • Challenging Nonlinear Scenario: A custom-designed simulation where sensors provide intermittent, low-noise readings that create sharp "peaks" in the probability distribution, often causing standard smoothers to fail.

Key Insights and Ideas

  • Smoothing as Optimization: The "aha moment" is the formal proof that IPLS is a GN method. Once you view a smoother as an optimizer, you can borrow powerful tools from the optimization community to fix convergence issues.
  • The Pseudo-Measurement Trick: Realizing that LM regularization is just a fake measurement of the current state estimate is a brilliant practical shortcut. It allows the use of standard Rauch–Tung–Striebel (RTS) smoothing equations to solve a regularized optimization problem.
  • IPLS > IEKS: The results reinforce that linearizing over a distribution (IPLS) is superior to linearizing at a single point (IEKS), especially when the initial guess is poor or the model is complex.

Key Results

  • Superior Stability: In highly nonlinear tests, standard IEKS and IPLS frequently diverged (errors went to infinity). The LM and LS versions remained stable and converged to accurate trajectories.
  • Faster Convergence: In standard scenarios, the proposed methods reached a low Root Mean Square Error (RMSE) in fewer iterations than their non-robust counterparts.
  • Accuracy: The LM–IPLS and LS–IPLS consistently achieved lower RMSE and better Normalised Estimation Error Squared (NEES) scores, indicating not just better estimates, but better-calibrated uncertainty.

Limitations and Future Work

  • Computational Cost: Each iteration of an IPLS-based method is more expensive than an EKS-based one because it requires calculating expectations (often via sigma points) rather than simple derivatives.
  • Additive Noise Assumption: The current derivation assumes additive noise in the motion and measurement models. Extending this to non-additive noise would be a natural next step.
  • Parameter Tuning: While the LM method is "adaptive," it still introduces parameters (like the initial $\lambda$ and the scaling factor $\nu$) that may require tuning for specific applications.

Summary updated

Abstract

This paper considers the problem of iterative Bayesian smoothing in nonlinear state-space models with additive noise using Gaussian approximations. Iterative methods are known to improve smoothed estimates but are not guaranteed to converge, motivating the development of methods with better convergence properties. The aim of this article is to extend Levenberg-Marquardt (LM) and line-search versions of the classical iterated extended Kalman smoother (IEKS) to the iterated posterior linearisation smoother (IPLS). The IEKS has previously been shown to be equivalent to the Gauss-Newton (GN) method. We derive a similar GN interpretation for the IPLS and use this to develop extensions to the IPLS, with improved convergence properties. We show that an LM extension for the IPLS can be achieved with a simple modification of the smoothing iterations, enabling algorithms with efficient implementations. We also derive the Armijo--Wolfe step length conditions for the IPLS enabling an efficient inexact line-search method. Our numerical experiments show the benefits of these extensions in highly nonlinear scenarios.
BibTeX
@misc{lindqvist2021posterior,
  author = {Jakob Lindqvist and Simo Särkkä and Ángel F. García‐Fernández and Matti Raitoharju and Lennart Svensson},
  title = {{Posterior linearisation smoothing with robust iterations}},
  year = {2021},
  eprint = {2112.03969},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2112.03969},
}
Figure