TL;DR
This paper introduces a more computationally efficient Extended Kalman Filter (EKF) approach for magnetic field Simultaneous Localization and Mapping (SLAM). By leveraging the analytical gradients of a Gaussian Process (GP) based magnetic field map, their EKF-based method achieves comparable drift compensation to existing particle filter methods but with significantly reduced computational cost.
What Problem Does This Paper Address?
Autonomous navigation systems often rely on odometry (dead reckoning) which integrates sensor measurements like accelerometers and gyroscopes to estimate position and orientation. However, this process is prone to drift, meaning the estimated position can become increasingly inaccurate over time, especially in environments where external positioning systems like GPS are unavailable (e.g., indoors, underwater). Magnetic field SLAM aims to mitigate this drift by using the Earth's ambient magnetic field, which exhibits unique spatial variations due to local ferromagnetic structures, as a form of "natural landmark." Existing magnetic field SLAM methods often employ Rao-Blackwellized Particle Filters (RBPFs), which can be computationally expensive due to the need to maintain multiple particles, each with its own magnetic field map. This paper addresses the computational bottleneck of RBPF-based magnetic field SLAM, seeking a more efficient alternative.
Methods
The core of this work is the development of an Extended Kalman Filter (EKF) for magnetic field SLAM. The authors build upon a previous approach that models the magnetic field using reduced-rank Gaussian Process (GP) regression with Laplace basis functions. A key innovation here is the exploitation of the analytical gradients of this GP-based magnetic field map.
Here's a breakdown of their approach:
- Magnetic Field Modeling: They use a reduced-rank GP with Laplace basis functions to represent the magnetic field. This allows for an analytical expression of the magnetic field's gradient with respect to position.
- EKF Formulation: Instead of a particle filter, they employ an EKF. This is possible because the dynamic model and the linearized measurement model are assumed to be sufficiently close to linear.
- Gradient-Based Measurement Update: The EKF's measurement update step utilizes the analytical gradients of the magnetic field map. This allows the filter to directly use the relationship between position and magnetic field measurements to correct the state estimate.
- Single Map Maintenance: A significant advantage of their EKF approach is that it only requires maintaining and updating a single magnetic field map, unlike RBPFs which maintain a map for each particle.
Datasets
The paper evaluates their proposed EKF-based magnetic field SLAM algorithm on two types of data:
- Model Ship Experiments: Magnetic field measurements collected onboard a model ship in an indoor pool. Ground truth position and orientation were obtained using a motion capture system.
- Foot-Mounted Sensor Data: An open-source dataset from a pedestrian wearing an IMU, which includes accelerometer, gyroscope, and magnetic field measurements. This dataset was originally used for ZUPT-aided EKF localization.
Key Insights and Ideas
- Analytical Gradients are Key: The ability to derive analytical expressions for the gradient of the magnetic field map is crucial. This allows the EKF to efficiently relate changes in magnetic field measurements to changes in position.
- EKF Efficiency: By moving from a particle filter to an EKF, the computational complexity per time step is drastically reduced from $O(N_p N_m)$ to $O(N_m)$, where $N_p$ is the number of particles and $N_m$ is the number of basis functions. This makes real-time implementation on resource-constrained platforms more feasible.
- Comparable Performance: Despite the significant computational savings, the EKF approach demonstrates drift-compensating abilities comparable to established RBPF methods.
- Linearization Assumption: The success of the EKF hinges on the assumption that the magnetic field is locally linear around the estimated position. This implies that the algorithm performs best when the uncertainty in the position estimate is small relative to the spatial variations (length scale) of the magnetic field.
Key Results
- Computational Efficiency: The proposed EKF algorithm has a computational complexity of $O(N_m)$ per time step, a significant improvement over the $O(N_p N_m)$ complexity of RBPF-based methods.
- Drift Compensation: Experiments on both the model ship and foot-mounted sensor data show that the EKF-SLAM algorithm effectively compensates for odometry drift, producing trajectories that are significantly more accurate than raw odometry.
- Performance Comparison: In simulations, the EKF performs comparably to particle filters with a large number of particles (e.g., 500), especially when the predictive position error is within a certain bound relative to the magnetic field's length scale.
- Model Ship Experiments: The algorithm demonstrated visible compensation for odometry drift, with RMSE values comparable to or better than RBPF methods, while being orders of magnitude faster.
- Foot-Mounted Sensor Data: The EKF also successfully compensated for drift in pedestrian tracking data, outperforming the raw odometry.
Limitations and Future Work
- Linearization Assumption: The paper acknowledges that the EKF's performance is dependent on the linearization assumption. When the predictive position uncertainty becomes large compared to the magnetic field's length scale, the algorithm's ability to compensate for drift diminishes.
- Hyperparameter Tuning: For real-time applications, hyperparameters for the GP model need to be selected a priori, as they cannot be optimized on-the-fly without sufficient data.
- Future Directions:
- Incorporating other odometry sources (e.g., visual odometry from cameras) for real-life applications.
- Exploring iterated EKF or other non-linear estimation methods to handle larger non-linearities.
- Further reducing computational requirements for reduced-rank GP regression.
Summary updated