Drift-Free Indoor Navigation Using Simultaneous Localization and Mapping of the Ambient Heterogeneous Magnetic Field

In the absence of external reference position information (e.g. GNSS) SLAM has proven to be an effective method for indoor navigation. The positioning drift can be reduced with regular loop-closures and global relaxation as the backend, thus achieving a good balance between exploration and exploitation. Although vision-based systems like laser scanners are typically deployed for SLAM, these sensors are heavy, energy inefficient, and expensive, making them unattractive for wearables or smartphone applications. However, the concept of SLAM can be extended to non-optical systems such as magnetometers. Instead of matching features such as walls and furniture using some variation of the ICP algorithm, the local magnetic field can be matched to provide loop-closure and global trajectory updates in a Gaussian Process (GP) SLAM framework. With a MEMS-based inertial measurement unit providing a continuous trajectory, and the matching of locally distinct magnetic field maps, experimental results in this paper show that a drift-free navigation solution in an indoor environment with millimetre-level accuracy can be achieved. The GP-SLAM approach presented can be formulated as a maximum a posteriori estimation problem and it can naturally perform loop-detection, feature-to-feature distance minimization, global trajectory optimization, and magnetic field map estimation simultaneously. Spatially continuous features (i.e. smooth magnetic field signatures) are used instead of discrete feature correspondences (e.g. point-to-point) as in conventional vision-based SLAM. These position updates from the ambient magnetic field also provide enough information for calibrating the accelerometer and gyroscope bias in-use. The only restriction for this method is the need for magnetic disturbances (which is typically not an issue indoors); however, no assumptions are required for the general motion of the sensor.


INTRODUCTION
With the VR/AR market predicted to be worth $108 billion by 2021 (Digi-Capital, 2017) one important technical challenge is indoor position and orientation tracking. AR in a shopping mall, classroom, or a volume too large for cost effective deployment of cameras can benefit from passive position tracking. Furthermore, head-tracking for AR/VR in harsh environments or confined spaces (e.g. aircraft cockpit) where it is difficult to maintain a camera or the field of view is too restricted, requires alternate sensors. MEMS inertial measurement units (IMUs) are getting smaller, cheaper, and more ubiquitous. For example, they are found in most smartphones, drones, and wearables these days. Although they can provide position information, their use is usually limited to orientation tracking. The reason is that small inertial errors can result in large positional errors due to the integration process. Another way of viewing this is that position updates are very informative for improving the inertial solution. Most commonly, such position information is provided via external sensors such as GNSS, UWB, cameras, sonar, and lasers. RF-based systems that provide absolute position updates typically require additional infrastructure that may not be available indoors. Optical systems that can provide relative position updates are more flexible but require direct line-of-sight, which means the sensor cannot be placed inside pockets or bags. Relative position updates from magnetometers provide a viable solution to all the above problems.
Magnetic signals have been used for indoor positioning by fingerprinting techniques (Zhang et al., 2016) (requires a presurveying phase) and SLAM techniques (does not require a presurveying phase) (Ferris et al., 2007). In SLAM, the pose is often estimated using a particle filter and the magnetic map is modelled separately (Vallivaara et al., 2011). The method presented in this paper combines the pose estimation with the magnetic map modelling in the Gaussian Process Regression (GPR) framework. Compared to popular LiDAR-based SLAM workflows it has a higher computation load, but has the advantage that many of the key steps are integrated into a single least-squares adjustment to allow tuning parameters to adapt to the dataset automatically. For example, most of the manually tuned parameters, such as neighbourhood size for surface modelling, are trained by the data.

Inertial Data
Most modern day IMUs perform strap-down integration at a high frequency (e.g. kilohertz) and then report odometry information as change in rotation (dq) and change in velocity (dv) at a lower, user-desired rate. Given the initial conditions, the orientation (q), velocity (v), and position (p) of an IMU can be computed efficiently using Equation 1. To compensate for the biases in the accelerometers (b a ) and gyroscopes (b ω ), a random walk model is adopted (Equation 2). Note: T is the strap-down integration period, g is the gravity vector, R is the rotation matrix, and the superscripts L and S represent the local frame and sensor frame, respectively. (1) (2)

Magnetic Data
A map of the ambient magnetic field strength can be easily constructed using spatial interpolation techniques (e.g. linear or bi-cubic interpolation) when the trajectory of the sensor is known (e.g. measured by a camera-based position tracking system (Solin et al., 2015)). However, such absolute position information is typically expensive or difficult to obtain. It is more common to have relative position information in indoor navigation.
Assuming a calibrated tri-axial magnetometer that is time synchronized with the IMU is available, the relationship between the observed magnetic field vector in sensor frame ( s y m ) and the magnetic field in the mapping frame ( L m) can be expressed using Equation 3, where x represents the location at which the magnetic field was sampled and the measurement noise ε m is assumed to follow a Gaussian distribution. It is worth mentioning that any small inertial errors may accumulate over time, resulting in significant drifts in the navigation solution. (3) Typically in machine learning textbooks the GPR solution is derived following the Bayesian inference principles (Rasmussen and Williams, 2006). Beginning with Bayes' rule (Equation 4), the marginal likelihood is integrated over the unknown parameters. The best estimate of the unknowns is then determined by maximizing Equation 5. (4) Although the computational complexity of this is O(n 3 ) and the memory requirement is O(n 2 ); this is an efficient GPR formulation when the objective is to predict the magnetic field strength at a new location given all the training magnetic measurements at known locations. In the case of a tightlycoupled IMU and magnetometer SLAM solution, it is more beneficial to follow the frequentist's interpretation, where the probability of the prior multiplied by the likelihood is maximized (Equation 6); this is the maximum a posteriori estimate of the trajectory and the magnetic field map. Note: r is the residuals, C l is the covariance matrix of the measurements, and the unknowns are The ambient magnetic fields are assumed to be realizations of a Gaussian random process prior with a given covariance matrix (i.e. kernel) K, where K is typically chosen to be the squared exponential kernel (Equation 7). This enforces simple smoothness in the local magnetic field.

(7)
Even though this is a valid assumption, recent research has shown that for magnetic fields the three directional components are actually correlated. Based on Maxwell's equation the magnetic field can be split into the B field and the H field; the B field needs to be divergence-free (no sinks) and the H field needs to be curl-free (no swirls) (Wahlström et al., 2013). The curl-free and divergence-free kernels are given in Equation 8. Instead of treating the x, y, and z magnetic fields as three independent smooth surfaces, their physical correlation is included to provide a more accurate model of the local magnetic map, which translates into a more accurate trajectory estimate. (8)

Simulated Datasets
In GP-SLAM the hyper-parameters characterize the ambient magnetic field and are estimated using the data rather than being tuned manually. It can be valuable to understand the impacts of different magnetic fields on the SLAM solution through simulation to better understand the requirements for the proposed methodology. In all simulations below, the sensor is assumed to be exhibiting translational motion only, in a plane where the magnetic field is measured every 25cm, the odometry noise is set to be 0.5mm (0.2% of the distance travelled) with a bias of 5.0mm in both X and Y directions, σ f is 0.1, and l is 10cm, unless otherwise stated.

Effects of varying σ f :
This parameter describes the amplitude of the magnetic signal. For instance, σ f is expected to be small when the magnetic field is homogeneous (e.g. outdoors) and large when near ferromagnetic materials (e.g. computers and motors). σ f can also be interpreted as the noise of the Gaussian Process constraint: the higher the value, the lower the weight of the spatial correlation in the least-squares adjustment, and vice versa.
Four different cases of σ f are shown below where GP-SLAM is performed with the hyper-parameters being fixed to their true value. From Figure 1 and Table 1 it can be observed that the variation of the ambient field must be sufficiently large relative to the sensor noise, but beyond a certain threshold, having more magnetic disturbances does not improve the accuracy of the SLAM solution.

Effects of a wrong σ f :
The quality of the estimated hyper-parameters has a direct impact on the overall GP-SLAM solution. In this simulation, the true σ f is 0.1, but the σ f in the GP-SLAM is fixed to four different values as shown below. It appears from the results that it is better to overestimate σ f rather than underestimating; this is likely because it is better to be conservative and place less weight on the magnetic field position updates rather than "over-trusting" the magnetic field.

Effects of varying l:
This length parameter determines the radius at which the magnetic field samples should be spatially correlated. A large l would indicate that even far points would have an impact on the query point. A small l usually occurs when the local magnetic field is varying rapidly; therefore, only nearby points are useful at predicting the magnetic field signal at the target location.
Two different cases are simulated where the l parameter is fixed to the true values of 0.1m and 0.4m (Figure 3 and Table 3). Smaller l means a smaller convergence region for GP-SLAM but it can yield more accurate results than a larger l. Furthermore, a better initial trajectory is needed to ensure convergence to the global minimum. A larger l can represent a homogeneous magnetic field; therefore the accuracy of the GP-SLAM solution is compromised due to the lack of uniqueness in the magnetic signature.  Table 3: Error in estimated trajectory for various l 3.1.4 Effects of a wrong l: Estimating the wrong length parameter is comparable to setting the wrong neighbourhood search size. In this case the magnetic field was simulated using l = 0.1m. In each trial, l is fixed to a different (wrong) value and its effects on the global SLAM solution are reported in Figure 4 and Table 4.  Table 4: Error in estimated trajectory when using the wrong l Small errors in l do not have a significant impact on the SLAM solution. While GP-SLAM does not appear to be hypersensitive to the neighbourhood radius, it is better to underestimate l than to overestimate; this is likely because it is better to ignore valuable information from nearby points rather than to allow spatially uncorrelated measurements from a larger neighbourhood to influence the query point.

Effects of varying odometry noise:
Like most targetless optical SLAM solutions, GP-SLAM is sensitive to the initial pose. When the initial trajectory is significantly different from the true trajectory, GP-SLAM tends to converge to a local minimum because it uses a nonlinear least-squares formulation. Unlike frame-based optical SLAM methods though, each magnetometer reading represents the measurement of a single point, which is insufficient to estimate the 3D position and 3D orientation. Hence, it relies heavily on the odometry information to link temporally close measurements. As shown in Table 5, a smaller odometry noise means a larger convergence region (i.e. larger biases in the odometer can be handled). Although a small odometry noise is always beneficial, beyond a certain threshold, more precise odometry does not seem to improve the solution significantly.

Real Datasets
In the following experiments, a MEMS-based IMU from Xsens Technologies, the MTi-300 Attitude and Heading Reference System was used.

Experiment 1
In the first test, the IMU was placed on a typical office desk with computer and smartphone within its vicinity. It was then moved along a rectangular trajectory four times with complete overlap between each loop. Even from looking at the magnitude of the magnetic measurements it is possible to see the similarities between the four loops ( Figure 5). In this trivial example, it is possible to perform loop-closure detection using auto-correlation since the variation in the magnetic field is as high as 1.67 a.u. (Jung and Myung, 2015). To obtain a sensible initial trajectory, a weak zero position update was applied to constrain the IMU position to be within a one metre radius sphere. The IMU trajectory before and after GP-SLAM is shown in Figure 6. Both the horizontal and vertical accuracies were improved from the centimetre-level to millimetre-level after 1800 iterations. The high number of iterations is an undesirable trait of GP-SLAM as the Lagrangian cost reduction is quite slow after every iteration (Figure 7).

Experiment 2
In the second experiment, the IMU was moved around in a spiral pattern similar to the simulated trajectory. This test highlights one of the strengths of GP-SLAM, which is the unnecessity for perfect overlap between each pass of the same area. Unlike the first experiment, by studying the magnetic signal alone it is difficult to perform loop-closure detection ( Figure 8). A threshold for the auto-correlation will need to be set and the possibility of revisiting the same position in a different direction will need to be handled in the logic (Jung and Myung, 2015). The estimated trajectory before and after using the magnetic field to perform GP-SLAM is provided in Figure 9. The adjustment took 797 iterations to converge and the RMSE of the trajectory is reduced from the centimetre-level to millimetre-level. One of the main factors that contributed to the difference in trajectory is the recovered accelerometer and gyroscope biases (Table 6).

Experiment 3
The third experiment was performed on a special aluminum table that is free of ferromagnetic materials. In addition, no magnetic objects were placed within one metre of the table. The IMU was moved in a square pattern multiple times. When studying the magnitude of the magnetic measurements the field appears to be fairly homogeneous. However, when examining the signal in detail, a range of 0.07 a.u. between the maximum and minimum magnetic reading can be perceived. Such low magnetic disturbance poses a challenging situation for GP-SLAM. In all other test cases, it did not matter if the squared exponential kernel or curl-free and divergence-free kernels were applied, the differences in the trajectories were insignificant. However, in a near-homogeneous field, a more noticeable impact of the chosen kernel can be perceived. Figure 10 shows the GP-SLAM solution using the two different kernels. In this scenario neither kernel was able to reconstruct a perfect squareshaped trajectory. The squared exponential kernel resulted in a higher maximum error but preserved the orthogonality of all edges, while the curl-free and divergence-free kernel closed all the loops properly but deformed the overall geometry of the trajectory into an arbitrary quadrilateral. The latter solution might be preferred if it is to be combined with other optical SLAM solutions, because it is more consistent internally (i.e. all loops were properly detected).

Experiment 4
The final indoor experiment took place on a typical wooden office desk. The MTi-300 was treated like a pen, as it was moved along the surface writing the word "Xsens". After GP-SLAM the handwritten characters became legible even though no active beacons were introduced.

Outdoor Environments:
In a real homogeneous magnetic field environment, GP-SLAM has a favourable property of not making the initial solution any worse. To demonstrate this, the aluminium table was moved outdoors into an open field where the IMU was moved along a rectangular track several times. In this case, the l of the hyperparameter approaches a large number rapidly, σ f approaches zero, and the adjustment converges. Mathematically it can be seen from Equation 7 that as l approaches infinity all the covariances approach σ f (which is zero), effectively disabling all spatial correlations automatically. In conventional loop-closures in SLAM, a wrong association can have a devastating effect on the entire solution due to the strong assumption being made in the global relaxation step. In GP-SLAM, the assumption being made is much weaker and therefore has a smaller impact on the solution when the assumptions are violated; i.e. it is more robust.

CONCLUSION AND FUTURE WORK
This paper addresses the problem of indoor navigation by exploiting magnetic disturbances in a GP-SLAM framework with a tightly-coupled MEMS IMU. The orientation, velocity, position, sensor biases, hyper-parameters, and magnetic field map were all estimated simultaneously while performing continuous loop-detection and loop-closure in a batch leastsquares adjustment. Through different experiments it was shown that magnetic field SLAM in a GPR framework can improve the overall trajectory without making the solution significantly worse. Although it has no effect on the overall trajectory when applied in typical outdoor environments, it can be argued that satellite signals are available outdoors and the magnetometer can be used for heading updates instead.
Future work will focus on improving the efficiency of GP-SLAM through sparse kernel approximations in order to scale it to larger problems. The current solution assumes the magnetic field is not changing; to make it applicable to more consumer applications (e.g. wearable technologies) the static magnetic map assumption will be lifted.