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

Abstract

We present a computationally efficient algorithm for using variations in the ambient magnetic field to compensate for position drift in integrated odometry measurements (dead-reckoning estimates) through simultaneous localization and mapping (SLAM). When the magnetic field map is represented with a reduced-rank Gaussian process (GP) using Laplace basis functions defined in a cubical domain, analytic expressions of the gradient of the learned magnetic field become available. An existing approach for magnetic field SLAM with reduced-rank GP regression uses a Rao-Blackwellized particle filter (RBPF). For each incoming measurement, training of the magnetic field map using an RBPF has a computational complexity per time step of $O\left(N_p{N}_m^2\right)$, where $N_p$ is the number of particles, and $N_m$ is the number of basis functions used to approximate the Gaussian process. Contrary to the existing particle filter-based approach, we propose applying an extended Kalman filter based on the gradients of our learned magnetic field map for simultaneous localization and mapping. Our proposed algorithm only requires training a single map. It, therefore, has a computational complexity at each time step of $O\left(N_m^2\right)$. We demonstrate the workings of the extended Kalman filter for magnetic field SLAM on an open-source data set from a foot-mounted sensor and magnetic field measurements collected onboard a model ship in an indoor pool. We observe that the drift compensating abilities of our algorithm are comparable to what has previously been demonstrated for magnetic field SLAM with an RBPF.

1. Introduction

Autonomous navigation using only onboard sensors is a desirable technology for various applications. There is no stable access to GNSS signals indoors, underground, or underwater, as they are blocked by building elements, earth and water, respectively [1,2,3]. In other environments, the use of GNSS signals for localization can also be challenging. Surface vehicles in harbours can be close to containers, bridges, or other industrial elements that can cause multi-path effects on the GNSS (Global navigation satellite system) measurements [4]. Autonomous navigation using only onboard sensors is challenging because of the drift in the position estimate obtained from sensor measurements that are independent of pre-deployed infrastructure [5]. Drift occurs when noisy measurements from, for example, gyroscopes, accelerometers, Doppler velocity logs or wheel encoders are integrated to estimate position without any absolute position measurements [6]. We will refer to the position estimates and orientation estimates obtained when integrating such noisy measurements as odometry. A range of other possible sensor readings may be available in these scenarios. The scope of our research is limited to the investigation of autonomous navigation using onboard odometry and magnetic field measurements.

Magnetic field simultaneous localization and mapping (SLAM) has been proposed to compensate for odometry drift when there is access to magnetic field measurements in a magnetic field with stationary spatial variations [7]. It has been demonstrated for indoor localization that magnetic field SLAM can be used to improve position estimates [8]. In environments with ferromagnetic structures, as for example in indoor environments, the magnetic field has rich spatial variation due to the magnetization of the metal [9]. Navigation using nonlinear variations in the ambient magnetic field has been proposed for a range of applications, such as indoor localization [1,8,10,11,12,13,14,15,16,17,18,19,20,21,22], underwater localization [23,24,25,26,27,28,29,30,31], and surface and aerial navigation [32,33]. Although [32] uses an extended Kalman filter (EKF) for localization in a learned magnetic field, the majority use a particle filter [1,10,11,12,13,15,16,17,18,22,25,27]. A comparative study has demonstrated that the particle filter is more accurate for underwater geomagnetic navigation in the case where the initial position is not known, while the EKF is more computationally efficient [34].

Computationally tractable magnetic field SLAM in three dimensions was proposed in [8], using a Rao-Blackwellized particle filter (RBPF) to simultaneously estimate the position and orientation of a pedestrian as well as the ambient magnetic field. A set of $N_p$ particles are used to represent the position and orientation [8]. The RBPF for magnetic field SLAM proposed by [8] uses Gaussian process (GP) regression to combine knowledge about the nature of the magnetic field from Maxwell’s equations with measurements of the magnetic field to create a magnetic field map. To this end, they build the magnetic field map for each particle using reduced-rank Gaussian process regression, which represents the magnetic field map as a linear combination of $N_m$ Laplace basis functions on hexagonal domains, and which represents the magnetic field map uncertainty as a matrix with $N_m^2$ entries. As each magnetic field map is represented with the weights of $N_m$ basis functions and the corresponding covariance of these weights, all of which require updating at each time step, the computational cost of updating the magnetic field map is $O\left(N_p{N}_m^2\right)$ [8]. In the case where the particle filter is run on a parallelized architecture, such as FPGAs, the computation time dependence on the number of particles can be reduced dramatically [35,36]. The scope of our research is limited to improving the speed of Magnetic field SLAM on non-parallelized architectures. Magnetic field SLAM has also been demonstrated feasible using an RBPF with Laplace basis functions defined on a single, cubic domain [22].

The contribution of this paper is an approach to magnetic field SLAM that is faster and requires less storage compared to the approach proposed in [8], inspired by the goal to run magnetic field SLAM in real-time on cheap carry-on units with low processing power. A property of using reduced-rank Gaussian process regression for magnetic field SLAM in a cubic domain is that the magnetic field map is given as a linear combination of analytically described basis functions [37]. We can therefore use the spatial derivatives of the closed-form solutions of the Laplacian to find the Jacobian of the magnetic field map with respect to the position estimate. To reduce computational expenses, we propose utilising the availability of analytical Jacobians of the reduced-rank Gaussian process magnetic field maps to perform magnetic field SLAM using an extended Kalman filter. This only requires building and updating a single copy of the magnetic field map at each time instance. Figure 1 shows the learned magnetic field map and estimated trajectory from our EKF algorithm for magnetic field SLAM, tested on magnetic field measurements collected onboard a model ship. The resulting computational cost is $O\left(N_m^2\right)$ at each time step, instead of $O\left(N_m^2{N}_p\right)$. The use of the EKF is possible if the dynamic model and measurement model are close to linear [38]. In the case of simultaneous localization and mapping, the world frame coordinate system is defined relative to the initial body frame coordinate system [8]. As there is no uncertainty in the initial position estimate due to this definition [8], the position estimate initially has zero covariance. In cases where the growth of the uncertainty of the pose estimate that comes from odometry drift is limited by frequent enough visitations of previous areas, magnetic field SLAM remedies drift in the position estimate, which means that the position estimation error no longer grows without bounds, but stays limited [8]. When the estimated position is close to the actual position, the magnetic field linearized about the estimated position provides a good local approximation to the magnetic field itself, as the magnetic field even in environments with strong stationary disturbances can be assumed to have limited spatial variability [8,11,20,39]. A key assumption for several implementations of estimating the magnetic field with GP regression is that the magnetic field in locations close to each other have a higher correlation than the magnetic field in locations that are further away [8,11,20,39]. In these implementations, how rapidly the correlation diminishes with increasing distance is encoded in a hyperparameter in the GP prior describing the length scale of the spatial variations in the magnetic field [39].

We illustrate with simulations that we can expect the EKF for a localization task to give accurate estimates when the max norm of the covariance from the predictive distribution is small relative to the length scale of the magnetic field anomalies. A requirement for using GP regression to represent the magnetic field map in magnetic field SLAM is the prior knowledge of hyperparameters describing the expected distribution of the magnetic field potential [8]. These hyperparameters contain information about the expected length scale of the magnetic field spatial variations [39]. Without adding any further assumptions to the magnetic field SLAM formulation presented in [8], we can therefore assume to have information available about how rapidly we can expect our learned magnetic field to vary spatially. In SLAM, the uncertainty of the position estimate will grow when the sensor moves through unexplored areas, as there is no map information available to correct the estimated pose [8]. When the sensor re-enters an area where it has already built a map of the local anomalies, our proposed algorithm can be expected to converge when the covariance of the position estimate is small compared to the length scale of the learned magnetic field map. We show with magnetic field data collected from a model ship in an indoor pool (see Figure 1) and simulated odometry that our proposed algorithm converges when the odometry noise is limited, for a trajectory when the time until the first revisitation of a mapped area is constant. We also show with magnetic field measurements and odometry obtained from an open-source implementation by [40] that our algorithm can compensate for drift in position estimates based on accelerometer and gyroscope measurements in a foot-mounted sensor.

The remainder of this paper is structured as follows. In Section 2, we define the model for our magnetic field SLAM estimation problem. In Section 3, we derive an EKF for magnetic field SLAM. In Section 4 we show the convergence properties of our algorithms in a simulated navigation task, where we can control the ratio of our position estimate uncertainty over the length scale of the magnetic field variations. In Section 5, we demonstrate the drift-compensating abilities of the EKF-SLAM algorithm on a set of data we collected with a model ship and on an open-source data-set from a foot-mounted sensor. Finally, in Section 6 we summarise our findings and discuss possible directions for future work. Our Matlab-implementation producing all results found in this paper can be found on https://github.com/fridaviset/EKFMagSLAM.

2. Modeling

Our simultaneous localization and mapping algorithm estimates the filtering distribution

$$p\left(x_t\right|y_{1:t}^{\mathrm{b}},\Delta p_{1:t}^{\mathrm{w}},\Delta q_{1:t}^{\mathrm{b}}),$$

(1)

where we denote the available magnetic field measurements by $y_{1:t}^{\mathrm{b}}$, the odometry describing the change in position and orientation $\Delta p_{1:t}^{\mathrm{w}},\Delta q_{1:t}^{\mathrm{b}}$, and the state we wish to estimate at each time step t as $x_t$. For a vector $x_t$, the set $\{x_1,\cdots ,x_t\}$ is denoted $x_{1:t}$ for brevity. We use the superscript b to denote the sensor’s body-frame, which is aligned with its sensor axes. The superscript w refers to the world frame, which is defined as the inertial frame that shares its origin with the body frame at time zero. The gravity field in this position is aligned with the negative z-axis, and where the initial yaw-angle between the body and world-frame at $t=0$ is zero. As we wish to estimate both position, orientation and the magnetic field map, we model our state as

$$x_t={\left[{\left(p_t^{\mathrm{w}}\right)}^{\top }{\left(q_t^{\mathrm{w}\mathrm{b}}\right)}^{\top }{m}^{\top }\right]}^{\top },$$

(2)

where $p_t^{\mathrm{w}}$ denotes the position, $q_t^{\mathrm{w}\mathrm{b}}$ denotes the orientation transformation from the world frame to the body frame, and m is a vector that describes our magnetic field map represented with reduced-rank GP regression.

2.1. Measurement Model

We consider the case where we have access to measurements of the magnetic field in a sensor attached to the object we aim to localize. The measurement equation is given by

$$y_t^{\mathrm{b}}=R_t^{\mathrm{b}\mathrm{w}}{\nabla }_{\mathrm{p}}\phi \left(p_t^{\mathrm{w}}\right)+e_{\mathrm{m},t}^{\mathrm{b}},e_{\mathrm{m},t}^{\mathrm{b}}\sim \mathcal{N}(0,{\sigma }_{\mathrm{m}}^2{\mathcal{I}}_3),$$

(3)

where $y_t^{\mathrm{b}}$ denotes the magnetic field measurement, $e_{\mathrm{m},t}^{\mathrm{b}}$ denotes the measurement noise and $R_t^{\mathrm{b}\mathrm{w}}$ denotes the rotation from world to body frame, corresponding to the conjugate of the quaternion $q_t^{\mathrm{w}\mathrm{b}}$, expressed as a rotation matrix. See [41] for definitions of the quaternion conjugate, and definitions of transformation from a quaternion to a rotation matrix. The function ${\nabla }_{\mathrm{p}}\phi \left(p_t^{\mathrm{w}}\right)$ is our model of the magnetic field. We model the magnetic field as in [42] as the gradient of the function $\phi \left(p_t^{\mathrm{w}}\right)$ with respect to the position $p_t^{\mathrm{w}}$, where $\phi :{\mathbb{R}}^3\to \mathbb{R}$ is a scalar potential distributed as a GP with prior

$$\phi \sim \mathcal{N}(0,{\kappa }_{\mathrm{SE}}(·,·)+{\kappa }_{\mathrm{lin}}(·,·)),$$

(4)

and where the kernel is defined by the functions

$$\begin{array}{cc}\hfill {\kappa }_{\mathrm{SE}}(p,p^{\prime })& ={\sigma }_{\mathrm{SE}}^{2}exp\left(-\frac{\parallel p-p^{\prime }{\parallel }_2^2}{2l_{\mathrm{SE}}^2}\right),\hfill \end{array}$$

(5a)

$$\begin{array}{cc}\hfill {\kappa }_{\mathrm{lin}}(p,p^{\prime })& ={\sigma }_{\mathrm{lin}}^2{p}^{\top }{p}^{\prime },\hfill \end{array}$$

(5b)

with ${\sigma }_{\mathrm{SE}}$, ${\sigma }_{\mathrm{lin}}$, $l_{\mathrm{SE}}$ and ${\sigma }_{\mathrm{m}}$ being hyperparameters. The hyperparameter $l_{\mathrm{SE}}$ refers to the length scale of the spatial variations in the magnetic field potential that is represented by the kernel [42]. The parameters ${\sigma }_{\mathrm{SE}}$, ${\sigma }_{\mathrm{lin}}$ and ${\sigma }_{\mathrm{m}}$ define the presence of the nonlinear components, linear components and measurement noise in the magnetic field respectively [42]. The linear component modelled by the kernel component ${\kappa }_{\mathrm{lin}}$ represents of the constant underlying earth magnetic field, while the nonlinear disturbances caused by the modelled by the nearby ferromagnetic structures is modelled by the kernel component ${\kappa }_{\mathrm{SE}}$. Modelling the magnetic field as the gradient of a scalar potential ensures that Maxwell’s equations are satisfied, under the assumption that no current passes through the domain where we construct our magnetic field map [42]. We use a reduced-rank approximation to the GP similar to the one used in [8] for mapping the indoor magnetic field for localization purposes with the same kernel. Our approach is an application of the GP approximation presented in [37], which is based on conditioning the GP prediction on a set of basis functions corresponding to a subset of the eigenbasis of the negative Laplace operator in a finite domain, subject to Dirichlet boundary conditions [37]. The reduced-rank approximation models the magnetic field potential as a sum of basis functions defined as the solutions to the Laplace equations over a finite domain $\mathrm{\Omega }\subset {\mathbb{R}}^3$

$$\left\{\begin{array}{cc}-{\nabla }_{\mathrm{p}}^2{\varphi }_i\left(p\right)={\lambda }_i{\varphi }_i\left(p\right),& p\in \mathrm{\Omega },\\ {\varphi }_i\left(p\right)=0,& p\in \delta \mathrm{\Omega },\end{array}\right.$$

(6)

where ${\varphi }_i$ is the i’th eigenfunction, and ${\lambda }_i$ is the i’th eigenvalue [37]. We approximate the GP with the first $N_m$ basis functions solving the Laplace equations defined over a cubical domain $\mathrm{\Omega }=[L_{l,1},L_{u,1}]\times [L_{l,2},L_{u,2}]\times [L_{l,3},L_{u,3}]$. In this case, using the $N_m$ first eigenfunctions to represent the potential gives the approximation

$$\phi \left(p\right)\approx \mathrm{\Phi }\left(p\right)m,$$

(7)

with $\mathrm{\Phi }\left(p\right)$ being the matrix

$$\mathrm{\Phi }\left(p\right)=\left[\begin{array}{cccc}{p}^{\top }& {\varphi }_1\left(p\right)& \cdots & {\varphi }_{N_m}\left(p\right)\end{array}\right],$$

(8)

where ${\varphi }_i$ is the i’th eigenfunction of the Laplace basis, and $m\in {\mathbb{R}}^{N_m+3}$ is a vector determining the contribution of each linear components as well as each basis function to the potential. Each eigenfunction ${\varphi }_i\left(p\right)$ has a closed form expression given by

$$\begin{array}{c}\hfill {\varphi }_i\left(p\right)=\prod _{d=1}^3\frac{\sqrt{2}}{\sqrt{L_{\mathrm{u},d}-L_{\mathrm{l},d}}}sin\left(\frac{\pi n_{i,d}(p_d+L_{\mathrm{l},d})}{L_{\mathrm{u},d}-L_{\mathrm{l},d}}\right),\end{array}$$

(9)

where the set $(n_{i,1},n_{i,2},n_{i,3})$ is the set of three natural numbers that is different from the sets $(n_{j,1},n_{j,2},n_{j,3})$ defined for all $j<i$, that gives the corresponding eigenvalue

$$\begin{array}{c}\hfill {\lambda }_i=\sum _{d=1}^D{\left(\frac{\pi n_{i,d}}{L_{\mathrm{u},d}-L_{\mathrm{l},d}}\right)}^2,\end{array}$$

(10)

as large as possible. The basis functions in (9) and eigenvalues in (10) are identical to those used in [22]. The vector m has a prior distribution given by

$$m\sim \mathcal{N}(0,\mathrm{\Lambda }),$$

(11)

where $\mathrm{\Lambda }$ is defined as

$$\begin{array}{cc}\hfill & \mathrm{\Lambda }=\mathrm{diag}\left[{\sigma }_{\mathrm{lin}}^2{\mathcal{I}}_3,S_{\mathrm{SE}}\left(\sqrt{{\lambda }_1}\right),\cdots ,S_{\mathrm{SE}}\left(\sqrt{{\lambda }_{N_m}}\right)\right],\hfill \end{array}$$

(12)

with $S_{\mathrm{SE}}(·)$ being the spectral density of the squared exponential kernel, as defined in [7]. This corresponds to the magnetic field potential $\phi \left(p\right)\approx \mathrm{\Phi }\left(p\right)m$ having a prior distribution given by (4) as $N_m$ goes to infinity, and the size of the domain goes to infinity [37]. Inserting this approximation to the magnetic field model gives the measurement model

$$y_t^{\mathrm{b}}\approx R_t^{\mathrm{b}\mathrm{w}}{\nabla }_{\mathrm{p}}\mathrm{\Phi }\left(p_t^{\mathrm{w}}\right)m+e_{\mathrm{m},t}^{\mathrm{b}}{e}_{\mathrm{m},t}^{\mathrm{b}}\sim \mathcal{N}(0,{\sigma }_{\mathrm{m}}^2{\mathcal{I}}_3),$$

(13)

with the closed form expressions for ${\nabla }_{\mathrm{p}}\mathrm{\Phi }\left(p_t^{\mathrm{w}}\right)$ given in Appendix A. This measurement model is identical to the measurement model used in [8], with the exception of the basis functions $\mathrm{\Phi }\left(p_t^{\mathrm{w}}\right)$, which are different as they are defined with respect to different domains.

2.2. Dynamic Model

We assume access to noisy odometry measurements $\Delta p_t^{\mathrm{w}}$ and $\Delta q_t^{\mathrm{b}}$ of the change in position and orientation at each time step. We model the change in position and orientation according to the dynamic model

$$\begin{array}{cc}{p}_{t+1}^{\mathrm{w}}=p_t^{\mathrm{w}}+\Delta p_t^{\mathrm{w}}+e_{\mathrm{p},t}^{\mathrm{w}},\hfill & \hfill e_{\mathrm{p},t}^{\mathrm{w}}\sim \mathcal{N}(0,{\sigma }_{\mathrm{p}}^2{\mathcal{I}}_3),\end{array}$$

(14a)

$$\begin{array}{ccc}{q}_{t+1}^{\mathrm{w}\mathrm{b}}\hfill & =q_t^{\mathrm{w}\mathrm{b}}\odot \Delta q_t^{\mathrm{b}}\odot {exp}_{\mathrm{q}}\left(e_{\mathrm{q},t}^{\mathrm{b}}\right),\hfill & \hfill e_{\mathrm{q},t}^{\mathrm{b}}\sim \mathcal{N}(0,{\sigma }_{\mathrm{q}}^2{\mathcal{I}}_3),\end{array}$$

(14b)

where $e_{\mathrm{p},t}^{\mathrm{w}}$ and $e_{\mathrm{q},t}^{\mathrm{b}}$ denote the position and orientation odometry measurement noise respectively, ⊙ is the quaternion product and ${exp}_{\mathrm{q}}$ is the operator that maps an axis-angle orientation deviation to a quaternion (see [41] for details on quaternion algebra).

3. Ekf for Magnetic Field Slam

We estimate our state with an EKF applied to the dynamic model in (14a,b) and the measurement model defined in (13), with predictive and filtered estimates denoted ${\widehat{p}}_{t|t-1}^{\mathrm{w}}$, ${\widehat{m}}_{t|t-1}$, ${\widehat{q}}_{t|t-1}^{\mathrm{w}\mathrm{b}}$ and ${\widehat{p}}_{t|t}^{\mathrm{w}}$, ${\widehat{m}}_{t|t}$, ${\widehat{q}}_{t|t}^{\mathrm{w}\mathrm{b}}$ respectively. We initialise the magnetic field state estimate as ${\widehat{m}}_{0|0}=0_{N_m\times 1}$ according to the reduced-rank GP prior in (11). We initialise the orientation estimate according to the initial rotation ${\widehat{q}}_{0|0}^{\mathrm{w}\mathrm{b}}=q_0^{\mathrm{w}\mathrm{b}}$ between the world and body frame as defined in Section 2. We initialise the position estimate as ${\widehat{p}}_{0|0}^{\mathrm{w}}=0_{3\times 1}$, also according to our definition of the world frame relative to the initial body frame from Section 2.

We represent the deviation between the true and estimated predictive state by an error state ${\xi }_t$ defined as

$$\begin{array}{cc}\hfill {\xi }_t& ={\left[{\left({\delta }_t^{\mathrm{w}}\right)}^{\top }{\left({\eta }_t^{\mathrm{w}}\right)}^{\top }{\nu }_t^{\top }\right]}^{\top },\hfill \end{array}$$

(15)

where ${\delta }_t^{\mathrm{w}}=p_t^{\mathrm{w}}-{\widehat{p}}_{t|t-1}^{\mathrm{w}}$ denotes the position estimation error, ${\nu }_t=m-{\widehat{m}}_{t|t-1}$ denotes the magnetic field state estimation error, and where ${\eta }_t^{\mathrm{w}}$ denotes the orientation estimation error parametrised as an axis-angle deviation according to

$$\begin{array}{cc}\hfill q_t^{\mathrm{w}\mathrm{b}}=& {exp}_{\mathrm{q}}\left({\eta }_t^{\mathrm{w}}\right)\odot {\widehat{q}}_{t|t-1}^{\mathrm{w}\mathrm{b}}.\hfill \end{array}$$

(16)

Similarly, we represent the deviation between the true and estimated filtered state by an error state ${\tilde{\xi }}_t$ defined as

$$\begin{array}{cc}\hfill {\tilde{\xi }}_t& ={\left[{\left({\tilde{\delta }}_t^{\mathrm{w}}\right)}^{\top }{\left({\tilde{\eta }}_t^{\mathrm{w}}\right)}^{\top }{\tilde{\nu }}_t^{\top }\right]}^{\top },\hfill \end{array}$$

(17)

where ${\tilde{\delta }}_t^{\mathrm{w}}=p_t^{\mathrm{w}}-{\widehat{p}}_{t|t}^{\mathrm{w}}$, ${\tilde{\nu }}_t=m-{\widehat{m}}_{t|t}$, and where ${\tilde{\eta }}_t^{\mathrm{w}}$ denotes the filtered orientation estimation error according to

$$\begin{array}{cc}\hfill q_t^{\mathrm{w}\mathrm{b}}=& {exp}_{\mathrm{q}}\left({\tilde{\eta }}_t^{\mathrm{w}}\right)\odot {\widehat{q}}_{t|t}^{\mathrm{w}\mathrm{b}}.\hfill \end{array}$$

(18)

Since we build our map relative to our initial position and orientation, the covariance of our initial position and orientation estimates is zero. The covariance of the initial magnetic field estimate is defined in (11) as the magnetic field map prior $\mathrm{\Lambda }$. Hence, our initial error state ${\xi }_0$ has a covariance

$$\begin{array}{c}\hfill P_{0|0}=\left[\begin{array}{cc}{0}_{6\times 6}& 0_{6\times (N_m+3)}\\ 0_{(N_m+3)\times 6}& \mathrm{\Lambda }\end{array}\right].\end{array}$$

(19)

To perform the dynamic update, we propagate our filtered state estimate through the nonlinear dynamic model (14a,b), giving the predictive updates as described in (26a,b). As the magnetic field is assumed stationary, its estimate is unchanged by the dynamic update defined in (26c). We derive the covariance update in the EKF by linearising about the filtered state estimate from the previous time step, with respect to the error state ${\tilde{\xi }}_t$. Inserting (15), (17) and (26a,b) into the dynamic model (14a,b) gives

$$\begin{array}{cc}\hfill {\widehat{p}}_{t|t-1}^{\mathrm{w}}+{\delta }_t^{\mathrm{w}}& ={\widehat{p}}_{t-1|t-1}^{\mathrm{w}}+\Delta p_t^{\mathrm{w}}+{\tilde{\delta }}_{t-1}^{\mathrm{w}}+e_{\mathrm{p},t}^{\mathrm{w}}\hfill \end{array}$$

(20a)

$$\begin{array}{cc}\hfill {exp}_{\mathrm{q}}\left({\eta }_t^{\mathrm{w}}\right)& ={exp}_{\mathrm{q}}\left({\tilde{\eta }}_{t-1}^{\mathrm{w}}\right)\odot {\widehat{q}}_{t|t-1}^{\mathrm{w}\mathrm{b}}\odot {exp}_{\mathrm{q}}\left(e_{\mathrm{q},t}^{\mathrm{b}}\right)\odot {\left({\widehat{q}}_{t|t-1}^{\mathrm{w}\mathrm{b}}\right)}^C,\hfill \end{array}$$

(20b)

where ${\left({\widehat{q}}_{t|t-1}^{\mathrm{w}\mathrm{b}}\right)}^C$ denotes the conjugate of the quaternion ${\widehat{q}}_{t|t-1}^{\mathrm{w}\mathrm{b}}$. The linearization of the dynamic model with respect to the error states gives the following propagation of the error states

$$\begin{array}{cc}\hfill {\delta }_t^{\mathrm{w}}& ={\tilde{\delta }}_{t-1}^{\mathrm{w}}+e_{\mathrm{p},t}^{\mathrm{w}},\hfill \end{array}$$

(21a)

$$\begin{array}{cc}\hfill {\eta }_t^{\mathrm{w}}& \approx {\tilde{\eta }}_{t-1}^{\mathrm{w}}+{\widehat{R}}_{t|t-1}^{\mathrm{w}\mathrm{b}}{e}_{\mathrm{q},t}^{\mathrm{b}},\hfill \end{array}$$

(21b)

$$\begin{array}{cc}\hfill {\nu }_t& ={\tilde{\nu }}_{t-1},\hfill \end{array}$$

(21c)

where ${\widehat{R}}_{t|t-1}^{\mathrm{w}\mathrm{b}}$ denotes the rotation matrix corresponding to the rotation represented by the quaternion ${\widehat{q}}_{t|t-1}^{\mathrm{w}\mathrm{b}}$. This linearization is exact for the position, and it is a good approximation for the orientation error state in the cases where the orientation error is small [41]. Equation (21a,b) can equivalently be written as

$${\xi }_t\approx {\tilde{\xi }}_{t-1}+e_{\mathrm{dyn},t},e_{\mathrm{dyn},t}\sim \mathcal{N}(0_{(N_m+9)\times 1},Q),$$

(22)

where

$$Q=\left[\begin{array}{ccc}{\sigma }_{\mathrm{p}}^2{\mathcal{I}}_3& 0_{3\times 3}& 0_{3\times (N_m+3)}\\ 0_{3\times 3}& {\sigma }_{\mathrm{q}}^2{\mathcal{I}}_3& 0_{3\times (N_m+3)}\\ 0_{(N_m+3)\times 3}& 0_{(N_m+3)\times 3}& 0_{(N_m+3)\times (N_m+3)}\end{array}\right].$$

(23)

As the linearization of the error state propagation is given in (22), the covariance $P_{t|t-1}$ of the predictive state error ${\xi }_t$ is given by (26d).

For the measurement update, we apply an EKF measurement update to the measurement model in (13). We linearise about the predictive state estimate, with respect to the error state ${\xi }_t$. The linearized measurement model is given by

$$y_t^b={\widehat{R}}_{t|t-1}^{\mathrm{b}\mathrm{w}}{\nabla }_{\mathrm{p}}\mathrm{\Phi }\left({\widehat{p}}_{t|t-1}^{\mathrm{w}}\right){\widehat{m}}_t+H_t{\xi }_t+e_{\mathrm{m},t}^{\mathrm{b}},e_{\mathrm{m},t}^{\mathrm{b}}\sim \mathcal{N}(0_{3\times 1},{\sigma }_{\mathrm{m}}^2{\mathcal{I}}_3),$$

(24)

where

$$H_t={\left[\begin{array}{c}{\left({\nabla }_{\mathrm{pp}}\mathrm{\Phi }\left({\widehat{p}}_{t|t-1}^{\mathrm{w}}\right){\widehat{m}}_t\right)}^{\top }\\ {\left[\left({\nabla }_{\mathrm{p}}\mathrm{\Phi }\left({\widehat{p}}_{t|t-1}^{\mathrm{w}}\right){\widehat{m}}_t\right)\times \right]}^{\top }\\ \left({\nabla }_{\mathrm{p}}\mathrm{\Phi }{\left({\widehat{p}}_{t|t-1}^{\mathrm{w}}\right)}^{\top }\right)\end{array}\right]}^{\top }$$

(25)

with $\left[v\times \right]$ being the scew-symmetric matrix representing the cross product $v\times u$ between two vectors $v,u\in {\mathbb{R}}^3$ as a matrix multiplication $\left[v\times \right]u$ (explicit expression for $\left[v\times \right]$ is given in [41]), and ${\nabla }_{\mathrm{pp}}\mathrm{\Phi }(·)$ being the Jacobian of the basis functions, given in Appendix A. Note that this Jacobian is a matrix with $3\times 3\times (N_m+3)$ entries, and multiplying it with the $(N_m+3)$-dimensional state vector m therefore gives a $3\times 3$ matrix. Applying the Kalman filter measurement update to this linearized measurement function gives the EKF measurement update in (27a–e). This gives an estimate ${\widehat{\xi }}_t$ of the predictive state error ${\xi }_t$, with a corresponding covariance. By concatenating the estimated predictive state error to the predictive state, we obtain the filtered state estimates as defined in (28a–c). The covariance of the filtered error state ${\tilde{\xi }}_t$ then becomes the same as the covariance of the estimated predictive error state ${\widehat{\xi }}_t$. Recursively applying the dynamic update, measurement update and re-linearization step results in Algorithm 1.

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 do 2:    Dynamic update $${\widehat{p}}_{t|t-1}^{\mathrm{w}}={\widehat{p}}_{t-1|t-1}^{\mathrm{w}}+\Delta p_t^{\mathrm{w}}$$ (26a) $${\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}}$$ (26b) $${\widehat{m}}_{t|t-1}={\widehat{m}}_{t-1|t-1}$$ (26c) $$P_{t|t-1}=P_{t-1|t-1}+Q$$ (26d) 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}$$ (27a) $$S_t=H_t{P}_{t|t-1}{H}_t^{\top }+{\sigma }_{\mathrm{m}}^2{\mathcal{I}}_3$$ (27b) $$K_t=P_{t|t-1}{H}_t^{\top }{S}_t^{-1}$$ (27c) $${\widehat{\xi }}_t=K_t{z}_t$$ (27d) $$P_{t|t}=P_{t|t-1}-K_t{S}_t{K}_t^{\top }$$ (27e) 4:    Relinearization $${\widehat{p}}_{t|t}^{\mathrm{w}}={\widehat{p}}_{t|t-1}^{\mathrm{w}}+{\widehat{\delta }}_t^{\mathrm{w}}$$ (28a) $${\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}}$$ (28b) $${\widehat{m}}_{t|t}={\widehat{m}}_{t|t-1}+{\widehat{\nu }}_t$$ (28c) 5: end for

4. Simulations

We study when Algorithm 1 for localization in a previously learned magnetic field gives a converging pose estimate in a known nonlinear field depending on the position uncertainty at time t. Since we assume the magnetic field map is know, we can replace ${\widehat{m}}_{t|t}$ with the known $m_{\mathrm{sim}}$ everywhere in our algorithm. As a consequence of this, we can alo skip (26c) and (28c), use $P_{0|0}=0_{6\times 6}$ and

$$H_t={\left[\begin{array}{c}{\left({\nabla }_{\mathrm{pp}}\mathrm{\Phi }\left({\widehat{p}}_{t|t-1}^{\mathrm{w}}\right)m\right)}^{\top }\\ {\left[\left({\nabla }_{\mathrm{p}}\mathrm{\Phi }\left({\widehat{p}}_{t|t-1}^{\mathrm{w}}\right)m\right)\times \right]}^{\top }\end{array}\right]}^{\top }.$$

(29)

We start the simulation at time t with a varying predictive position estimation error, and set the standard deviation of our position uncertainty equal to the distance between the actual and estimated position at the beginning of the simulation. This artificially introduces a predictive position estimation error representing the estimation error that can accumulate over time in magnetic field SLAM. Position errors can, for example, accumulate when the sensor is moved for a long time through areas with no information about the magnetic field available from previous measurements [8].

We simulate positions $p_t$ along a square trajectory moving with constant velocity for four laps, and simulate the odometry by adding sampled realisations of the white noises $e_{\mathrm{p},t}^{\mathrm{w}}$ and $e_{\mathrm{q},t}^{\mathrm{b}}$ to (14a,b). We simulate a nonlinear field by drawing a sample from the reduced-rank GP prior $m_{\mathrm{sim}}\sim \mathcal{N}(0,\mathrm{\Lambda })$, with ${\sigma }_{\mathrm{lin}}=0$, ${\sigma }_{\mathrm{SE}}=0.1$ and $l_{\mathrm{SE}}=0.2$. We used 50 basis functions to represent the magnetic field map. Using a domain that is $3\mathrm{m}\times 3\mathrm{m}\times 1\mathrm{m}$. This number of basis functions ensures that for a GP trained with 2000 sampled measurements the root mean squared error (RMSE) between the approximate and the full GP predictions in 1000 randomly selected locations is below the measurement noise. To prevent ill effects from the boundary conditions we ensured that both the training and test data was at least 0.5 m away from the border. We then simulate magnetic field measurements by adding white noise to the gradient of the nonlinear field in the ground truth position according to (13), replacing ${\widehat{m}}_t$ with $m_{\mathrm{sim}}$, and using ${\sigma }_{\mathrm{m}}=0.03$. We use the incoming magnetic field measurements and the odometry to estimate the position and orientation using an EKF and a particle filter. The EKF is implemented according to Algorithm 1, but reducing the state-space to only contain the position and orientation, and inserting $m_{\mathrm{sim}}$ in place of ${\widehat{m}}_t$ in (27a). We implement a particle filter for navigation in a magnetic field represented by a field learned with GP regression according to Algorithm 1 in [20], with the difference that we use the simulated reduced-rank map instead of a learnt full GP map, and that we perform the prediction step using our odometry model in (14a,b).

In Figure 2, we can see two examples of PFs and an EKFs estimate of the position filtered distribution, represented with a particle cloud and a mean and an uncertainty interval, respectively. In Figure 2a, the initial uncertainty of the position estimate is so large that the particle cloud becomes multi-modal, making it impossible for the EKF to correctly approximate the true nature of the filtered distribution. The estimated position is, therefore, far away from the true position. In addition, the uncertainty estimate of the EKF does not reflect this, as it relies upon a linearization of the nonlinear magnetic field about the predictive estimate. In Figure 2b, the position estimate at time t is still wrong, but close enough that the particle cloud representation of the filtered distribution appears uni-modal, and the EKF estimate of the filtered distribution is now closer to the estimate from the particle filter. As we see in Figure 3a displaying the position estimation error at the end of the trajectory estimates across the four laps, the position estimates from the EKF are accurate across the entire trajectory if the predictive position error is lower than $0.3$ m. The particle filter on the other hand, is accurate even beyond these predictive position accuracies when using 500 particles, while it is only slightly improving the prediction accuracy over Algorithm 1 when using 100 or 200 particles. The accuracy is better for 200 particles than for 100 particles. For only 100 particles, the average prediction accuracy is worse than for Algorithm 1 for a predictive position accuracy of $0.2$ m, likely due to the fact that the particle filter is a Monte-Carlo method, meaning that there is never a guarantee for convergence [43]. In Figure 3b, the average estimation accuracy for varying length scales of the simulated magnetic field is displayed. As in Figure 3a, the performance of the particle filter improves with increasing amount of particles. Algorithm 1 is able to compensate for odometry drift and achieve estimation error on average below $0.2$ m for length scales between $0.1$ and $0.4$ m, using a constant predictive position error of $0.1$ m. For length scales below $0.1$ m, the linearization error becomes too big for the approximation accuracy of our linearized model to give a good result. For length scales higher than $0.4$ m, the variations in the magnetic field are not rich enough to provide valuable information about the position of the sensor. Therefore, as the length scale of the field increases, even though the field becomes closer to linear and the linearization error continues to decrease, the estimation accuracy does not improve—because the signal-to-noise ratio from the magnetic field measurements also decreases. As we for the simulation results in Figure 3a use a simulated magnetic field map with spatial variations of $0.3$ m, this indicates that as long as the covariance of the predictive distribution does not exceed the length scale of the magnetic field, we can expect Algorithm 1 to have the same estimation accuracy as particle-based methods.

5. Experimental Results

5.1. Model Ship Experiments

We performed experiments to test Algorithm 1 on a model ship in a pool. The magnetic field on the model ship was measured using an Xsens MTi-100 Inertial Measurement Unit (IMU). We recorded the ground truth position and orientation using a motion capture system with cameras and optical markers mounted on the model ship, as shown in Figure 1. The motion capture markers and the IMU were rigidly attached to the ship. The IMU measurements were collected on a computer onboard the ship. The magnetic field was disturbed by metal railings and building structures near the pool. We steered the model ship around in long loops in the pool, with a ground truth trajectory that is displayed in red in Figure 4. The magnetic field measurements were collected in the IMU at 200 Hz, and down-sampled to 5 Hz. The odometry was simulated based on the ground truth position and orientation, according to the odometry model in (14a,b), also at a frequency of 5 Hz (but with some motion capture measurement dropouts due to pool reflections). We simulate drifting odometry by computing the change in position and orientation at each time step from the ground truth and adding a simulated white noise with standard deviation ${\sigma }_{\mathrm{p}}=0.01$, ${\sigma }_{\mathrm{q}}=0.001$. For these experiments, we use real magnetic field data and simulated odometry to investigate the effects of changing odometry noise on our algorithm. In addition, we simulate a constant position odometry bias of $\left[0.0030.0030\right]$ m/time step. Our algorithm was not originally designed to compensate for constant position odometry biases. However, as this often occurs in practice (for example, when the odometry sensors are not perfectly calibrated), we chose to include it in our simulated odometry to test our algorithms’ ability to compensate for a drift that consists both of integrated white noise and a constant disturbance.

In magnetic field SLAM, there is usually no possibility of optimising the hyperparameters for GPs prior to estimating the map, as there is no magnetic field data available. This motivates us to choose hyperparameters prior to running Algorithm 1 [8]. The hyperparameters were set to $l_{\mathrm{SE}}=0.8$, ${\sigma }_{\mathrm{SE}}^2=1$, ${\sigma }_{\mathrm{m}}^2=0.01$, ${\sigma }_{\mathrm{lin}}^2=1$ and 50 basis functions. The basis functions were defined with respect to a cubical domain $\mathrm{\Omega }$ which is as small as possible, and whose border is at least 1 m away from the closest ground truth position. We confirmed empirically that 50 basis functions is sufficient to ensure that the RMSE between the approximation and the full GP predictions in 1000 randomly selected locations in the domain is below the measurement noise. We based the predictions on 2000 samples from the full GP prior, sampled from randomly selected locations in the domain at least 1 m away from the domain border. For the first set of experiments, we investigate and compare the position estimation error of Algorithm 1 with the particle filter-based approach to magnetic field SLAM [8] for odometry noise levels of ${\sigma }_{\mathrm{p}}=0.01$, ${\sigma }_{\mathrm{q}}=0.001$. In Figure 5a, the norm of the magnetic field measurements in locations where there were no motion-capture dropouts are displayed. The norm of the measured magnetic field ranges between $0.46$ and $0.83$ (the Xsens MTi-100 provides unit-less measurements proportionatal to the magnetic field strength). The spatial variations in the measured magnetic field norm are visible in Figure 5a, and the magnetic field stays close to constant for position changes of less than $0.1$ m, while it can change as much as from $0.46$ to $0.83$ when the position change is more than 1 m. As the spatial variations in the magnetic field potential are the sources of the spatial variations in the magnetic field norm [39], we expect that our assumed length scale of $0.8$ m is close enough to the actual length scale of the magnetic field variations to allow for Algorithm 1 to compensate for position estimation drift in the odometry. Figure 5b shows the norm of the magnetic field map learned by Algorithm 1, and comparing to the measured magnetic field norms in Figure 5a, we can see that the norm of the magnetic field estimates and the estimated trajectory are similar, with the learned magnetic field map prediction being more certain in and near the areas where there are more magnetic field measurements available. In Figure 4a, the estimated trajectory from Algorithm 1 is compared with the ground truth trajectory, as well as the dead reckoning position estimate from the simulated odometry. The position estimate from Algorithm 1 compensates visibly for the drift in the odometry.

In Figure 6 the position estimation error of the odometry and Algorithm 1 can be seen to increase at the beginning of the trajectory. After around 31 s, Algorithm 1 can use the learned magnetic field map in combination with the incoming magnetic field measurements to compensate for drift in the estimated position and orientation. In

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

, the position estimation RMSE values for 4 collected data sets of a similar shape as the one displayed in Figure 4a is shown after repeated experiments with the same odometry noise and the same constant drift in the xy-plane, showing that the reduction of RMSE is comparable also for repeated experiments. The position estimation error of the dead reckoning can increase potentially unbounded, while the position estimation error of Algorithm 1 remains bounded when the ship revisits previously mapped areas. However, it will only remain bounded if the quality of the map is good enough to provide information to the position estimate. In

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

, the runtime for each of the algorithms is displayed. The runtime of the PF methods grows proportionally with the number of particles used. As can be seen in Table 1, the PF performs on average worse than the EKF. The trajectory of the highest-weight particle from a single run of each of the PFs is shown in Figure 4b. From our simulation results, given enough particles, the particle filter compensates for drift at least as well as Algorithm 1. However, in contrast to our simulation results where we investigate how well the particle filter performs localization given a previously learned map, in practice, the particle filter has to rely upon magnetic field maps created conditionally on each particle. From these results a standard implementation of the PF in [8] with cubic domain basis functions and resampling at every timestep, with the same hyperparameter settings as the EKF performs worse on our collected model ship data, even given 500 particles. In general, the performance of the particle filter can depend on the resampling strategy, the measurement noise and the process noise [44]. Another possible explanation why the particle filter performs worse for the full SLAM scenario compared to the simulation case, is the fact that for long trajectories, the resampling step can cause loss of diversity amount the particles [45].

The linearization of the measurement function in (13) is performed around the predictive position estimate. The covariance of the estimate can grow when the sensor is moved through an area where the map is previously unknown. The growth rate will depend on the odometry noise. This is demonstrated in Figure 7b, where for 100 Monte Carlo simulations with different odometry noise realisations, the max norm of the predictive estimate from Algorithm 1 can be seen to increase with increasing odometry noise for the same trajectory. In Figure 7a, it can be seen that if we increase the simulated odometry noise above ${\sigma }_p^2=0.002$, the position estimation error of Algorithm 1 is no longer able to compensate for drift in the dead reckoning. A higher odometry noise means more drift is likely to accumulate to the predictive estimation error before revisiting a previously mapped area. The inability of Algorithm 1 to compensate for drift caused by odometry noises above ${\sigma }_p^2=0.002$, therefore, reflects how the assumptions of the measurement function being locally linear no longer hold when the covariance of the predictive distribution becomes large compared to the length scale of the magnetic field disturbances. For the experimental results, for odometry noises above ${\sigma }_p^2=0.002$, we observe a predictive covariance max norm of $0.15$ m in the results in Figure 7b and an accumulated drift of $0.5$ m, which is combination is comparable in magnitude to our length scale $l_{\mathrm{SE}}$ of $0.8$ m. These results are comparable to our simulation results in Section 4, where Algorithm 1 for localization only converges when the position error is $0.3$ m using a length scale of $0.2$ m. In both cases, when the order of the prediction error goes beyond the length scale of the magnetic field variations, Algorithm 1 is no longer able to compensate for drift in the position estimate.

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

Using accelerometer and gyroscope measurements from an IMU mounted on the foot of a pedestrian, it is possible to estimate the position of the pedestrian with high accuracy on a short timescale using a zero-velocity-update (ZUPT) aided EKF [46]. The estimate is obtained by integrating the change in orientation and velocity. In addition, the assumption that when the foot is in the stationary part of the step, it has zero velocity is used to reduce the drift of the position and orientation estimates [46]. The position and orientation estimates are typically accurate at the beginning of a trajectory but can drift over time if biases and/or white noise affect the measurements [47]. This filter was implemented in an open-source implementation by [47].

To test the capabilities of Algorithm 1 on measurements from a foot-mounted sensor, we used magnetic field measurements present in the open-source data set used in [40], to remedy the drift present in the position estimates obtained form their ZUPT-aided EKF. The data set from [40] contains multiple measurement series from an IMU collected in the same hallway, walking the same trajectory. Although the implementation in [40] only used the accelerometer and gyroscope measurements from the IMU, magnetic field measurements were also collected, and are included in the published data. We first ran the open-source implementation from [40] of the ZUPT-aided EKF on the 12 available measurement sequences that were made by collected while walking in a similar trajectory. We ran the ZUPT-aided EKF independently on the 12 experiments and obtained 12 sets of position and orientation estimates. We then concatenated the 12 estimated trajectories by initialising each trajectory at the position and orientation where the previous trajectory ended. This gave a drifting odometry estimate of the position of the pedestrian. The drifting odometry is displayed in Figure 8b. This odometry has an increasing error in position and orientation over time partly because the ZUPT-aided EKF will have some position and orientation drift inherently and partly because of the assumption that the foot-mounted sensor ends in the same orientation at the end of each collected data set as the beginning of the next data set may not be exactly true. However, we can see that most drift accumulates in a constant direction, and drift caused by wrong orientation initialisation should cause twisting of the trajectory. It is, therefore, likely that most of the drift visible in Figure 8b is present due to inherent drift in the ZUPT-aided EKF. To use the odometry in Algorithm 1, we down-sampled the position and orientation estimates to 10 Hz and computed the change in position and orientation between each time step. We then used the changes in position and orientation as input odometry. As SLAM is performed in real-time, we cannot know the hyperparameters a priori to running the algorithm. The magnetic field measurements available in the open-source data set from [40] were without reported units but had a norm that ranged between $0.2429$ and $0.8584$. We therefore selected the expected nonlinear variations ${\sigma }_{\mathrm{SE}}^2=1$. We assumed that the contribution from the constant earth magnetic field had approximately the same order of magnitude and so selected ${\sigma }_{\mathrm{lin}}^2=1$. We set the length scale to $l_{\mathrm{SE}}=2$ m, and we set the measurement noise to be ${\sigma }_{\mathrm{m}}^2=0.01$. We used 1850 basis functions to approximate the magnetic field map. We selected a domain which was the smallest possible cube that was still at least 10 m away from the first lap of the odometry. We found empirically that 1850 were a sufficient amount of basis functions using the same approach as in Section 4 and Section 5. The resulting position estimate from Algorithm 1 compensates for drift in the odometry, as shown in Figure 8a.

6. Conclusions and Future Work

We proposed using an EKF for magnetic field SLAM, which is computationally more efficient and requires less memory than previously proposed methods for magnetic field SLAM. Promisingly, we demonstrated that our proposed algorithm compensates for odometry drift in a way that is comparable to previously proposed, more computationally expensive methods. Using an experiment with magnetic field measurements collected onboard a model ship and using simulated odometry, we ran Monte-Carlo simulations investigating the capabilities of our algorithm to compensate for odometry drift for varying amounts of odometry noise, illustrating that when the uncertainty of the estimate is small compared to the length scale of the magnetic field variations, our proposed algorithm will give a position estimate that compensates for drift in odometry. We also demonstrated the abilities of our proposed algorithm to compensate for drift on an open-source data-set collected with a foot-mounted sensor.

To employ our proposed algorithm in real-life applications such as indoor, surface, underground or underwater navigation, it would be necessary to incorporate sources of odometry information that are available in real-life scenarios, such as inertial sensors or visual odometry from cameras. Another possible direction of future work could be to implement an iterated EKF or another extended Kalman-filter based estimation method that can handle larger non-linearities compared to the EKF [48], and investigate if this improves the convergence of the method. Future research could also look into further reducing the computational requirements associated with reduced-rank GP regression.