Data-Driven Batch Localization and SLAM Using Koopman Linearization

TL;DR

This work tackles batch localization and SLAM for robots with unknown or imperfect dynamics by learning a lifted bilinear model via Koopman lifting. It introduces Reduced Constrained Koopman Linearization (RCKL) that combines learned lifted-process models with manifold constraints enforced through an SQP, yielding linear-time per-iteration complexity with respect to timesteps. The method unifies UKL, CKL, and its reduced variant, demonstrating that CKL and especially RCKL achieve robust, accurate localization and SLAM across simulations and real datasets (indoor laser and RFID-based mapping) even when prior models are imperfect. The results indicate that data-driven lifting can match or surpass model-based performance in realistic conditions and offers improved convergence properties, signaling practical impact for robust robotic navigation with unknown dynamics.

Abstract

We present a framework for model-free batch localization and SLAM. We use lifting functions to map a control-affine system into a high-dimensional space, where both the process model and the measurement model are rendered bilinear. During training, we solve a least-squares problem using groundtruth data to compute the high-dimensional model matrices associated with the lifted system purely from data. At inference time, we solve for the unknown robot trajectory and landmarks through an optimization problem, where constraints are introduced to keep the solution on the manifold of the lifting functions. The problem is efficiently solved using a sequential quadratic program (SQP), where the complexity of an SQP iteration scales linearly with the number of timesteps. Our algorithms, called Reduced Constrained Koopman Linearization Localization (RCKL-Loc) and Reduced Constrained Koopman Linearization SLAM (RCKL-SLAM), are validated experimentally in simulation and on two datasets: one with an indoor mobile robot equipped with a laser rangefinder that measures range to cylindrical landmarks, and one on a golf cart equipped with RFID range sensors. We compare RCKL-Loc and RCKL-SLAM with classic model-based nonlinear batch estimation. While RCKL-Loc and RCKL-SLAM have similar performance compared to their model-based counterparts, they outperform the model-based approaches when the prior model is imperfect, showing the potential benefit of the proposed data-driven technique.

Figures (11)

• Figure 1: Visualization of the trajectory output of RCKL-Loc (left), and the trajectory and landmark output of RCKL-SLAM (right), on Experiment 2 described in Section \ref{['sec:exp-golf-cart']}, showing the estimators' mean states and mean landmark positions compared to the groundtruth. The green regions and the grey regions are, respectively, the $3\sigma$ covariances of the trajectory and of the landmarks. The estimators' trajectories and landmarks are close to the groundtruth and are within the estimated $3\sigma$ bounds, despite the algorithm having no prior knowledge of the system models.
• Figure 2: Visualization of the trajectory output of UKL-Loc (left), and the trajectory and landmark output of UKL-SLAM (right), on Experiment 2 described in Section \ref{['sec:exp-golf-cart']}, showing the estimators' mean states and mean landmark positions compared to the groundtruth. The pink lines show the landmark correspondances between the groundtruth and the output of UKL-SLAM. We observe that the trajectory estimate of UKL-Loc is poor, which we typically see when the measurements are sporadic. For UKL-SLAM, both the trajectory and the landmark estimates are poor, which we typically see regardless of the regularity of measurements.
• Figure 3: UKL vs. CKL vs. RCKL in noiseless dead reckoning. We train the three estimators on noiseless data corresponding to Simulation 1 described in Section \ref{['sec:simu-1']}, then compare their dead-reckoning outputs on a noiseless test trajectory. UKL's output is on top of groundtruth at first but eventually drifts off as the states deviate from the feature manifold. CKL's output remains on the feature manifold, but solution is worsened by the poor process models on the features. Meanwhile, RCKL stays on the manifold and its output corresponds to the groundtruth almost exactly.
• Figure 4: Localization and SLAM results for Simulation 1 (left) and Simulation 2 (right). RMSEs and Mahalanobis distances for localization (left plots) and SLAM (right plots) for UKL, CKL, RCKL, MB (model-based with correct $\mu$), and MBI (model-based with imperfect $\mu$). In both simulations, RCKL has slightly higher RMSEs than MB but encounters local minima less frequently, and it has lower RMSEs than MBI. UKL-SLAM RMSEs are off the charts, and CKL-SLAM RMSEs are more reasonable than UKL-SLAM but still higher than RCKL-SLAM, demonstrating the necessity of the constraints and the reduced process model in RCKL. RCKL is also more consistent than MB and MBI.
• Figure 5: Setup for Experiment 1. A wheeled robot drives around 17 cylindrical landmarks (2 are not visible in this photo) in an indoor environment. It logs wheel odometry and measures the range to its surrounding landmarks using a laser rangefinder. The landmarks for testing are highlighted in red.