Next Article in Journal
Potential of Near-Infrared Spectroscopy for the Determination of Olive Oil Quality
Next Article in Special Issue
Multipath-Assisted Radio Sensing and State Detection for the Connected Aircraft Cabin
Previous Article in Journal
Research on UAV Robust Adaptive Positioning Algorithm Based on IMU/GNSS/VO in Complex Scenes
Previous Article in Special Issue
Meaningful Test and Evaluation of Indoor Localization Systems in Semi-Controlled Environments
 
 
Font Type:
Arial Georgia Verdana
Font Size:
Aa Aa Aa
Line Spacing:
Column Width:
Background:
Article

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

1
Delft Center for Systems and Control, Delft University of Technology, 2628 CD Delft, The Netherlands
2
Maritime and Transport Technology, Delft University of Technology, 2628 CD Delft, The Netherlands
*
Author to whom correspondence should be addressed.
Sensors 2022, 22(8), 2833; https://doi.org/10.3390/s22082833
Submission received: 25 February 2022 / Revised: 1 April 2022 / Accepted: 5 April 2022 / Published: 7 April 2022
(This article belongs to the Special Issue Advances in Indoor Positioning and Indoor Navigation)

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 ( N p N m 2 ) , 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 ( N m 2 ) . 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 ( N p N m 2 )  [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 ( N m 2 ) at each time step, instead of O ( N m 2 N p ) . 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 ( x t | y 1 : t b , Δ p 1 : t w , Δ q 1 : t b ) ,
where we denote the available magnetic field measurements by y 1 : t b , the odometry describing the change in position and orientation Δ p 1 : t w , Δ q 1 : t 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 , , 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 = [ ( p t w ) ( q t w b ) m ] ,
where p t w denotes the position, q t w 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 b = R t b w p φ ( p t w ) + e m , t b , e m , t b N ( 0 , σ m 2 I 3 ) ,
where y t b denotes the magnetic field measurement, e m , t b denotes the measurement noise and R t b w denotes the rotation from world to body frame, corresponding to the conjugate of the quaternion q t w 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 p φ ( p t w ) is our model of the magnetic field. We model the magnetic field as in [42] as the gradient of the function φ ( p t w ) with respect to the position p t w , where φ : R 3 R is a scalar potential distributed as a GP with prior
φ N ( 0 , κ SE ( · , · ) + κ lin ( · , · ) ) ,
and where the kernel is defined by the functions
κ SE ( p , p ) = σ SE 2 exp p p 2 2 2 l SE 2 ,
κ lin ( p , p ) = σ lin 2 p p ,
with σ SE , σ lin , l SE and σ m being hyperparameters. The hyperparameter l SE refers to the length scale of the spatial variations in the magnetic field potential that is represented by the kernel [42]. The parameters σ SE , σ lin and σ 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 κ 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 κ 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 Ω R 3
p 2 ϕ i ( p ) = λ i ϕ i ( p ) , p Ω , ϕ i ( p ) = 0 , p δ Ω ,
where ϕ i is the i’th eigenfunction, and λ 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 Ω = [ L l , 1 , L u , 1 ] × [ L l , 2 , L u , 2 ] × [ L l , 3 , L u , 3 ] . In this case, using the N m first eigenfunctions to represent the potential gives the approximation
φ ( p ) Φ ( p ) m ,
with Φ ( p ) being the matrix
Φ ( p ) = p ϕ 1 ( p ) ϕ N m ( p ) ,
where ϕ i is the i’th eigenfunction of the Laplace basis, and m 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 ϕ i ( p ) has a closed form expression given by
ϕ i ( p ) = d = 1 3 2 L u , d L l , d sin π n i , d ( p d + L l , d ) L u , d L l , d ,
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
λ i = d = 1 D π n i , d L u , d L l , d 2 ,
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 N ( 0 , Λ ) ,
where Λ is defined as
Λ = diag σ lin 2 I 3 , S SE ( λ 1 ) , , S SE ( λ N m ) ,
with S SE ( · ) being the spectral density of the squared exponential kernel, as defined in [7]. This corresponds to the magnetic field potential φ ( p ) Φ ( p ) 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 b R t b w p Φ ( p t w ) m + e m , t b e m , t b N ( 0 , σ m 2 I 3 ) ,
with the closed form expressions for p Φ ( p t w ) given in Appendix A. This measurement model is identical to the measurement model used in [8], with the exception of the basis functions Φ ( p t w ) , which are different as they are defined with respect to different domains.

2.2. Dynamic Model

We assume access to noisy odometry measurements Δ p t w and Δ q t 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
p t + 1 w = p t w + Δ p t w + e p , t w , e p , t w N ( 0 , σ p 2 I 3 ) ,
q t + 1 w b = q t w b Δ q t b exp q ( e q , t b ) , e q , t b N ( 0 , σ q 2 I 3 ) ,
where e p , t w and e q , t b denote the position and orientation odometry measurement noise respectively, ⊙ is the quaternion product and exp 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 p ^ t | t 1 w , m ^ t | t 1 , q ^ t | t 1 w b and p ^ t | t w , m ^ t | t , q ^ t | t w b respectively. We initialise the magnetic field state estimate as m ^ 0 | 0 = 0 N m × 1 according to the reduced-rank GP prior in (11). We initialise the orientation estimate according to the initial rotation q ^ 0 | 0 w b = q 0 w b between the world and body frame as defined in Section 2. We initialise the position estimate as p ^ 0 | 0 w = 0 3 × 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 ξ t defined as
ξ t = [ ( δ t w ) ( η t w ) ν t ] ,
where δ t w = p t w p ^ t | t 1 w denotes the position estimation error, ν t = m m ^ t | t 1 denotes the magnetic field state estimation error, and where η t w denotes the orientation estimation error parametrised as an axis-angle deviation according to
q t w b = exp q ( η t w ) q ^ t | t 1 w b .
Similarly, we represent the deviation between the true and estimated filtered state by an error state ξ ˜ t defined as
ξ ˜ t = [ ( δ ˜ t w ) ( η ˜ t w ) ν ˜ t ] ,
where δ ˜ t w = p t w p ^ t | t w , ν ˜ t = m m ^ t | t , and where η ˜ t w denotes the filtered orientation estimation error according to
q t w b = exp q ( η ˜ t w ) q ^ t | t w b .
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 Λ . Hence, our initial error state ξ 0 has a covariance
P 0 | 0 = 0 6 × 6 0 6 × ( N m + 3 ) 0 ( N m + 3 ) × 6 Λ .
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 ξ ˜ t . Inserting (15), (17) and (26a,b) into the dynamic model (14a,b) gives
p ^ t | t 1 w + δ t w = p ^ t 1 | t 1 w + Δ p t w + δ ˜ t 1 w + e p , t w
exp q η t w = exp q η ˜ t 1 w q ^ t | t 1 w b exp q ( e q , t b ) ( q ^ t | t 1 w b ) C ,
where ( q ^ t | t 1 w b ) C denotes the conjugate of the quaternion q ^ t | t 1 w b . The linearization of the dynamic model with respect to the error states gives the following propagation of the error states
δ t w = δ ˜ t 1 w + e p , t w ,
η t w η ˜ t 1 w + R ^ t | t 1 w b e q , t b ,
ν t = ν ˜ t 1 ,
where R ^ t | t 1 w b denotes the rotation matrix corresponding to the rotation represented by the quaternion q ^ t | t 1 w 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
ξ t ξ ˜ t 1 + e dyn , t , e dyn , t N ( 0 ( N m + 9 ) × 1 , Q ) ,
where
Q = σ p 2 I 3 0 3 × 3 0 3 × ( N m + 3 ) 0 3 × 3 σ q 2 I 3 0 3 × ( N m + 3 ) 0 ( N m + 3 ) × 3 0 ( N m + 3 ) × 3 0 ( N m + 3 ) × ( N m + 3 ) .
As the linearization of the error state propagation is given in (22), the covariance P t | t 1 of the predictive state error ξ 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 ξ t . The linearized measurement model is given by
y t b = R ^ t | t 1 b w p Φ ( p ^ t | t 1 w ) m ^ t + H t ξ t + e m , t b , e m , t b N ( 0 3 × 1 , σ m 2 I 3 ) ,
where
H t = pp Φ ( p ^ t | t 1 w ) m ^ t p Φ ( p ^ t | t 1 w ) m ^ t × p Φ ( p ^ t | t 1 w )
with v × being the scew-symmetric matrix representing the cross product v × u between two vectors v , u R 3 as a matrix multiplication v × u (explicit expression for v × is given in [41]), and pp Φ ( · ) being the Jacobian of the basis functions, given in Appendix A. Note that this Jacobian is a matrix with 3 × 3 × ( N m + 3 ) entries, and multiplying it with the ( N m + 3 ) -dimensional state vector m therefore gives a 3 × 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 ξ ^ t of the predictive state error ξ 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 ξ ˜ t then becomes the same as the covariance of the estimated predictive error state ξ ^ t . Recursively applying the dynamic update, measurement update and re-linearization step results in Algorithm 1.
Algorithm 1: EKF for magnetic field SLAM
   Input:  Δ p t w , Δ q t b , y t b t = 1 N
   Output:  p ^ t | t w t = 1 N , q ^ t | t w b t = 1 N , m ^ t | t t = 1 N
          Initialisation: p ^ 0 | 0 w = 0 3 × 1 , q ^ 0 | 0 w b = q 0 w b , m ^ 0 | 0 = 0 ( N m + 3 ) × 0 , (19)
1: for t = 1 to N do
2:    Dynamic update
p ^ t | t 1 w = p ^ t 1 | t 1 w + Δ p t w
q ^ t | t 1 w b = q ^ t 1 | t 1 w b Δ q t b
m ^ t | t 1 = m ^ t 1 | t 1
P t | t 1 = P t 1 | t 1 + Q
3:    Measurement update
z t = R ^ t | t 1 w b y t b Φ p ( p ^ t | t 1 w ) m ^ t | t 1
S t = H t P t | t 1 H t + σ m 2 I 3
K t = P t | t 1 H t S t 1
ξ ^ t = K t z t
P t | t = P t | t 1 K t S t K t
4:    Relinearization
p ^ t | t w = p ^ t | t 1 w + δ ^ t w
q ^ t | t w b = exp q ( η ^ t w ) q ^ t | t 1 w b
m ^ t | t = m ^ t | t 1 + ν ^ t
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 m ^ t | t with the known m sim everywhere in our algorithm. As a consequence of this, we can alo skip (26c) and (28c), use P 0 | 0 = 0 6 × 6 and
H t = pp Φ ( p ^ t | t 1 w ) m p Φ ( p ^ t | t 1 w ) m × .
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 p , t w and e q , t b to (14a,b). We simulate a nonlinear field by drawing a sample from the reduced-rank GP prior m sim N ( 0 , Λ ) , with σ lin = 0 , σ SE = 0.1 and l SE = 0.2 . We used 50 basis functions to represent the magnetic field map. Using a domain that is 3 m × 3 m × 1 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 m ^ t with m sim , and using σ 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 sim in place of 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 σ p = 0.01 , σ 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 [ 0.003 0.003 0 ] 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 SE = 0.8 , σ SE 2 = 1 , σ m 2 = 0.01 , σ lin 2 = 1 and 50 basis functions. The basis functions were defined with respect to a cubical domain Ω 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 σ p = 0.01 , σ 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, 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, 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 σ 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 σ 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 σ 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 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 σ SE 2 = 1 . We assumed that the contribution from the constant earth magnetic field had approximately the same order of magnitude and so selected σ lin 2 = 1 . We set the length scale to l SE = 2 m, and we set the measurement noise to be σ 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.

Author Contributions

Conceptualization, M.K. and F.V.; methodology, F.V.; software, F.V.; formal analysis, F.V.; data curation, and F.V.; writing—original draft preparation, M.K. and F.V.; writing—review and editing, M.K. and R.H. and F.V.; supervision, R.H. and M.K.; funding acquisition, R.H. and M.K. All authors have read and agreed to the published version of the manuscript.

Funding

This research was supported by ResearchLab Autonomous Shipping (RAS). This publication is part of the project “Sensor Fusion For Indoor localization Using The Magnetic Field” with project number 18213 of the research program Veni which is (partly) financed by the Dutch Research Council (NWO).

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

The data used to obtain the results in this paper can be found in https://github.com/fridaviset/EKFMagSLAM.

Acknowledgments

The experiments supporting this work were carried out in collaboration with Matt Hepworth, with ResearchLab Autonomous Shipping (RAS). We would like to thank him for his help in obtaining ground truth position and orientation estimates using a motion capture system, and for collecting IMU measurements onboard the model ship.

Conflicts of Interest

The authors declare no conflict of interest. The funders had no role in the design of the study; in the collection, analyses, or interpretation of data; in the writing of the manuscript, or in the decision to publish the results.

Abbreviations

The following abbreviations are used in this manuscript:
SLAMSimultaneous localization and mapping
EKFExtended Kalman filtering
RBPFRao-Blackwellized particle filter
GPGaussian process
IMUInertial measurement unit
ZUPTZero-velocity upate
RMSERoot mean squared error
GNSSGlobal navigation satelite system

Appendix A. Analytical Jacobians

The gradient of the basis functions p Φ ( p ) used in (13) (the basis functions Φ ( p ) are defined in (8)) is a 3 × ( N m + 3 ) matrix. The first three columns are given by an identity matrix
{ p Φ ( p ) } 1 : 3 = I 3 × 3 ,
and the j + 3 rd column is given by the gradient of the j’th basis function
{ p Φ ( p ) } j + 3 = p ϕ j ( p ) = π n j ( 1 ) L u , 1 L l , 1 c 1 s 2 s 3 π n j ( 2 ) L u , 2 L l , 2 s 1 c 2 s 3 π n j ( 3 ) L u , 3 L l , 3 s 1 s 2 c 3 ,
with s d and c d defined for the three spatial dimensions d = 1 , 2 , 3 as
s d = sin π n j ( d ) ( p d L l , d ) ( L u , d L l , d ) 1 1 2 ( L u , d L l , d ) ,
c d = cos π n j ( d ) ( p d L l , d ) ( L u , d L l , d ) 1 1 2 ( L u , d L l , d ) .
The Jacobians of the basis functions used in (29) is a 3 × 3 × ( N m + 3 ) matrix. The first 3 entries along the third dimensions are all zero-matrices
{ pp Φ ( p ) } 1 = { pp Φ ( p ) } 2 = { pp Φ ( p ) } 3 = 0 3 × 3 ,
and the ( j + 3 ) rd entry along the third dimension is given by
{ pp Φ ( p ) } j = pp ϕ j ( p ) = π n j ( 1 ) L u , 1 L l , 1 2 s 1 s 2 s 3 π n j ( 1 ) L u , 1 L l , 1 π n j ( 2 ) L u , 2 L l , 2 c 1 c 2 s 3 π n j ( 2 ) L u , 2 L l , 2 π n j ( 1 ) L u , 1 L l , 1 c 1 c 2 s 3 π n j ( 2 ) L u , 2 L l , 2 2 s 1 s 2 s 3 π n j ( 3 ) L u , 3 L l , 3 π n j ( 1 ) L u , 1 L l , 1 c 1 s 2 c 3 π n j ( 3 ) L u , 3 L l , 3 π n j ( 2 ) L u , 2 L l , 2 s 1 c 2 c 3 π n j ( 1 ) L u , 1 L l , 1 π n j ( 3 ) L u , 3 L l , 3 c 1 s 2 c 3 π n j ( 2 ) L u , 2 L l , 2 π n j ( 3 ) L u , 3 L l , 3 s 1 c 2 c 3 π n j ( 3 ) L u , 3 L l , 3 2 s 1 s 2 s 3 .

References

  1. 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]
  2. 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]
  3. 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]
  4. 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]
  5. 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]
  6. Woodman, O.J. An Introduction to Inertial Navigation; Technical Report; Computer laboratory University of Cambridge: Cambridge, UK, 2007. [Google Scholar]
  7. 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]
  8. 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]
  9. 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]
  10. 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]
  11. 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]
  12. 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]
  13. 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]
  14. 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]
  15. 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]
  16. 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]
  17. 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]
  18. 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]
  19. 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]
  20. 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]
  21. 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]
  22. 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]
  23. 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]
  24. 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]
  25. 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]
  26. 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]
  27. 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]
  28. 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]
  29. Wu, Y.; Shi, W. On Calibration of Three-Axis Magnetometer. IEEE Sens. J. 2015, 15, 6424–6431. [Google Scholar] [CrossRef] [Green Version]
  30. 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]
  31. 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]
  32. 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]
  33. Canciani, A.; Raquet, J. Airborne Magnetic Anomaly Navigation. IEEE Trans. Aerosp. Electron. Syst. 2017, 53, 67–80. [Google Scholar] [CrossRef]
  34. 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]
  35. 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]
  36. 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]
  37. 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]
  38. Kok, M.; Schön, T.B. Magnetometer Calibration Using Inertial Sensors. IEEE Sens. J. 2016, 16, 5679–5689. [Google Scholar] [CrossRef] [Green Version]
  39. 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]
  40. 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).
  41. 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]
  42. 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]
  43. Chen, Z. Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond. Statistics 2003, 182, 1–69. [Google Scholar] [CrossRef]
  44. Gustafsson, F. Statistical Sensor Fusion; Studentlitteratur AB: Lund, Sweden, 2013. [Google Scholar]
  45. 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]
  46. Foxlin, E. Pedestrian tracking with shoe-mounted inertial sensors. IEEE Comput. Graph. Appl. 2005, 25, 38–46. [Google Scholar] [CrossRef] [PubMed]
  47. 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]
  48. 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 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.
Sensors 22 02833 g001
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 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.
Sensors 22 02833 g002
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 SE .
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 SE .
Sensors 22 02833 g003
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 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.
Sensors 22 02833 g004
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 p Φ ( p ) m ^ N | N 2 , and the opacity is inversely proportional with the trace of the covariance matrix of the magnetic field map estimate in each location, Tr ( p Φ ( p ) P N | N ( p Φ ( p ) ) ) . (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 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 p Φ ( p ) m ^ N | N 2 , and the opacity is inversely proportional with the trace of the covariance matrix of the magnetic field map estimate in each location, Tr ( p Φ ( p ) P N | N ( p Φ ( p ) ) ) . (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.
Sensors 22 02833 g005
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 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.
Sensors 22 02833 g006
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 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.
Sensors 22 02833 g007
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.
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.
Sensors 22 02833 g008
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.
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 RMSEsData Set 1Data Set 2Data Set 3Data Set 4
Algorithm 10.53 ± 0.150.58 ± 0.180.53 ± 0.240.98 ± 0.62
RBPF with 100 particles0.85 ± 0.270.92 ± 0.260.95 ± 0.421.53 ± 0.53
RBPF with 200 particles0.85 ± 0.220.89 ± 0.270.98 ± 0.471.48 ± 0.46
RBPF with 500 particles0.87 ± 0.190.86 ± 0.241.00 ± 0.401.53 ± 0.50
Odometry1.98 ± 0.541.52 ± 0.481.76 ± 0.521.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.
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.
RuntimesData Set 1Data Set 2Data Set 3Data Set 4
Algorithm 10.06 ± 0.010.05 ± 0.000.14 ± 0.010.05 ± 0.00
RBPF with 100 particles12.85 ± 0.269.24 ± 0.1021.55 ± 0.329.91 ± 0.14
RBPF with 200 particles25.70 ± 0.4918.44 ± 0.16642.64 ± 0.2719.72 ± 0.20
RBPF with 500 particles64.24 ± 0.8246.02 ± 0.20106.93 ± 0.3449.43 ± 1.29
Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

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

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Metrics

Back to TopTop