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 classical Bayesian filters and smoothers. We formulate the estimation problem as a dynamic generalised group Lasso problem and develop a class of smoothing-and-splitting methods to solve it. The Levenberg–-Marquardt iterated extended Kalman smoother-based multi-block alternating direction method of multipliers (LM-IEKS-mADMM) algorithms are based on the alternating direction method of multipliers (ADMM) framework. This leads to minimisation subproblems with an inherent structure to which three new augmented recursive smoothers are applied. Our methods can deal with large-scale problems without pre-processing for dimensionality reduction. Moreover, the methods allow one to solve nonsmooth nonconvex optimisation problems. We then prove that under mild conditions, the proposed methods converge to a stationary point of the optimisation problem. By simulated and real-data experiments including multi-sensor range measurement problems, marine vessel tracking, autonomous vehicle tracking, and audio signal restoration, we show the practical effectiveness of the proposed methods.
The Challenge of Structured Sparsity
In autonomous navigation for ships, cars, and drones, estimating a "state" (such as position and velocity) from noisy sensor data is a fundamental task. Standard tools like the Kalman Filter typically assume noise is Gaussian and continuous. However, real-world physics often dictates sparsity. For example, a car at a red light has zero velocity and zero angular change; these variables are "sparse" because they are zero frequently, and they are "grouped" because they tend to go to zero simultaneously.
Existing sparse tracking methods often penalize individual variables (standard Lasso), which ignores these physical group relationships. Furthermore, solving these optimization problems for long time series is computationally intensive—standard "batch" solvers slow down exponentially as the duration of the track increases.
Methods: Dynamic Generalised Group Lasso
The authors propose a Dynamic Generalised Group Lasso formulation to address these structured sparsity patterns. To solve it efficiently, they utilize a "smoothing-and-splitting" approach that breaks the complex, non-smooth optimization problem into smaller, manageable subproblems.
Algorithmic Innovation
The core innovation is the realization that the subproblems within the Multi-block ADMM (mADMM) framework share the exact mathematical structure of a Kalman smoothing problem. By "fusing" ADMM constraints into artificial state-space models, the authors can apply Augmented Recursive Smoothers:
- Markovian Leveraging: Instead of treating the whole track as one giant matrix, the algorithm exploits the fact that the state at time t only depends on time t-1. This reduces computational complexity from $O(T^3)$ to $O(T)$.
- LM-IEKS: For non-linear and difficult problems, they incorporate a Levenberg-Marquardt (LM) adaptation to ensure robust convergence to a stationary point, even in non-convex scenarios.
Experimental Validation
The researchers evaluated their methods across five distinct scenarios, ranging from simulated linear tracking to real-world GPS and AIS data. Key datasets included:
- Marine Vessel Tracking: Real-world AIS data from the Danish Maritime Authority.
- Autonomous Vehicle Tracking: GPS data from urban streets in Helsinki, Finland, featuring many "stop-and-go" segments.
- Audio Signal Restoration: A glockenspiel recording treated as a sparse signal estimation problem.
Key Results and Insights
- Massive Speedup: In a linear tracking test with 10,000 time steps, the proposed KS-mADMM took just 1.92 seconds, while the standard batch mADMM took 9,646 seconds (over 2.5 hours).
- Improved Accuracy: In marine and car tracking experiments, the method significantly reduced estimation error compared to standard Iterated Extended Kalman Smoothers (IEKS) by correctly "pushing" near-zero velocities to exactly zero.
- Group Sparsity Effectiveness: By penalizing groups of elements rather than individual ones, the tracker is far more effective at identifying physical events like a vehicle coming to a complete stop.
Conclusion
This framework provides a scalable and accurate solution for autonomous tracking under sparsity constraints. While the algorithm currently requires manual hyperparameter tuning, its ability to handle large-scale, non-convex problems in linear time marks a significant advancement. Future work may involve extending this framework into optimal control for energy-efficient path planning.
Summary updated