2021

[1]Xuezhi Wang

[1]School of Science, RMIT University, Vic 3000, Australia

2]Department of Electrical & Electronic Engineering, University of Melbourne,
Vic 3053, Australia

3]ARC Centre of Excellence for Nanoscale BioPhotonics, RMIT University,
Victoria 3000, Australia

4]School of Chemistry, University of Melbourne, Vic 3010, Australia

5]School of Physics, University of Melbourne, Vic 3010, Australia

# Quantum diamond magnetometry for navigation in GNSS denied environments

<EMAIL_ADDRESS>Wenchao Li<EMAIL_ADDRESS>Bill Moran
<EMAIL_ADDRESS>Brant C. Gibson<EMAIL_ADDRESS>Liam T. Hall
<EMAIL_ADDRESS>David Simpson<EMAIL_ADDRESS>Allison N. Kealy
<EMAIL_ADDRESS>Andrew D. Greentree<EMAIL_ADDRESS>*  [
[  [  [

###### Abstract

Satellite-based navigation is a transformational technology that underpins
almost all aspects of modern life. However, there are environments where
global navigation satellite systems (GNSS) are not available, for example
undersea or underground, and navigation that is robust to GNSS outages is also
required for resilient systems. In this context, we are exploring the
potential for quantum diamond magnetometers to be used as aids for inertial
measurement units for navigation in GNSS-denied environments. We perform
simulations of the magnetic field measurements combined with probabilistic
data association for data mapping; and probabilistic multiple hypotheses (or
Viterbi) map matching filters. These methods are used to explore the expected
navigation errors likely to be present in scalar and vector magnetometry with
diamond at the sensitivities available using current and expected near-term
devices.

###### keywords:

Map Matching, Total Magnetic Intensity Data Map, Expectation Maximisation,
Multiple Hypotheses Tracker, Probabilistic Data Association, diamond,
magnetometry

##  1 Introduction

Next-generation quantum technologies are poised to cause disruption to a wide
range of sectors 2017:Degen . Such potential derives from features including
extreme sensitivity, with (in some cases) nano-scale sensor elements, new
sensing modalities including entangled sensing (Giovannetti2004, ), and the
promise of low-Size, Weight and Power (SWAP). Superconductors represent a
mature quantum sensing platform (Porrati2019, ), albeit one restricted to
cryogenic temperatures, whereas atomic vapour cells (Knappea, ) and diamond
(the subject of this work) (Taylor2008, ) are emerging as some of the most
promising room-temperature compatible quantum sensors.

Our focus here is on the potential application of diamond containing the
negatively-charged nitrogen-vacancy center (NV) for navigation in GNSS denied
environments. NV diamond is promising for such applications due to its ambient
temperature and magnetic field operation (Barry2020, ), and operation in both
projection and vector magnetometry (Schloss2018, ) modes. Because NV diamond
is a still developing technology, we present out results as a function of
sensitivity, noting that NV magnetometers have demonstrated sensitivity down
to 900 fT/$\sqrt{\text{Hz}}$ (Wolf2015, ).

In GNSS-denied environments, platform navigation performance is dominated by
the accuracy of onboard inertial sensors. Even with high end inertial sensors,
which exhibit extremely low bias and drift, it is not possible to avoid the
build-up of navigation errors over long time frames (Titterton2004a, ).
Removing these accumulated navigation errors is therefore crucial for
navigation accuracy (Groves2013b, ). This removal, or correction, is achieved
using one or more aiding sources that provide positional information, i.e. a
position fix.

Geophysical map matching is an effective method for localisation and
navigation where GNSS is not available; such as underwater, urban, or hostile
environments. Such maps may derive from a variety of geophysical data, e.g.
gravity, airborne electromagnetics, passive seismic, active seismic,
magnetotellurics, magnetic, radiometric, and elevation (Tuohy1996, ). Map
matching estimates a platform’s location by directly matching the geophysical
measurement to a point in the map, i.e. the matching occurs explicitly in the
map space. The resulting location estimate is then integrated into the
navigation system in a similar way to a loosely coupled GNSS/INS system
(Titterton2004a, ).

Although conceptually simple, map matching with geophysical maps suffers from
map measurement ambiguity issues. First, the geophysical measurements
themselves are degraded by sensor noise so the measurements will not match the
map exactly. Second, the measurements may match multiple points within the map
(i.e. non-unique solution) as the map lookup process is a scalar to vector
mapping. Third, the locations where the measurements were acquired is of
course uncertain. Finally, the map itself suffers from finite spatial and
signal resolution.

Various approaches for resolving map measurement ambiguity can be found in the
literature. The basic idea is to improve knowledge of the platform location by
processing multiple measurements with vehicle trajectory constraints. For
example, (Tuohy1996, ) combines multiple geophysical maps (altimetry, magnetic
and gravity) with a multiple hypotheses tracker. In (Kamgar-Parsi1999, ) a
batch of possible platform locations on a map are identified from field
measurements by finding the closest contour points using a downhill simplex
method to solve the position minimization problem. Following the same idea, an
iterated closest contour point (ICCP) algorithm is proposed in (Han2018, ),
later on improved in (Wang2022a, ). In (Hostetler1983, ) a similar technique
was used for terrain-aided navigation under the name of multilateral arc
matching. Nevertheless, these approaches do not properly model the
uncertainties of measurement origin in the statistical sense.

More recently, a probabilistic multiple hypotheses tracking map matching
(PMHT-MM) filter was proposed in (Wang2022, ) for gravitational map matching
localisation to aid an inertial navigation system (INS). Unlike the ICCP
method, the filter takes both measurement ambiguity and platform kinematic
constraints into account in an iterative procedure of forward and backward
filtering. The gravitational map matching localisation is also approached
using the Viterbi algorithm under the maximum likelihood criterion in (Li2022,
). Both approaches show more accurate, robust and effective performance over
the standard ICCP method for the case of INS aiding using gravitational maps.

Here we consider magnetometery aided inertial navigation with total magnetic
intensity (TMI) maps. A probabilistic data association (PDA) method is adopted
to address the measurement ambiguity problem, which is a nontrivial problem in
map matching localisation using geophysical data maps. We show that the PDA
method provides an effective way to map a field measurement into geolocation,
but also enables a quantitative analysis of localisation error with respect to
the magnetometer noise levels for a given TMI reference map. Furthermore, we
implement a magnetometery aided INS using this method to determine the
relationship between magnetometer noise levels and navigation performance.

This manuscript is organised as follows: We first briefly introduce the
mechanism for magnetic field sensing with diamond. Second, the problem of
aided inertial navigation is formulated using Bayesian filtering, and the
issues associated with magnetometery map matching are presented in Section 3.
We then describe the probabilistic multiple hypothesis map matching method in
Section 4. In Section 5, the performance of the proposed algorithm for aiding
of INS using online maps are demonstrated in a realistic navigation scenario
without GNSS.

##  2 Magnetometry using nitrogen-vacancy centers in diamond

Quantum sensing with diamond is typically performed using the negatively
charged nitrogen-vacancy (NV) color center. This defect is formed when a
substitutional nitrogen is next to a lattice vacancy (missing carbon atom).
The center is usually found in one of two charge states, neutral and singly
negatively charged, with the single negative charge state being the only one
used for magnetometry. The full details of the center are well reviewed in
(DOHERTY2013, ).

The standard readout for NV is via some form of optically detected magnetic
resonance (ODMR). The NV electronic ground state is a spin triplet. By
illuminating off-resonantly (usually in the green) NV centers are optically
polarised into the spin 0 state. The degeneracy between spin 0 and spin $\pm
1$ states is lifted by a crystal field splitting around 2.88 GHz, and the
Zeeman effect lifts the degeneracy between the $+1$ and $-1$ states, with
increasing separation as a function of magnetic field.

The optical spin polarisation is due to the combination of spin conserving and
spin non-conserving transitions, with the spin $\pm 1$ states more likely to
de-excite via the spin-non-conserving singlet pathway. This process is slower
than direct spontaneous emission and occurs at 1040 nm, instead of the usual
NV fluorescence spectrum from 630 to 750 nm.

A resonant radio-frequency field will drive transitions from spin 0 to one of
the $\pm 1$ spin states. The resonance will tend to mix populations, whereas
the green excitation tends to polarise. The combination of these effects means
that when the RF field is in resonance, the fluorescence level drops, leading
to a clear optical signal.

Lastly, we note that because of the crystallagraphic structure of diamond,
there are four possible orientations for NV centers in the lattice.
Accordingly, the projection of an arbitrary magnetic field along each NV axis
will be different, and so a single fluorescence diamond can be used as a
vector magnetometer by reading out the magnetic field along each
crystallographic axis.(Schloss2018, ).

Diamond magnetometry is a rapidly developing field, with many techniques
designed to improve sensitivity including isotopic enrichment
(Balasubramanian2009, ), portability through embedding in optical fibers
(Ruan2018, ; Bai2020, ; FILIPKOWSKI202210, ), spin to charge readout
(Shields2015, ), electrically detected magnetic resonance (Bourgeois2015, ),
and laser threshold magnetometry (Jeske2016, ; Dumeige:19, ; Hahl2022, ). As
such, we have chosen to perform our subsequent analyses as a function of the
magnetic field sensitivity, to better inform the sensitivities required for
future platform development.

##  3 INS aiding via map matching

The process of an aided INS can be described by a recursive Bayesian filtering
system, where the system prediction is given by the onboard INS, and system
update is based on the measurements from external aiding sources. Fig. 1
illustrates the block diagram of a generic aided INS with aiding from a map
matching system. The INS is initialised from known parameters and at time $k$
propagates the navigation state ${\bf X}_{\mathrm{INS},k\mid k-1}$ based on
the earth surface motion model and inertial measurements $({\bf
f}_{b}\,\bm{\omega}_{b})$. The global position measurements, estimated from
map matching, are assumed to be Gaussian distributed with mean
$\hat{\bm{x}}^{s}$ \- the estimated sensor location where $s$ is taken and
covariance $\Sigma^{s}$, and are incorporated into the system via a widely
used method to update the navigation state ${\bf X}_{\mathrm{INS},k\mid k}$.

Figure 1: Illustration of a generic single recursion map matching aided
Inertial Navigation System (INS). The map matching block takes the current
kinematic state estimate $\hat{\bm{x}}$ and covariance $\bm{\Sigma}$ of the
platform, and augments its position component by matching the geophysical
measurement $\bm{s}_{k}$ to a geo-reference map. The prior kinematic state,
$\hat{\bm{x}}$, is taken from the components of inertial navigation state
$\bm{X}_{\mathrm{INS}}$, the latter is propagated using the accelerator and
gyroscope measurements, respectively $\bm{f}_{b}$ and $\bm{\omega}_{b}$, based
on the motion model on the surface of the earth in the INS block.

Finally, $\bm{X}_{\rm{INS}}$ denotes the full state vector from the INS,
$\hat{\bm{x}}^{s}$ the estimated platform position and $\bm{\Sigma}^{s}$ the
error covariance using $\bm{s}_{k}$ by the map matching.

For simplicity, we denote the navigation state at time $k$ as ${\bf
X}_{k}\in\mathbb{R}^{n}$: this typically comprises the components of vehicle
kinematic state (position and velocity) expressed in the geographical
coordinates, vehicle attitude (roll, pitch and yaw), and inertial sensor bias
terms. Based on inertial sensor measurements, the propagation of the
navigation state can be represented by

${\bf X}_{k}={\bf F}_{\rm INS}({\bf X}_{k-1},\bm{\omega}_{b},{\bf f}_{b})+{\bf
w}_{k},$ (1)

where the function ${\bf F}_{\rm INS}(\cdot)$ signifies the mechanization of
INS which involves the prior navigation state ${\bf X}_{k-1}$, the
measurements of accelerometer ${\bf f}_{b}$ and gyroscope $\bm{\omega}_{b}$ at
$k$, respectively (Titterton2004a, ), where ${\bf w}\sim\mathcal{N}(0,\bm{Q})$
accounts for system process noise including the errors from accelerometer and
gyroscope111We have chosen this approach because it is widely used in the
navigation literature (Titterton2004a, ), though the incorporation of
accelerometer and gyroscope readings in this way is fraught with issues and
leads to significant errors. This approach has the advantage of being generic;
a better method would require more realistic platform models. We intend to
discuss this issue in a future publication. $\mathcal{N}({\bf a},\bm{B})$
signifies a Gaussian distribution with mean vector ${\bf a}$ and covariance
matrix $\bm{B}$.

At each aiding update time $k$, the aiding position measurement is coupled
into the navigation state via

$\bm{y}_{k}={\bf H}{\bf X}_{k}+{\bf v}_{k}.$ (2)

where ${\bf H}$ is a constant matrix and ${\bf v}\sim\mathcal{N}(0,\bm{R})$ is
a Gaussian zero-mean noise term modeling the measurement errors.

The INS aiding problem is to find the posterior density $p({\bf
X}_{k}\mid\bm{y}_{1:k})$ based on the sequence of measurements $\bm{y}_{1:k}$
from aiding sources. Recursive Bayesian solution can be written as

$p({\bf X}_{k}\mid\bm{y}_{1:k})=\frac{p(\bm{y}_{k}\mid{\bf X}_{k})p({\bf
X}_{k}\mid\bm{y}_{1:k-1})}{\int p(\bm{y}_{k}\mid{\bf X}_{k})p({\bf
X}_{k}\mid\bm{y}_{1:k-1})d{\bf X}_{k}}.$ (3)

where the predicted density of the navigation state is given by

$p({\bf X}_{k}\mid\bm{y}_{1:k-1})=\int p({\bf X}_{k}\mid{\bf X}_{k-1})p({\bf
X}_{k-1}\mid\bm{y}_{1:k-1})d{\bf X}_{k-1},$ (4)

and the navigation state prediction ${\bf X}_{k}\mid{\bf X}_{k-1}$ is a
nonlinear process via the INS iterations. (4) is calculated by (1) as

$p({\bf X}_{k}\mid\bm{y}_{1:k-1})\approx\mathcal{N}(\hat{{\bf X}}_{k\mid
k-1},\,\bm{\Sigma}_{k\mid k-1})$ (5)

As shown in Fig. 1, at time $k$, the predicted density of the navigation state
(5) is the output of the INS. For position aiding, the likelihood function
$p(\bm{y}_{k}\mid{\bf X}_{k})$ in (3) can be written as

$p(\bm{y}_{k}\mid{\bf X}_{k})=p(\bm{y}_{k}\mid\bm{x}_{k}).$ (6)

where $\bm{x}$ signifies the position components of ${\bf X}$. In the context
of this work, $\bm{y}_{k}$ is the position measurement at $k$ to be obtained
from map matching. The map matching problem is to find the posterior density
of the vehicle position $\bm{x}_{k}$, denoted by
$p(\bm{x}_{k}\mid\bm{y}_{1:k})$, using a TMI geo-referenced map $\mathcal{M}$
based on the magnetometer measurement sequence $\bm{y}_{1:k}$.

Similar to other geophysical data maps, the map matching localisation using
TMI maps is facing the same challenges as we mentioned in Section 1. Next, we
introduce a probabilistic data association method to address the map
measurement ambiguity problem and carry out the “curve fitting” using a batch
based probabilistic multiple hypothesis tracker.

##  4 Probabilistic multiple hypothesis map matching

The probabilistic multiple hypothesis map matching involves probabilistic data
association (PDA) for data mapping from TMI signal domain to vehicle position
domain, and a batch based multiple hypothesis tracking algorithm to
iteratively optimise the estimated vehicle trajectory.

###  4.1 Data mapping via probability data association

Let $s_{k}$ represent the magnetometer measurement at time $k$. The
measurement model is expressed as

$s_{k}=s_{k}^{o}+\nu_{k},$ (7)

where $s_{k}^{o}$ signifies the ground truth value of magnetic intensity
measurement and $\nu_{k}$ is a noise term to cover the errors due to the
imperfect of sensor. In this work, it is assumed to be a Gaussian distributed
random variable, i.e., $\nu\sim\mathcal{N}(0,\sigma^{2})$.

According to (7), we need to consider a set of candidate measurements from a
single measurement $s_{k}$, one of which is the true sensor measurement. Let
$Z_{m}=\\{\bm{z}_{i},\,i=1,\cdots,n\\}$ denote the set of possible measurement
locations on the map corresponding to $s_{k}$. In the meantime, we assume that
at time $k$, the location of magnetometer, which takes the magnetic intensity
measurement $s_{k}$, is a Gaussian random variable with mean $\bm{x}_{k}^{s}$
and covariance matrix $\Sigma^{s}_{k}$. Then, the location of true magnetic
intensity measurement $\bm{z}_{i},\,i=1,\cdots,n$ should satisfy

$(\bm{z}_{i}-\bm{x}^{s})(\bm{\Sigma}^{s})^{-1}(\bm{z}_{i}-\bm{x}^{s})^{\prime}\leq\gamma,$
(8)

where $\gamma$ is a probability threshold, and its value determines an
ellipsoid area on the data map containing the location of the magnetometer
with a certain level of confidence. In this work, we refer to such an area as
a search window.

PDA is a key method to determine where a magnetometer measurement originates
on the geophysical data map in the presence of map measurement ambiguity. Fig.
2 illustrates the data PDA mapping process. As part of the integration
filtering process shown in Fig. 1, the position components of the navigation
state $\bm{X}_{INS}$, denoted by $\bm{x}_{INS}$, serve as the filter predicted
position vector and will be updated using UKF if a position fix from an
external source (e.g. GPS location, map matching position, etc.) is present.
Note that if no external fix is present, the system shown in Fig. 1 is a
standard INS. $\bm{x}_{INS}$ and its covariance $\bm{\Sigma}$ describes a
statistical bound (or uncertainty area) about where the signal $s$ is measured
from the map. Therefore, a finite set of potential locations for signal $s$ on
the map can be obtained via (8).

Figure 2: Illustration of the collection of candidate signal locations
$\\{\bm{z}^{s}_{i},\,i=1,2,\cdots,n\\}$ obtained via (3) and (8) based on
knowledge of predicted vehicle position $\bm{x}_{INS}$ from INS, and sensor
noise level.

The probability weight of each candidate location $\bm{z}_{i}$ is proportional
to the geometric distance between $\bm{z}_{i}$ and the window centre
$\bm{x}^{s}$ (i.e., $\bm{x}_{INS}$). The probability weight can be found as

$w_{i}=\frac{p(\bm{z}_{i}\mid\bm{x}^{s})}{\sum_{j=1}^{n}p(\bm{z}_{j}\mid\bm{x}^{s})},$
(9)

where
$p(\bm{z}_{i}\mid\bm{x}^{s})\sim\mathcal{N}\bigl{(}\bm{z}_{i}-\bm{x}^{s},\,\bm{R}_{i}(\sigma)\bigr{)}$,
and $\bm{R}_{i}(\sigma)$ is the associated variance which is a function of the
signal noise variance, or in other words, signal-to-noise ratio (SNR). Thus,
the PDA solution for the map location on magnetic intensity measurement
$s_{k}$ is the weighted mean:

$\bar{\bm{z}}=\sum_{i=1}^{n}w_{i}\bm{z}_{i}.$ (10)

and the associated weighted variance:

$\bar{\bm{R}}=\sum_{i=1}^{n}w_{i}\left[\bm{R}_{i}(\sigma)+(\bm{z}_{i}-\bar{\bm{z}})(\bm{z}_{i}-\bar{\bm{z}})^{\prime}\right].$
(11)

###  4.2 Characterise map matching quality

The performance of using PDA for magnetometer data mapping can be measured by
the PDA error distance $\varepsilon_{\rm PDA}$, which is defined as the
Euclidian distance between the true magnetometer location222We assume that
$\bm{x}=\bm{x}^{s}$ in the context of this work. Practically, we approximate
the true magnetometer location by the position predicted by INS, i.e.,
$\bm{x}^{s}\approx\bm{x}_{INS}$. and the location estimated via PDA.

$\varepsilon_{\rm PDA}=\|{\bf\hat{x}}_{PDA}-\bm{x}^{s}\|.$ (12)

Note that (12) is a function of the magnetometer noise level $\sigma$, the
location uncertainty of the magnetometer described by $\Sigma^{s}$ and
resolution of the underlying geophysical data map.

Figure 3: The total magnetic intensity map used in the simulation from
TMI_online_map . The platform travel trajectory (yellow line) is superimposed
on the map. The green and red boxes illustrate the data areas considered in
the simulations with results shown in Fig. 4 and Fig. 5, respectively.

Here, we use a simulation result to demonstrate this point. The total magnetic
intensity map used in the simulation is downloaded from TMI_online_map . As
shown in Fig. 3, the actual data grid size is $85\times 85$ metres. The
simulation is carried out in the area surrounded by the green solid line
rectangle. For every sensor noise level, 1000 samples are drawn randomly in
the area, which are treated as the mean of sensor locations. The values of
sensor location covariance $\Sigma^{s}$ and probability threshold $\gamma$ are
chosen such that a search window approximately $6.8\,\mbox{km}^{2}$ is formed
for collecting candidate measurement locations. PDA error distances are then
calculated as a function of sensor noise level $\sigma$ and map grid size.

Figure 4: Comparison of PDA error distances vs. sensor noise levels in the TMI
map at original, 5 times and 10 times downsampled data grids. The result is
averaged from the results drawn in 1000 sample points in the green rectangle
area shown in Fig. 3. For a given resolution, there is a plateau where
increasing magnetometer sensitivity provides negligible improvement in
position resolution.

Fig. 4 shows the plot of PDA error distances versus sensor noise levels with
original TMI map and the TMI maps of downsamped 5 and 10 times, respectively.
The plots show that 1) map matching localisation error can be reduced if a
magnetometer of low noise is used; 2) for this given map, about an accuracy of
150 metres may be achieved if a magnetometer of noise level 0.01 nT is used;
3) the map matching localisation error will increase if a TMI map of lower
data grid is used, or if the magnetometer measurement noise level increases.

We use Map Feature Variability (MFV) as a measure of data variation sparsity
of the geophysical data map. The MFV at a data point $i$ on a data map is
defined as

$\mathcal{C}_{i}=\frac{1}{n}\sum_{j}^{n}(s_{x_{i}}-s_{x_{j}})^{2}\hskip
8.5359pt\forall\,\bm{x}_{j}\in\mbox{search
window},\,\bm{x}_{j}\neq\,\bm{x}_{i}.$ (13)

Fig. 5(b) shows an example of the normalised map feature variability over the
map area shown inside the red rectangle in Fig. 3. For reference, we also show
the original TMI map in Fig. 3(a), and the PDA error distance maps for sensor
noise levels $\sigma=0.015$ nT and $0.15$ nT in Fig. 3(c) and (d),
respectively.

Figure 5: TMI map quality analysis in the red rectangle area shown in Fig. 3.
(a) TMI map. (b) Map feature variability. (c) PDA error distance for
$\sigma=0.015$ nT. (d) PDA error distance for $\sigma=0.15$ nT.

From map matching localisation view point, MFV provides a local measure on map
data variation sparsity. In a map matching based INS aiding, the value of
$C_{i}^{-1}$ may be used to weight the estimated sensor location covariance to
provide additional parameter that locally describes quality of the data map
used.

###  4.3 Map matching localisation

We use a batch form Bayesian filter for map matching localisation from a
sequence of magnetometer measurements. As shown in Fig. 1, the filter
estimated posterior probability density of vehicle/sensor location
$\mathcal{N}(\hat{\bm{x}}_{k}^{s},\Sigma_{k}^{s})$ based on a batch of $m$
magnetometer measurements $\\{s_{k-m+1},\cdots,s_{k}\\}$ and a set of prior of
the vehicle location $\\{(\hat{\bm{x}},\Sigma)_{k-m+1\mid
k-m},\cdots,(\hat{\bm{x}},\Sigma)_{k\mid k-1}\\}$.

Two algorithms for such a filter may be used for this work, one is the
probabilistic multiple hypothesis tracker based map matching (PMHT-MM)
proposed in (Wang2022, ) and the other is the Veterbi map matching algorithm
proposed in (Li2022, ). Both algorithms work under a stochastic optimisation
procedure which iteratively estimates the current vehicle location from a
batch of measurements under vehicle dynamic constraints and are shown robust
and accurate for INS navigation aiding with gravitational field data.

##  5 Navigation experiment

The simulation scenario is a constant velocity vehicle traveling along the
surface of the earth at a fixed height of 100 m from
$[-38^{\circ},144.5^{\circ}]$ to $[-35^{\circ},150^{\circ}]$ (i.e., from
Melbourne area to Sydney area) and at a ground speed of 22 m/s. The entire
journey takes more than 3.6 hours and navigation is conducted by an onboard
INS in GNSS denied environment. The inertial sensors (both accelerometer and
gyroscope) used in the INS are precision grade with errors specified in Table
1, and they take measurement at the frequency of 1 Hz and are assumed to be
well calibrated before the gurney starts.

Table 1: Bias and noise ranges of inertial sensors in the simulation according to 2005:Christopher_Jekeli . Sensor Grade | Sensor Type | Bias $b$ | White Noise $\sigma$
---|---|---|---
Precision (PC) | Accel. horiz. | $2\times 10^{-6}m/s^{2}$ | $8\times 10^{-5}m/s^{2}/\sqrt{Hz}$
| Accel. Vert. | $2.5\times 10^{-8}m/s^{2}$ | $1.6\times 10^{-6}m/s^{2}/\sqrt{Hz}$
| Gyro. horiz. | $2\times 10^{-5}deg/h$ | $1\times 10^{-3}deg/h/\sqrt{Hz}$
| Gyro. vert. | $1\times 10^{-3}deg/h$ | $3\times 10^{-2}deg/h/\sqrt{Hz}$

Figure 6: Illustration of the INS aiding timing in the simulation. We assume
that measurement from inertial sensors is sampled at frequency 1 Hz and the
magnetometer measurement is taken at an interval of every 10 seconds. Each
time, the number of magnetometer measurements for PMHT-MM filtering are 30,
which means that the actual aiding interval is 300 seconds.

We assume that a low noise magnetometer is onboard to take magnetic intensity
measurement at an interval of every 10 seconds. The PMHT-MM algorithm works
with a batch of 30 magnetic intensity measurements at a time to estimate the
vehicle current location and the estimated location is then used to update the
navigation state via the UKF integration filter. This results in an aiding
interval 300 seconds. Fig. 6 illustrates the actual timing of data sampling in
the simulation.

As we have shown in Fig. 3, a TMI map, which covers the area of southeast
Australia, is obtained online from Geoscience Australia (TMI_online_map, ) and
is used in the simulation of map matching aided INS. The vehicle travel
trajectory is shown by yellow solid line on the map.

In this experiment, two noise levels for the magnetometer are considered: 1)
$\sigma=0.0015$ nT, which is to model a magnetometer of very high precision;
2) $\sigma=0.15$ nT, which represents the level of sensitivity of
magnetometers that are commercially available. We plot the vehicle root-mean-
squared (RMS) position errors in Fig. 7 along with case of INS-only without
aiding. The results were averaged from 100 Monte Carlo runs for each of cases.

Figure 7: Comparison of RMS position errors of the aided INS for magnetometer
measurement noise levels 0.015 nT (red) and 0.15 nT (blue), along with INS-
only case (black). Note that the position error of INS-only case grows with
time as a result of the inertial sensor bias, mainly from gyroscope,
accumulating over time and its oscillation at a period approximately 1.4 hours
is due to the Schuler pendulum effect occurring when navigation on a rotating
frame Titterton2004a .

The simulation results in Fig. 7 show that

  * •

when the noise level of magnetometer is 0,0015 nT, the INS with magnetometery
aiding can achieve an average of RMS position error of 250 metres; the RMS
position error will double if the noise level is 10 times larger at 0.15 nT.

  * •

we observed that the magnetometery INS aiding is robust with 100% success rate
with a batch length (i.e., the number of magnetometer measurements to be
processed in a batch) 30 at each time in this simulation. If a lower batch
length or a high noise level magnetometer is used, the RMS position error will
increases and the magnetometery INS aiding will not be completely reliable.

  * •

the magnetometery INS aiding is able to remove the position drift, as
indicated by the INS-only case, which is accumulated over time due to the
imperfection of inertial sensors.

##  6 Conclusions

In this paper, a probabilistic method for map matching localisation based on
magnetometery measurement and total magnetic intensity maps is described. We
show that the method is able to effectively address the challenge issues
associated with map matching using geophysical maps and provides a mechanism
of handling map measurement ambiguity and a way of evaluating the underlying
quality. Furthermore, the effectiveness of the magnetometery map matching
localisation is demonstrated using the simulation of removing position drift
of an inertial navigation system, that arises in INS over a long duration, by
the magnetometery aiding in the absence GNSS positioning. Simulation results
using online maps verified the robustness and effectiveness of the proposed
algorithm, particularly, the aiding precision will be getting better if a high
sensitivity magnetometer is used.

Acknowledgments This work was funded by the Department of Defence Australia
through the Next Generation Technology Fund.

## References

  *   * (1) Degen CL, Reinhard F, Cappellaro P.  Quantum sensing.  Rev Mod Phys. 2017 Jul;89:035002.  10.1103/RevModPhys.89.035002.
  * (2) Giovannetti V, Lloyd S, Maccone L.  Quantum-Enhanced Measurements: Beating the Standard Quantum Limit.  Science. 2004 nov;306(5700):1330–1336.  10.1126/science.1104149.
  * (3) Porrati F, Barth S, Sachser R, Dobrovolskiy OV, Seybert A, Frangakis AS, et al.  Crystalline Niobium Carbide Superconducting Nanowires Prepared by Focused Ion Beam Direct Writing.  ACS Nano. 2019 may;13(6):6287–6296.  10.1021/acsnano.9b00059.
  * (4) Knappe S, Velichansky V, Robinson HG, Liew L, Moreland J, Kitching J, et al.  Atomic vapor cells for miniature frequency references.  In: IEEE International Frequency Control Sympposium and PDA Exhibition Jointly with the 17th European Frequency and Time Forum, 2003. Proceedings of the 2003. Tampa, FL, USA: IEEE; 2003. .
  * (5) Taylor JM, Cappellaro P, Childress L, Jiang L, Budker D, Hemmer PR, et al.  High-sensitivity diamond magnetometer with nanoscale resolution.  Nature Physics. 2008 sep;4(10):810–816.  10.1038/nphys1075.
  * (6) Barry JF, Schloss JM, Bauch E, Turner MJ, Hart CA, Pham LM, et al.  Sensitivity optimization for NV-diamond magnetometry.  Rev Mod Phys. 2020 Mar;92:015004.  10.1103/RevModPhys.92.015004.
  * (7) Schloss JM, Barry JF, Turner MJ, Walsworth RL.  Simultaneous Broadband Vector Magnetometry Using Solid-State Spins.  Phys Rev Applied. 2018 Sep;10:034044.  10.1103/PhysRevApplied.10.034044.
  * (8) Wolf T, Neumann P, Nakamura K, Sumiya H, Ohshima T, Isoya J, et al.  Subpicotesla Diamond Magnetometry.  Phys Rev X. 2015 Oct;5:041001.  10.1103/PhysRevX.5.041001.
  * (9) Titterton DH, Weston JL.  Strapdown Inertial Navigation Technology. 2nd.  London: The Institution of Electrical Engineers; 2004.
  * (10) Groves P.  Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems.  Boston: Artech House; 2013.
  * (11) Tuohy ST, Patrikalakis NM, Leonard JJ, Bellingham JG, Chryssostomidis C.  Map Based Navigation For Autonomous Underwater Vehicles.  International Journal of Offshore and Polar Engineering. 1996 03;6(01).  https://onepetro.org/IJOPE/article-pdf/2188316/isope-96-06-1-009.pdf.
  * (12) Kamgar-Parsi B, Kamgar-Parsi B.  Vehicle localization on gravity maps.  In: Gerhart GR, Gunderson RW, Shoemaker CM, editors. Unmanned Ground Vehicle Technology. vol. 3693. Orlando, FL, United States: SPIE; 1999. .
  * (13) Han Y, Wang B, Deng Z, Fu M.  A Combined Matching Algorithm for Underwater Gravity-Aided Navigation.  IEEE/ASME Transactions on Mechatronics. 2018 Feb;23(1):233–241.  10.1109/tmech.2017.2774296.
  * (14) Wang B, Ma Z, Huang L, Deng Z, Fu M.  A Filtered-Marine Map-Based Matching Method for Gravity-Aided Navigation of Underwater Vehicles.  IEEE/ASME Transactions on Mechatronics. 2022;p. 1–11.  10.1109/tmech.2022.3159596.
  * (15) Hostetler L, Andreas R.  Nonlinear Kalman filtering techniques for terrain-aided navigation.  IEEE Transactions on Automatic Control. 1983 mar;28(3):315–323.  10.1109/tac.1983.1103232.
  * (16) Wang X, Gilliam C, Kealy A, Close JE, Moran B.  Probabilistic Map Matching for Robust Inertial Navigation Aiding.  ArXiv. 2022;abs/2203.16932.
  * (17) Li W, Gilliam C, Wang X, Kealy A, Greentree AD, Moran B.  Gravity aided navigation using Viterbi map matching algorithm.  arXiv: http://arxivorg/abs/220410492v1. 2022;http://arxiv.org/abs/2204.10492v1.
  * (18) Doherty MW, Manson NB, Delaney P, Jelezko F, Wrachtrup J, Hollenberg LCL.  The nitrogen-vacancy colour centre in diamond.  Physics Reports. 2013;528(1):1–45.  The nitrogen-vacancy colour centre in diamond. https://doi.org/10.1016/j.physrep.2013.02.001.
  * (19) Balasubramanian G, Neumann P, Twitchen D, Markham M, Kolesov R, Mizuochi N, et al.  Ultralong spin coherence time in isotopically engineered diamond.  Nature Materials. 2009 May;8(5):383–387.  10.1038/nmat2420.
  * (20) Ruan Y, Simpson DA, Jeske J, Ebendorff-Heidepriem H, Lau DWM, Ji H, et al.  Magnetically sensitive nanodiamond-doped tellurite glass fibers.  Scientific Reports. 2018 Jan;8(1):1268.  10.1038/s41598-018-19400-3.
  * (21) Bai D, Huynh MH, Simpson DA, Reineck P, Vahid SA, Greentree AD, et al.  Fluorescent diamond microparticle doped glass fiber for magnetic field sensing.  APL Materials. 2020;8(8):081102.  10.1063/5.0013473.  https://doi.org/10.1063/5.0013473.
  * (22) Filipkowski A, Mrózek M, St$\mbox{\rm\bf d}_{e}$pniewski G, Kierdaszuk J, Drabińska A, Karpate T, et al.  Volumetric incorporation of NV diamond emitters in nanostructured F2 glass magneto-optical fiber probes.  Carbon. 2022;196:10–19.  https://doi.org/10.1016/j.carbon.2022.04.024.
  * (23) Shields BJ, Unterreithmeier QP, de Leon NP, Park H, Lukin MD.  Efficient Readout of a Single Spin State in Diamond via Spin-to-Charge Conversion.  Phys Rev Lett. 2015 Mar;114:136402.  10.1103/PhysRevLett.114.136402.
  * (24) Bourgeois E, Jarmola A, Siyushev P, Gulka M, Hruby J, Jelezko F, et al.  Photoelectric detection of electron spin resonance of nitrogen-vacancy centres in diamond.  Nature Communications. 2015 Oct;6(1):8577.  10.1038/ncomms9577.
  * (25) Jeske J, Cole JH, Greentree AD.  Laser threshold magnetometry.  New Journal of Physics. 2016 Jan;18(1):013015.  10.1088/1367-2630/18/1/013015.
  * (26) Dumeige Y, Roch JF, Bretenaker F, Debuisschert T, Acosta V, Becher C, et al.  Infrared laser threshold magnetometry with a NV doped diamond intracavity etalon.  Opt Express. 2019 Jan;27(2):1706–1717.  10.1364/OE.27.001706.
  * (27) Hahl FA, Lindner L, Vidal X, Luo T, Ohshima T, Onoda S, et al.  Magnetic-field-dependent stimulated emission from nitrogen-vacancy centers in diamond.  Science Advances. 2022;8(22):eabn7192.  10.1126/sciadv.abn7192.  https://www.science.org/doi/pdf/10.1126/sciadv.abn7192.
  * (28) Australia G.: Geophysical Archive Data Delivery System.  https://portal.ga.gov.au/persona/gadds.
  * (29) Jekeli C.  Navigation Error Analysis of Atom Interferometer Inertial Sensor.  Navigation. 2005;52(1):1–14.  10.1002/j.2161-4296.2005.tb01726.x.