# An Extended Kalman Filter for Magnetic Field SLAM Using Gaussian Process Regression

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Modeling

#### 2.1. Measurement Model

#### 2.2. Dynamic Model

## 3. Ekf for Magnetic Field Slam

Algorithm 1: EKF for magnetic field SLAM |

Input: ${\left\{\Delta {p}_{t}^{\mathrm{w}},\Delta {q}_{t}^{\mathrm{b}},{y}_{t}^{\mathrm{b}}\right\}}_{t=1}^{N}$ Output: ${\left\{{\widehat{p}}_{t|t}^{\mathrm{w}}\right\}}_{t=1}^{N}$, ${\left\{{\widehat{q}}_{t|t}^{\mathrm{w}\mathrm{b}}\right\}}_{t=1}^{N}$, ${\left\{{\widehat{m}}_{t|t}\right\}}_{t=1}^{N}$ Initialisation: ${\widehat{p}}_{0|0}^{\mathrm{w}}={0}_{3\times 1}$, ${\widehat{q}}_{0|0}^{\mathrm{w}\mathrm{b}}={q}_{0}^{\mathrm{w}\mathrm{b}}$, ${\widehat{m}}_{0|0}={0}_{({N}_{m}+3)\times 0}$, (19) 1: for $t=1$ to N do2: Dynamic update
$${\widehat{p}}_{t|t-1}^{\mathrm{w}}={\widehat{p}}_{t-1|t-1}^{\mathrm{w}}+\Delta {p}_{t}^{\mathrm{w}}$$
$${\widehat{q}}_{t|t-1}^{\mathrm{w}\mathrm{b}}={\widehat{q}}_{t-1|t-1}^{\mathrm{w}\mathrm{b}}\odot \Delta {q}_{t}^{\mathrm{b}}$$
$${\widehat{m}}_{t|t-1}={\widehat{m}}_{t-1|t-1}$$
$${P}_{t|t-1}={P}_{t-1|t-1}+Q$$
3: Measurement update
$${z}_{t}={\widehat{R}}_{t|t-1}^{\mathrm{w}\mathrm{b}}{y}_{t}^{\mathrm{b}}-\nabla {\mathrm{\Phi}}_{\mathrm{p}}\left({\widehat{p}}_{t|t-1}^{\mathrm{w}}\right){\widehat{m}}_{t|t-1}$$
$${S}_{t}={H}_{t}{P}_{t|t-1}{H}_{t}^{\top}+{\sigma}_{\mathrm{m}}^{2}{\mathcal{I}}_{3}$$
$${K}_{t}={P}_{t|t-1}{H}_{t}^{\top}{S}_{t}^{-1}$$
$${\widehat{\xi}}_{t}={K}_{t}{z}_{t}$$
$${P}_{t|t}={P}_{t|t-1}-{K}_{t}{S}_{t}{K}_{t}^{\top}$$
4: Relinearization
$${\widehat{p}}_{t|t}^{\mathrm{w}}={\widehat{p}}_{t|t-1}^{\mathrm{w}}+{\widehat{\delta}}_{t}^{\mathrm{w}}$$
$${\widehat{q}}_{t|t}^{\mathrm{w}\mathrm{b}}={exp}_{\mathrm{q}}\left({\widehat{\eta}}_{t}^{\mathrm{w}}\right)\odot {\widehat{q}}_{t|t-1}^{\mathrm{w}\mathrm{b}}$$
$${\widehat{m}}_{t|t}={\widehat{m}}_{t|t-1}+{\widehat{\nu}}_{t}$$
5: end for |

## 4. Simulations

## 5. Experimental Results

#### 5.1. Model Ship Experiments

#### 5.2. Magnetic Field Slam for Pedestrians with Foot-Mounted Sensor

## 6. Conclusions and Future Work

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Data Availability Statement

## Acknowledgments

## Conflicts of Interest

## Abbreviations

SLAM | Simultaneous localization and mapping |

EKF | Extended Kalman filtering |

RBPF | Rao-Blackwellized particle filter |

GP | Gaussian process |

IMU | Inertial measurement unit |

ZUPT | Zero-velocity upate |

RMSE | Root mean squared error |

GNSS | Global navigation satelite system |

## Appendix A. Analytical Jacobians

## References

- Storms, W.; Shockley, J.; Raquet, J. Magnetic field navigation in an indoor environment. In Proceedings of the Ubiquitous Positioning Indoor Navigation and Location Based Service, Kirkkonummi, Finland, 14–15 October 2010; pp. 1–10. [Google Scholar]
- Paull, L.; Saeedi, S.; Seto, M.; Li, H. AUV Navigation and Localization: A Review. IEEE J. Ocean. Eng.
**2014**, 39, 131–149. [Google Scholar] [CrossRef] - Dong, L.; Sun, D.; Han, G.; Li, X.; Hu, Q.; Shu, L. Velocity-Free Localization of Autonomous Driverless Vehicles in Underground Intelligent Mines. IEEE Trans. Veh. Technol.
**2020**, 69, 9292–9303. [Google Scholar] [CrossRef] - Mekik, C.; Can, O. Multipath effects in RTK GPS and a case study. J. Aeronaut. Astronaut. Aviat. Ser. A
**2010**, 42, 231–240. [Google Scholar] - Cadena, C.; Carlone, L.; Carrillo, H.; Latif, Y.; Scaramuzza, D.; Neira, J.; Reid, I.; Leonard, J.J. Past, Present, and Future of Simultaneous Localization and Mapping: Toward the Robust-Perception Age. IEEE Trans. Robot.
**2016**, 32, 1309–1332. [Google Scholar] [CrossRef] [Green Version] - Woodman, O.J. An Introduction to Inertial Navigation; Technical Report; Computer laboratory University of Cambridge: Cambridge, UK, 2007. [Google Scholar]
- Solin, A.; Kok, M.; Wahlström, N.; Schön, T.B.; Särkkä, S. Modeling and Interpolation of the Ambient Magnetic Field by Gaussian Processes. IEEE Trans. Robot.
**2018**, 34, 1112–1127. [Google Scholar] [CrossRef] [Green Version] - Kok, M.; Solin, A. Scalable Magnetic Field SLAM in 3D Using Gaussian Process Maps. In Proceedings of the 21st International Conference on Information Fusion, Cambridge, UK, 10–13 July 2018; pp. 1353–1360. [Google Scholar] [CrossRef] [Green Version]
- Yamazaki, K.; Kato, K.; Ono, K.; Saegusa, H.; Tokunaga, K.; Iida, Y.; Yamamoto, S.; Ashiho, K.; Fujiwara, K.; Takahashi, N. Analysis of magnetic disturbance due to buildings. IEEE Trans. Magn.
**2003**, 39, 3226–3228. [Google Scholar] [CrossRef] - Haverinen, J.; Kemppainen, A. A global self-localization technique utilizing local anomalies of the ambient magnetic field. In Proceedings of the IEEE International Conference on Robotics and Automation, Kobe, Japan, 12–17 May 2009; pp. 3142–3147. [Google Scholar] [CrossRef]
- Vallivaara, I.; Haverinen, J.; Kemppainen, A.; Röning, J. Simultaneous localization and mapping using ambient magnetic field. In Proceedings of the IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), Karlsruhe, Germany, 23–25 September 2010; pp. 14–19. [Google Scholar] [CrossRef]
- Kemppainen, A.; Haverinen, J.; Vallivaara, I.; Röning, J. Near-optimal SLAM exploration in Gaussian processes. In Proceedings of the IEEE Conference on Multisensor Fusion and Integration, Salt Lake City, UT, USA, 5–7 September 2010; pp. 7–13. [Google Scholar] [CrossRef]
- Vallivaara, I.; Haverinen, J.; Kemppainen, A.; Röning, J. Magnetic field-based SLAM method for solving the localization problem in mobile robot floor-cleaning task. In Proceedings of the IEEE 15th International Conference on Advanced Robotics: New Boundaries for Robotics (ICAR 2011), Tallinn, Estonia, 20–23 June 2011; pp. 198–203. [Google Scholar] [CrossRef]
- Chung, J.; Donahoe, M.; Schmandt, C.; Kim, I.J.; Razavi, P.; Wiseman, M. Indoor location sensing using geo-magnetism. In Proceedings of the 9th International Conference on Mobile Systems, Applications, and Services, Bethesda, MA, USA, 28 June–1 July 2011; pp. 141–154. [Google Scholar] [CrossRef] [Green Version]
- Le Grand, E.; Thrun, S. 3-Axis magnetic field mapping and fusion for indoor localization. In Proceedings of the IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), Hamburg, Germany, 13–15 September 2012; pp. 358–364. [Google Scholar] [CrossRef]
- Kim, S.E.; Kim, Y.; Yoon, J.; Kim, E.S. Indoor positioning system using geomagnetic anomalies for smartphones. In Proceedings of the International Conference on Indoor Positioning and Indoor Navigation (IPIN), Sydney, NSW, Australia, 13–15 November 2012; pp. 1–5. [Google Scholar]
- Frassl, M.; Angermann, M.; Lichtenstern, M.; Robertson, P.; Julian, B.J.; Doniec, M. Magnetic maps of indoor environments for precise localization of legged and non-legged locomotion. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo, Japan, 3–7 November 2013; pp. 913–920. [Google Scholar] [CrossRef] [Green Version]
- Robertson, P.; Frassl, M.; Angermann, M.; Doniec, M.; Julian, B.; Puyol, M.; Khider, M.; Lichtenstern, M.; Bruno, L. Simultaneous Localization and Mapping for Pedestrians using Distortions of the Local Magnetic Field Intensity in Large Indoor Environments. In Proceedings of the International Conference on Indoor Positioning and Indoor Navigation (IPIN 2013), Montbeliard, France, 28–31 October 2013. [Google Scholar] [CrossRef]
- Jung, J.; Lee, S.M.; Myung, H. Indoor Mobile Robot Localization and Mapping Based on Ambient Magnetic Fields and Aiding Radio Sources. IEEE Trans. Instrum. Meas.
**2015**, 64, 1922–1934. [Google Scholar] [CrossRef] - Solin, A.; Särkkä, S.; Kannala, J.; Rahtu, E. Terrain navigation in the magnetic landscape: Particle filtering for indoor positioning. In Proceedings of the European Navigation Conference (ENC), Helsinki, Finland, 30 May–2 June 2016; pp. 1–9. [Google Scholar] [CrossRef]
- Kim, H.S.; Seo, W.; Baek, K. Indoor Positioning System Using Magnetic Field Map Navigation and an Encoder System. Sensors
**2017**, 17, 651. [Google Scholar] [CrossRef] - Viset, F.; Gravdahl, J.; Kok, M. Magnetic field norm SLAM using Gaussian process regression in foot-mounted sensors. In Proceedings of the European Control Conference, Delft, The Netherlands, 29 June–2 July 2021; pp. 392–398. [Google Scholar] [CrossRef]
- Zheng, H.; Wang, H.; Wu, L.; Chai, H.; Wang, Y. Simulation research on gravity-geomagnetism combined aided underwater navigation. J. Navig.
**2013**, 66, 83–98. [Google Scholar] [CrossRef] [Green Version] - Nygren, I. Robust and efficient terrain navigation of underwater vehicles. In Proceedings of the IEEE/ION Position, Location and Navigation Symposium, Monterey, CA, USA, 5–8 May 2008; pp. 923–932. [Google Scholar] [CrossRef]
- Lager, M.; Topp, E.A.; Malec, J. Underwater Terrain Navigation Using Standard Sea Charts and Magnetic Field Maps. In Proceedings of the IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), Daegu, Korea, 16–18 November 2017; pp. 78–84. [Google Scholar] [CrossRef]
- Tal, A.; Klein, I.; Katz, R. Inertial Navigation System/Doppler Velocity Log (INS/DVL) Fusion with Partial DVL Measurements. Sensors
**2017**, 17, 415. [Google Scholar] [CrossRef] - Tyrén, C. Magnetic terrain navigation. In Proceedings of the 5th International Symposium on Unmanned Untethered Submersible Technology (UUST 1987), Durham, NH, USA, June 1987; pp. 245–256. [Google Scholar] [CrossRef]
- Wu, Z.; Hu, X.; Wu, M.; Mu, H.; Cao, J.; Zhang, K.; Tuo, Z. An experimental evaluation of autonomous underwater vehicle localization on geomagnetic map. Appl. Phys. Lett.
**2013**, 103, 104102. [Google Scholar] [CrossRef] - Wu, Y.; Shi, W. On Calibration of Three-Axis Magnetometer. IEEE Sens. J.
**2015**, 15, 6424–6431. [Google Scholar] [CrossRef] [Green Version] - Huang, Y.; Hao, Y. Method of separating dipole magnetic anomaly from geomagnetic field and application in underwater vehicle localization. In Proceedings of the IEEE International Conference on Information and Automation, Harbin, China, 20–23 June 2010; pp. 1357–1362. [Google Scholar] [CrossRef]
- Teixeira, F.C.; Quintas, J.; Pascoal, A. Robust methods of magnetic navigation of marine robotic vehicles. IFAC-PapersOnLine
**2017**, 50, 3470–3475. [Google Scholar] [CrossRef] - Mu, H.; Wu, M.; Hu, X.; Hongxu, M. Geomagnetic surface navigation using adaptive EKF. In Proceedings of the Second IEEE Conference on Industrial Electronics and Applications, Harbin, China, 23–25 May 2007; pp. 2821–2825. [Google Scholar] [CrossRef]
- Canciani, A.; Raquet, J. Airborne Magnetic Anomaly Navigation. IEEE Trans. Aerosp. Electron. Syst.
**2017**, 53, 67–80. [Google Scholar] [CrossRef] - Quintas, J.; Cruz, J.; Pascoal, A.; Teixeira, F.C. A Comparison of Nonlinear Filters for Underwater Geomagnetic Navigation. In Proceedings of the IEEE/OES Autonomous Underwater Vehicles Symposium (AUV), St. Johns, NL, Canada, 30 September–2 October 2020; pp. 1–6. [Google Scholar] [CrossRef]
- Liu, S.; Mingas, G.; Bouganis, C.S. Parallel resampling for particle filters on FPGAs. In Proceedings of the International Conference on Field-Programmable Technology (FPT), Shanghai, China, 10–12 December 2014; pp. 191–198. [Google Scholar] [CrossRef]
- Varsi, A.; Maskell, S.; Spirakis, P.G. An O(log2N) Fully-Balanced Resampling Algorithm for Particle Filters on Distributed Memory Architectures. Algorithms
**2021**, 14, 342. [Google Scholar] [CrossRef] - Solin, A.; Särkkä, S. Hilbert Space Methods for Reduced-Rank Gaussian Process Regression. Stat. Comput.
**2020**, 30, 419–446. [Google Scholar] [CrossRef] [Green Version] - Kok, M.; Schön, T.B. Magnetometer Calibration Using Inertial Sensors. IEEE Sens. J.
**2016**, 16, 5679–5689. [Google Scholar] [CrossRef] [Green Version] - Wahlström, N.; Gustafsson, F. Magnetometer Modeling and Validation for Tracking Metallic Targets. IEEE Trans. Signal Process.
**2014**, 62, 545–556. [Google Scholar] [CrossRef] [Green Version] - Skog, I. OpenShoe Matlab Framework. In OpenShoe—Foot-Mounted INS for Every Foot; KTH Royal Institute of Technology, Stockholm, Sweden and Indian Institute of Science: Bangalore, India, 2011; Available online: http://www.openshoe.org/?page_id=362 (accessed on 18 November 2021).
- Kok, M.; Hol, J.D.; Schön, T.B. Using Inertial Sensors for Position and Orientation Estimation. Found. Trends Signal Process.
**2017**, 11, 1–153. [Google Scholar] [CrossRef] [Green Version] - Wahlström, N.; Kok, M.; Schön, T.B.; Gustafsson, F. Modeling magnetic fields using Gaussian processes. In Proceedings of the IEEE International Conference on Acoustics, Speech and Signal Processing, Vancouver, BC, Canada, 26–31 May 2013; pp. 3522–3526. [Google Scholar] [CrossRef] [Green Version]
- Chen, Z. Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond. Statistics
**2003**, 182, 1–69. [Google Scholar] [CrossRef] - Gustafsson, F. Statistical Sensor Fusion; Studentlitteratur AB: Lund, Sweden, 2013. [Google Scholar]
- Arulampalam, M.; Maskell, S.; Gordon, N.; Clapp, T. A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking. IEEE Trans. Signal Process.
**2002**, 50, 174–188. [Google Scholar] [CrossRef] [Green Version] - Foxlin, E. Pedestrian tracking with shoe-mounted inertial sensors. IEEE Comput. Graph. Appl.
**2005**, 25, 38–46. [Google Scholar] [CrossRef] [PubMed] - Nilsson, J.; Skog, I.; Händel, P.; Hari, K.V.S. Foot-mounted INS for everybody—An open-source embedded implementation. In Proceedings of the IEEE/ION Position, Location and Navigation Symposium, Myrtle Beach, SC, USA, 23–26 April 2012; pp. 140–145. [Google Scholar] [CrossRef] [Green Version]
- Skoglund, M.A.; Hendeby, G.; Axehill, D. Extended Kalman filter modifications based on an optimization view point. In Proceedings of the 18th International Conference on Information Fusion (Fusion), Washington, DC, USA, 6–9 July 2015; pp. 1856–1861. [Google Scholar]

**Figure 1.**Learned magnetic field variations in test pool. The color corresponds to the estimated norm of the magnetic field map, while the opacity is inversely proportional with the variance of the estimate.

**Figure 2.**Comparison of approximations of the filtered position distribution given measurements from a simulated nonlinear field. The color indicates the norm of the simulated magnetic field. The covariance ellipsoids indicate the $68\%$ confidence interval of the EKF estimate. (

**a**) Estimates of the filtered distribution based on predictive estimates with error 0.40 m. (

**b**) Estimates of the filtered distribution based on predictive estimates with error 0.05 m.

**Figure 3.**Simulation, investigating drift-compensating abilities given varying predictive position estimation errors. Comparison of position estimation error at the end of the trajectory between Algorithm 1 and a particle filter for localization in a known map with varying predictive position errors at the initialisation of the simulation. The lines connect the average results after 100 Monte Carlo repetitions with different realisations of the odometry noise, and the error bars represent one standard deviation. (

**a**) Estimation accuracies with varying predictive position error. (

**b**) Estimation accuracies with varying length scales ${l}_{\mathrm{SE}}$.

**Figure 4.**Comparison of the model ship position trajectory estimates for a single realisation of simulated odometry noise from a birds-eye view. (

**a**) Comparing Algorithm 1 and the odometry to the ground truth. (

**b**) Comparison of the position estimates from the RBPF with 100, 200 and 500 particles respectively to the ground truth.

**Figure 5.**Measured and estimated magnetic field and position trajectories for the model ship. The upper plot marks with circles the locations where magnetic field measurements were successfully collected and matched with a ground truth position in the model ship, and the colors of the circles correspond to the norm of the measured magnetic field. The lower plot displays the trajectory estimate from applying Algorithm 1 in black. It also shows the learned magnetic field map, where the color corresponds to the norm of the estimated magnetic field $\parallel {\nabla}_{p}\mathrm{\Phi}\left(p\right){\widehat{m}}_{N|N}{\parallel}_{2}$, and the opacity is inversely proportional with the trace of the covariance matrix of the magnetic field map estimate in each location, $\mathrm{Tr}\left({\nabla}_{p}\mathrm{\Phi}\left(p\right){P}_{N|N}{\left({\nabla}_{p}\mathrm{\Phi}\left(p\right)\right)}^{\top}\right)$. (

**a**) Measured magnetic field norm in ground truth positions. (

**b**) The estimated magnetic field norm is displayed with the semi-transparent color map and the estimated trajectory is displayed with the black line.

**Figure 6.**Comparison of model ship position estimation errors from Algorithm 1, drifting odometry and the PF with 100, 200 and 500 particles respectively for a single realisation of simulated odometry noise.

**Figure 7.**Investigation of the effect of varying odometry noise on the model ship position estimate. The lines connect the average results of the ship position estimation after 100 Monte Carlo repetitions with different realisations of the simulated odometry for varying amounts of odometry noise. (

**a**) Model ship position estimation error at the end of the trajectory for varying amounts of odometry noise. (

**b**) The max norm of the predictive covariance of the estimate from Algorithm 1 depending on varying odometry noise.

**Figure 8.**Trajectory and magnetic field map estimate for the foot-mounted sensor data. The estimated trajectory obtained with Algorithm 1 is compared to odometry from the foot-mounted sensor data obtained via [40] implementation of the ZUPT-aided EKF using a foot-mounted accelerometer and gyroscope. The color of the magnetic field map corresponds to the norm of the estimated magnetic field, and the opacity is inversely proportional with the sum of the marginal variance for each of the three estimated magnetic field components. (

**a**) Learned magnetic field displayed with the semi-transparent color map and estimated trajectory displayed with the black line with odometry from foot-mounted sensor from a birds eye view. (

**b**) Trajectory estimate from Algorithm 1 compared to odometry from a birds eye view.

**Table 1.**Trajectory RMSE values in meters for the 4 collected data sets from the model ship. Values are given as averages ± one standard deviation, after 100 Monte Carlo repetitions with different realisations of the simulated odometry noise.

Trajectory RMSEs | Data Set 1 | Data Set 2 | Data Set 3 | Data Set 4 |
---|---|---|---|---|

Algorithm 1 | 0.53 ± 0.15 | 0.58 ± 0.18 | 0.53 ± 0.24 | 0.98 ± 0.62 |

RBPF with 100 particles | 0.85 ± 0.27 | 0.92 ± 0.26 | 0.95 ± 0.42 | 1.53 ± 0.53 |

RBPF with 200 particles | 0.85 ± 0.22 | 0.89 ± 0.27 | 0.98 ± 0.47 | 1.48 ± 0.46 |

RBPF with 500 particles | 0.87 ± 0.19 | 0.86 ± 0.24 | 1.00 ± 0.40 | 1.53 ± 0.50 |

Odometry | 1.98 ± 0.54 | 1.52 ± 0.48 | 1.76 ± 0.52 | 1.65 ± 0.47 |

**Table 2.**Measured time to run the estimation algorithm (in seconds) for the 4 collected data sets from the model ship. Values are given as averages ± one standard deviation, after 100 Monte Carlo repetitions with different realisations of the simulated odometry noise.

Runtimes | Data Set 1 | Data Set 2 | Data Set 3 | Data Set 4 |
---|---|---|---|---|

Algorithm 1 | 0.06 ± 0.01 | 0.05 ± 0.00 | 0.14 ± 0.01 | 0.05 ± 0.00 |

RBPF with 100 particles | 12.85 ± 0.26 | 9.24 ± 0.10 | 21.55 ± 0.32 | 9.91 ± 0.14 |

RBPF with 200 particles | 25.70 ± 0.49 | 18.44 ± 0.166 | 42.64 ± 0.27 | 19.72 ± 0.20 |

RBPF with 500 particles | 64.24 ± 0.82 | 46.02 ± 0.20 | 106.93 ± 0.34 | 49.43 ± 1.29 |

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. |

© 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Viset, F.; Helmons, R.; Kok, M.
An Extended Kalman Filter for Magnetic Field SLAM Using Gaussian Process Regression. *Sensors* **2022**, *22*, 2833.
https://doi.org/10.3390/s22082833

**AMA Style**

Viset F, Helmons R, Kok M.
An Extended Kalman Filter for Magnetic Field SLAM Using Gaussian Process Regression. *Sensors*. 2022; 22(8):2833.
https://doi.org/10.3390/s22082833

**Chicago/Turabian Style**

Viset, Frida, Rudy Helmons, and Manon Kok.
2022. "An Extended Kalman Filter for Magnetic Field SLAM Using Gaussian Process Regression" *Sensors* 22, no. 8: 2833.
https://doi.org/10.3390/s22082833