Autonomous Tracking and State Estimation With Generalized Group Lasso

Rui Gao, Simo Särkkä, Rubén Claveria-Vega, Simon Godsill

Abstract

We address the problem of autonomous tracking and state estimation for marine vessels, autonomous vehicles, and other dynamic signals under a (structured) sparsity assumption. The aim is to improve the tracking and estimation accuracy with respect to the classical Bayesian filters and smoothers. We formulate the estimation problem as a dynamic generalized group Lasso problem and develop a class of smoothing-and-splitting methods to solve it. The Levenberg-Marquardt iterated extended Kalman smoother-based multiblock alternating direction method of multipliers (LM-IEKS-mADMMs) algorithms are based on the alternating direction method of multipliers (ADMMs) framework. This leads to minimization subproblems with an inherent structure to which three new augmented recursive smoothers are applied. Our methods can deal with large-scale problems without preprocessing for dimensionality reduction. Moreover, the methods allow one to solve nonsmooth nonconvex optimization problems. We then prove that under mild conditions, the proposed methods converge to a stationary point of the optimization problem. By simulated and real-data experiments, including multisensor range measurement problems, marine vessel tracking, autonomous vehicle tracking, and audio signal restoration, we show the practical effectiveness of the proposed methods.

Introduction

In the domain of autonomous systems—from self-driving cars to marine vessels—estimating a system's state (position, velocity, orientation) from noisy sensor data is a fundamental challenge. Traditional Bayesian filters, such as the Kalman Filter, are the industry standard but often struggle when signals exhibit specific structures like sparsity. For example, a vehicle may remain stationary for long periods, or an audio signal may consist of only a few active frequencies. This paper introduces a framework that integrates structured sparsity into tracking by combining the statistical rigor of Kalman smoothing with the flexibility of the Generalized Group Lasso (GGL).

Methodology: The LM-IEKS-mADMM Framework

The primary innovation of this work is the LM-IEKS-mADMM algorithm. This method addresses the computational bottleneck of standard optimization solvers, which typically scale cubically ($O(T^3)$) with the number of time steps $T$. By leveraging the Markovian structure of time-series data, the proposed method achieves linear time complexity ($O(T)$).

Generalized Group Lasso (GGL)

The tracking problem is formulated as a minimization task where the penalty term is a GGL. This allows for grouping related variables—such as $x$ and $y$ velocity components—and forcing them to be zero simultaneously, which is physically more realistic than penalizing them individually. The framework supports a wide variety of regularization types as detailed below:

Table 1: Regularization types and their matrix descriptions in the GGL framework.

Sparsity in Dynamic Systems

The framework is versatile, handling both linear (Affine Gaussian) and nonlinear systems. Sparsity can be enforced either on the state itself or on the process noise, depending on the physical characteristics of the system being modeled. This flexibility allows the tracker to "know" when a ship is likely to be stationary or when an audio signal is sparse in the Gabor domain.

Table 2: Sparsity configurations for different dynamic system models.

Augmented Recursive Smoothers

The Alternating Direction Method of Multipliers (ADMM) is used to split the complex optimization problem into smaller subproblems. The authors recognized that the "primal" update in this splitting process has the same mathematical structure as a state-space model. They developed augmented Kalman smoothers (KS, GN-IEKS, and LM-IEKS) to solve these subproblems efficiently. The Levenberg-Marquardt (LM) version ensures robustness even with highly nonlinear sensor models or rank-deficient matrices.

Experimental Results and Insights

The performance of the LM-IEKS-mADMM framework was evaluated across diverse scenarios, including simulated car tracking, marine vessel tracking using AIS data, and audio signal restoration.

  • Computational Efficiency: In a car tracking simulation with 10,000 time steps, the proposed KS-mADMM took only 1.92 seconds, whereas a standard batch ADMM solver required 9,646 seconds.
  • Estimation Accuracy: In marine vessel tracking, the method correctly identified periods of zero motion, eliminating the "jitter" typically seen in standard filters due to sensor noise.
  • Audio Restoration: The method achieved a higher Signal-to-Noise Ratio (12.4 dB) and better perceptual scores than traditional Gibbs sampling methods, while running significantly faster.

Conclusion

The proposed framework bridges the gap between high-fidelity structured sparsity optimization and the efficiency of recursive Bayesian filtering. It provides formal convergence guarantees for nonsmooth and nonconvex problems, making it a robust choice for real-time autonomous tracking. Future work will focus on automating the tuning of penalty parameters and extending the framework to handle even more extreme nonlinearities via sigma-point filters.

Summary updated

Abstract

We address the problem of autonomous tracking and state estimation for marine vessels, autonomous vehicles, and other dynamic signals under a (structured) sparsity assumption. The aim is to improve the tracking and estimation accuracy with respect to the classical Bayesian filters and smoothers. We formulate the estimation problem as a dynamic generalized group Lasso problem and develop a class of smoothing-and-splitting methods to solve it. The Levenberg-Marquardt iterated extended Kalman smoother-based multiblock alternating direction method of multipliers (LM-IEKS-mADMMs) algorithms are based on the alternating direction method of multipliers (ADMMs) framework. This leads to minimization subproblems with an inherent structure to which three new augmented recursive smoothers are applied. Our methods can deal with large-scale problems without preprocessing for dimensionality reduction. Moreover, the methods allow one to solve nonsmooth nonconvex optimization problems. We then prove that under mild conditions, the proposed methods converge to a stationary point of the optimization problem. By simulated and real-data experiments, including multisensor range measurement problems, marine vessel tracking, autonomous vehicle tracking, and audio signal restoration, we show the practical effectiveness of the proposed methods.
BibTeX
@article{gao2021autonomous,
  author = {Rui Gao and Simo Särkkä and Rubén Claveria-Vega and Simon Godsill},
  title = {{Autonomous Tracking and State Estimation With Generalized Group Lasso}},
  journal = {IEEE Transactions on Cybernetics},
  year = {2021},
  doi = {10.1109/tcyb.2021.3085426},
}
Figure