TLDR
Active navigation using map and positional belief encoded uncertainty.
This paper presents an uncertainty-aware magnetic navigation framework (InfoMagNav) that directly approximates information from the localization distribution to optimize trajectory selection. By minimizing information loss while ensuring goal attainment, it achieves improved accuracy and efficiency.
We explore the impact of information-driven guidance on the trajectories chosen online and their overall effect on localization performance in a variety of different scenarios.
First we explore a case study with a single simulated magnetic anomaly:
From observing the two plots above, we can see that as the information weight in the optimization is increased, the trajectories veer closer towards the anomalies. We can clearly see that accordingly to this variation, the covariance during the updates on the localization filter reduces, giving an optimized path.
From the Monte-Carlo simulations run and compared against 3 methods and 2 baselines; (i) an observability-based state-of-the-art method (DPSolver) previously applied to scalar field navigation, such as magnetic anomalies, (ii) an information-gain greedy method (Info-Gain Greedy) that is the standard for information-gain optimization in navigation, (iii) a multi-goal exploration-RRT* (E-RRT), (iv) a baseline myopic goal directed planner (MPC Goal Conditioned) that is conditioned to exploit actions that navigate it to the goal, and (v) a baseline graph-based method (Graph) that navigates to the goal.
| Algorithm | RMSE X (m) ↓ | RMSE Y (m) ↓ | MAE X (m) ↓ | MAE Y (m) ↓ | Tr. PF Cov (m2) ↓ | Tr. Stat Cov (m2) ↓ | Completion Rate (%) ↑ |
|---|---|---|---|---|---|---|---|
| Baseline Methods | |||||||
| Graph | 0.0803 | 0.0410 | 0.0600 | 0.0300 | 0.0019 | 0.0051 | 100 |
| MPC Goal Cond. | 0.0747 | 0.0419 | 0.0601 | 0.0305 | 0.0016 | 0.0055 | 100 |
| Information Driven Methods | |||||||
| Info-Gain Greedy | 0.0898 | 0.1118 | 0.0683 | 0.0826 | 0.0017 | 0.0251 | 56 |
| E-RRT | 0.0799 | 0.0849 | 0.0595 | 0.0616 | 0.0015 | 0.0137 | 94 |
| DPSolver | 0.0588 | 0.0492 | 0.0485 | 0.0367 | 0.0019 | 0.042 | 64 |
| InfoMagNav | 0.0610 | 0.0451 | 0.0498 | 0.0323 | 0.0016 | 0.040 | 98 |
We can clearly see that the DPSolver and the InfoMagNav are evenly matched, although the failure rate is significantly higher when observing the DPSolver.
Similarily if the computation efficiency of each algorithm is compared, InfoMagNav outperforms the SoTA (DPSolver) by over 99%
| Algorithm | Computation Time (ms) ↓ | ||||||
|---|---|---|---|---|---|---|---|
| Baseline Methods | |||||||
| Graph | 537.559 | ||||||
| MPC Goal Cond. | 3.4382 | ||||||
| Information Driven Methods | |||||||
| Info-Gain Greedy | 44.0905 | ||||||
| E-RRT | 96.1154 | ||||||
| DPSolver | 537.621 | ||||||
| InfoMagNav | 3.2186 | ||||||
An approach for MagNav that directly leverages information available from the map and the localization distribution would provide a more complete representation of the uncertainty and overall localization performance available.
Information in distributions is often measured through the use of Shannon Entropy:
$$ H\left(p\left(\mathbf{x}_{k}\right)\right) = -\int_{\mathbf{x}_{k}} p\left(\mathbf{x}_{k}\right) \cdot \log \left(p\left(\mathbf{x}_{k}\right)\right) d \mathbf{x}_{k} $$Here we sought to leverage this notion through the use of information gain:
$$ I\left(z_{k}, \mathbf{u}_{k} \right) = H\left( p\left(\mathbf{x}_{k-1} \right) \right) - H\left(p\left(\mathbf{x}_{k} \mid \mathbf{u}_{k}, z_{k} \right) \right) $$Which is then simply optimized using a cost function that balances exploration and exploitation:
$$ J(\mathbf{a}_{j}) = w_{h} \cdot \alpha^{EER(\mathbf{a}_{j})} + w_{d} \cdot \frac{d(\mathbf{a}_{j})}{\max{d(\mathbf{a}_{j})}} $$
Which is a pretty typical process! However the difficulty comes is estimating the entropy efficiently.
To solve this we directly use the information available through the particle filter samples to estimate the entropy efficiently:
$$ H\left( p\left(\mathbf{x}_k \mid z_k \right)\right) \approx \log \left( \sum_{i=1}^{N} p \left( z_k \mid \mathbf{x}^{[i]}_{k} \right) w^{[i]}_{k-1} \right) \\ -\sum_{i=1}^{N} \log\Bigg( p(\left(z_k \mid \mathbf{x}_k^{[i]}\right) \cdot\left(\sum_{j=1}^{N} p\left(\mathbf{x}_k^{[i]}\mid \mathbf{x}_{k-1}^{[j]}\right) w^{[j]}_{k-1} \right) w_k^{[i]} \Bigg) $$Which includes the positional, sensor and weight samples available from the particle filter. This tends to be a more refined approach when compared to the the naive methods involved in estimating the entropy from the particle distribution due to the inclusion of the non-weight parameters
Overall, InfoMagNav shows a significant reliability and efficiency leap while slightly outperforming SoTA methods.
This work is supported by the Air Force Research Laboratory under Award FA8651-23-1-0003.
This website was inspired by TILTED