The Vector Matching Method in Geomagnetic Aiding Navigation

Abstract

In this paper, a geomagnetic matching navigation method that utilizes the geomagnetic vector is developed, which can greatly improve the matching probability and positioning precision, even when the geomagnetic entropy information in the matching region is small or the geomagnetic contour line’s variety is obscure. The vector iterative closest contour point (VICCP) algorithm that is proposed here has better adaptability with the positioning error characteristics of the inertial navigation system (INS), where the rigid transformation in ordinary ICCP is replaced with affine transformation. In a subsequent step, a geomagnetic vector information fusion algorithm based on Bayesian statistical analysis is introduced into VICCP to improve matching performance further. Simulations based on the actual geomagnetic reference map have been performed for the validation of the proposed algorithm.

1. Introduction

Geomagnetic matching is a key aiding navigation technology that can rectify the indication trace of the Inertial Navigation System (INS) by comparing the geomagnetic profile acquired on board with the stored geomagnetic map, which is an ideal autonomous navigation candidate for long range Unmanned Aerial Vehicle (UAV) application due to its merits such as the all-weather, whole-day working consistentency [1,2].

There is a kind of scalar matching method often used [3,4,5,6,7,8,9,10,11], which minimizes the square of the differences between the norms of magnetometer outputs and the magnitude of stored geomagnetic reference field in correcting the indication trace. Those methods convert the matching problem into a state estimation problem by using datasets collected in the candidate matching region, most of which are not feasible for long time and long range application because the selection of matching region along the expected trace with high adaptability is difficult, and the estimated algorithm might diverge because of insignificant geomagnetic characteristics [3]. At present, there are three types of scalar geomagnetic matching algorithms: the Extended Kalman Filter (EKF) based algorithm [4,5,6], the Correlation Matching (CM) algorithm [7,8,9] and intelligent algorithm [10]. The conventional EKF and CM based algorithm usually lead to the local optimum solutions, depending on the geomagnetic entropy in the matching region and the matching probability and positioning precision will reduce a lot. The intelligent algorithm generally needs a large number of sample data sets for training, which will result in an inconsistent solution with the small matching region. Recent work in [11] developed a particle filter algorithm based the measurement of static H-field that can only be applied to ground vehicles due to the long time preparation of local magnetic field maps. Due to the development of a low cost, light weight three-axis magnetoresistive magnetometer and flux-gate magnetometer, the onboard measurement accuracy of the geomagnetic vector field has been improved to the order of nanotesla in recent years [12,13]. Therefore, a vector geomagnetic matching method that utilizes the multidimensional geomagnetic element is developed, which aims at obtaining relatively stable and precise positioning performance with geomagnetic terrain uncertainty.

This paper presents a significant improvement upon previous methods which is based on the geomagnetic field vector measurement. The affine transformation, which is often used in machine vision, is adopted to ICCP for the first time to deal with the scale error of INS. The error models of geomagnetic field vector measurement and INS combined effect of all linear time-invariant distortions will firstly be described. Then, the vector matching algorithm, namely Vector Iterative Closest Contour Point (VICCP), is adopted to estimate the trace with an accuracy and robustness solution. Finally, we will present the simulation results for the validation of the proposed algorithms.

2. The Error Model of Geomagnetic Vector Matching System

2.1. Geomagnetic Field Vector Measurement Error

The error model of geomagnetic field vector measurement combines the effect of all linear time-invariant distortions, which can be described as follows [14]:

$$h_M=C_s^e\left(C_O{C}_S\left(D_{SI}{h}_E+D_{HI}\right)+b_S\right)+n_S$$

(1)

where $h_E(3\times 1)$ is the error-free geomagnetic field in the sensor frame, and whereas $h_M(3\times 1)$ is the readings from the triad of magnetometer in the earth frame. The triangular matrix $C_O(3\times 3)$ stands for the nonorthogonality of three-axis magnetometer. The diagonal matrix $C_S(3\times 3)$ is scale factor and $b_s(3\times 1)$ is the zero offset of the magnetometer. $D_{HI}(3\times 1)$ and $D_{SI}(3\times 3)$ stand for the hard magnetic iron and soft magnetic iron interference respectively. $C_s^e(3\times 3)$ is external attitude information of the vehicle. $n_S$ is Gaussian noise following ~N(0, ${\sigma }_n^2$). After introducing matrix $C_M(3\times 3)$ and vector $d_M(3\times 1)$, the error model can be simplified as:

$$h_M=C_M{h}_E+d_M+n_S$$

(2)

where

$$\{\begin{array}{l}{C}_M=C_s^e{C}_O{C}_S{D}_{SI}\\ d_M=C_s^e\left(C_O{C}_S{D}_{HI}+b_S\right)\end{array}$$

(3)

$C_M$ and $d_M$ are a linear combination of above-mentioned errors, which can be determined prior to its application or by onboard calibration.

2.2. Error Model of INS

Since an INS is available in this paper, we will use an INS error propagation model given as follows [15,16].

$$\dot{x}(t)=F_{INS}x(t)+w(t)$$

(4)

omitting the time variable t, then $x={[\begin{array}{ccccccc}{(\varphi )}^T& {(\delta v^n)}^T& {(\delta p)}^T& {({\epsilon }^b)}^T& {({\nabla }^b)}^T& {(\delta K_g)}^T& {(\delta K_a)}^T\end{array}]}^T,$ where platform misalignment angles $\varphi ={\left[\begin{array}{ccc}{\varphi }_E& {\varphi }_N& {\varphi }_U\end{array}\right]}^T$, velocity errors $\delta v^n={\left[\begin{array}{ccc}\delta v_E^n& \delta v_N^n& \delta v_U^n\end{array}\right]}^T$ and $\delta p={\left[\begin{array}{ccc}\delta L& \delta \lambda & \delta h\end{array}\right]}^T$ with $\delta L,\delta \lambda ,\delta h$ represent latitude, longitude and altitude errors respectively. ${\epsilon }^b={\left[\begin{array}{ccc}{\epsilon }_x^b& {\epsilon }_y^b& {\epsilon }_z^b\end{array}\right]}^T$ is gyro drift errors expressed in body frame and ${\nabla }^b={\left[\begin{array}{ccc}{\nabla }_x^b& {\nabla }_y^b& {\nabla }_z^b\end{array}\right]}^T$ is accelerometer biases expressed in body frame. $\delta K_g$ can be represented as: $\delta K_g={\left[\begin{array}{ccccccccc}\delta k_{gxx}& \delta k_{gyx}& \delta k_{gzx}& \delta k_{gxy}& \delta k_{gyy}& \delta k_{gzy}& \delta k_{gxz}& \delta k_{gyz}& \delta k_{gzz}\end{array}\right]}^T$, where $\delta k_{gii},(i=x,y,z)$ are gyro scale factor errors and $\delta k_{gij},(i,j=x,y,z,i\ne j)$ are gyro actual axis misalignment angles with respect to ideal body frame axis. $\delta K_a$ can be represented as: $\delta K_a={\left[\begin{array}{cccccc}\delta k_{axx}& \delta k_{ayx}& \delta k_{azx}& \delta k_{ayy}& \delta k_{azy}& \delta k_{azz}\end{array}\right]}^T$, $\delta k_{aii},(i=x,y,z)$ are accelerometer scale factor errors and $\delta k_{aij},(i,j=x,y,z,i\ne j)$ are accelerometer actual axis misalignment angles with respect to ideal body frame axis. The components ${\epsilon }^b$, ${\nabla }^b$, $\delta K_g$, $\delta K_a$ are all assumed to be constant vectors. $w(t)$ are white Gaussian noise sources including the first-order Gauss-Markov bias process for the accelerometer and gyro.

The $F_{INS}$ in Equation (4) is expressed as:

$$F_{INS}=\left[\begin{array}{ccccccc}{M}_{aa}& M_{av}& M_{ap}& -C_b^n& 0_{3\times 3}& M_{ag}& 0_{3\times 6}\\ M_{va}& M_{vv}& M_{vp}& 0_{3\times 3}& C_b^n& 0_{3\times 9}& M_{vf}\\ 0_{3\times 3}& M_{pv}& M_{pp}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 9}& 0_{3\times 6}\\ & & & 0_{21\times 30}& & & \end{array}\right]$$

(5)

The specific formulae of $F_{INS}$ are given in Appendix A.

According to the error model of INS, the positioning errors should grow at least quadratically over time. However, the matching algorithm usually executes once by using a dozen positioning points acquired within a very short period of time or in a small region, so that the INS indicated as trace can be regard as a linear transformation from the true trace during this short period. Then the relationship between indication trace $trac{e}_i$ and real trace $trac{e}_t$ can be described as Equation (6) in the matching region.

$$trac{e}_i=f_S(f_R(trac{e}_t+{\delta }_T))$$

(6)

where $f_S$ is the scaling factor, $f_R$ is the rotating factor, and ${\delta }_T$ is the translation.

3. Geomagnetic Matching Algorithm

3.1. The Principle of ICCP Algorithm

The scenario for a UAV using the geomagnetic field to be located is such that the target starts from a position and travels along a 3D trace. Because that the altitude information of UAV can usually be given with high accuracy altimeter, Geomagnetic matching method only rectifies the indication trace of INS with 2D location points, as shown in Figure 1. Let us denote the indicated trace by $\left\{H_i\right\}$ $(i=1,2,\cdots ,N)$ (N is the total number of points in matching region), which will be different from the actual trace $\left\{L_i\right\}$ $(i=1,2,\cdots ,N)$ due to errors and drifts in instruments and random external effects referring to Equation (6). At the same time, the magnetic sensor provides the corresponding measured total geomagnetic intensity $\left\{g_i\right\}$ $(i=1,2,\cdots ,N)$, and $\left\{C_i\right\}$ $(i=1,2,\cdots ,N)$ is the corresponding set of geomagnetic field contour. If the actual trace $\left\{X_i\right\}$ $(i=1,2,\cdots ,N)$ is known, the navigational errors can be corrected by a rigid transformation of the indicated trace into the actual trace. The rigid transformation T minimizes the distance between the sets $\left\{H_i\right\}$ and $\left\{X_i\right\}$ [17].

$$Min E_j={\displaystyle \sum _{i=1}^{N}di{s}_j(X_i-X_{i-1},H_i-H_{i-1})}+T{\displaystyle \sum _{i=1}^{N}di{s}_j(X_i,C_i)}$$

(7)

where $dis\left(X,H\right)={\Vert X-H\Vert }^2$. The extraction of the closest contour point is discussed in [16]. The minimization process of $E_j$ is iterated so that in the j-th iteration the set of measured points is $H^j=T{H}^{j-1}$ and the set of points $X^j$ is determined from the new points $H^j$. The iteration process is continued until T becomes negligible.

The rigid transformation T consists of the rotation matrix and the translation vector as shown in Equation (2), which can be solved with the quaternion method.

$$H_{i+1}=\left(\begin{array}{l}{t}_x\\ t_y\end{array}\right)+\left(\begin{array}{ll}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right)H_i$$

(8)

where $\theta$ refers to the rotation angle. $t_x$ and $t_y$ are translation.

3.2. VICCP Algorithm

In order to make full use of geomagnetic vector information, the rule of approaching of indicated trace toward closest contour points should be adjusted because there are three contours for the geomagnetic vector. We define an overall match error that must be minimized with respect to all $\left\{H_i\right\}$:

$$\begin{array}{ll}{E}_j& ={\displaystyle \sum _{i=1}^N{d}_j\left(X_i-X_{i-1},H_i-H_{i-1}\right)}\\ & +T_A{\displaystyle \sum _{k=1}^3{\displaystyle \sum _{i=1}^N{\lambda }_k{d}_j\left(X_i,\Omega \left(C_{i,k}\right)\right)/{\displaystyle \sum _{i=1}^N{\lambda }_k}}}\end{array}$$

(9)

where ${\lambda }_k$ is weighting coefficient, taking ${\lambda }_k=1$ without a priori knowledge. $\left\{C_{i,k}\right\}$ stands for the contour of each geomagnetic element. If the error model parameter of three-axis magnetometer is pre-determined, the geomagnetic vector can be corrected by inversing the measurement model with Equation (2).

$$h_E=C_M^{-1}\left(h_M-d_M\right)$$

(10)

Then define corresponding contours extracted with the corrected geomagnetic vector as $\Omega \left(C_{i,k}\right)$.

Since we introduce more geomagnetic information in ICCP, a more complicated transformation can be adopted to improve the adaptability with the error characteristics of INS described with Equation (6), and without causing the divergence of the algorithm. The simplified affine transformation T_A is used in this paper to solve the problem that traditional ICCP cannot reduce the scaling error of indication track in INS. Defining rotation matrix R, translation t and scale factor S, the matching process of the indication track sequence $H_i$ to the real track sequence $L_i$ of each iterative can be performed with Equation (11).

$$L_i=SR{H}_i+t$$

(11)

The least squares solution of this transformation can be given by solving the Procrustes problem [18]. The cost function can be written as:

$$\begin{array}{cc}f{\left(S,R,t\right)}_i& ={\displaystyle \sum _{i=1}^n{\Vert L_i-\left(SR{H}_i+t\right)\Vert }^2}\hfill \\ & =n{\Vert L_0-\left(SR{H}_0+t\right)\Vert }^2+{\displaystyle \sum _{i=1}^n{\Vert L_i^{\prime }-\left(SR{H}_i^{\prime }+t\right)\Vert }^2}\hfill \end{array}$$

(12)

where $L_0$ and $H_0$ denote the centroids of $L_i$ and $H_i$, $L_i^{\prime }$ and $H_i^{\prime }$ is the relative vectors relative to their centroids. Then normalize $L_i$ and $H_i$:

$$L_{s,i}=L_i/\sqrt{{\displaystyle \sum L_0^2}},\hspace{1em}{H}_{s,i}=H_i/\sqrt{{\displaystyle \sum H_0^2}}$$

(13)

Define the matrix $A=L_s{H}_s^T$, where L_s = [L_s,1 L_s,2…L_s,n] and H_s = [H_s,1 H_s,2…H_s,n]. The least squares estimate of the rotation matrix R is Equation (14) when A is nonsingular.

$$R=A{\left(A^{T}A\right)}^{-1/2}$$

(14)

In terms of the singular value decomposition of A as UΛV^T, the optimal rotation matrix R can be expressed as:

$$R=V\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& \det \left(U\right)\det \left(V\right)\end{array}\right]U^T$$

(15)

The scale factor S is

$$S=tr(\Lambda )\cdot \sqrt{{\displaystyle \sum L_0^2}}/\sqrt{{\displaystyle \sum H_0^2}}$$

(16)

Then minimize the first term contribution in Equation (12) by

$$t=L_0-SR{H}_0$$

(17)

4. Simulation

The simulation flow is as shown in Figure 2, and the effectiveness of the proposed matching method can be examined by this numerical simulation. The reference matching maps are generated with Enhanced Magnetic Model (EMM 2015) of order 720, and disturbed by a certain level of Gaussian noise. The simulated magnetic vector data are acquired based on the geomagnetic reference field and disturbed by inherent sensor errors and external interferences, with key parameters shown in

Table 1. The parameters of simulation.

Parameter	Parameter Values
Matching points	20
Reference map noise	~(0, 25) nT
Matching region size	8 km × 10 km
Center location of matching region	(108.98° E, 34.91° N)
Average altitude	~20 km
Gird size of matching region	100 × 80
Measurement Gaussian noise	~10 nT/axis
Matching tolerance	200% of uncorrected trace’s mean error

. The INS simulation in this paper is carried out for a route under the condition shown in

Table 2. Simulation condition of INS.

Symbol	Quantity	Unit
Platform misalignment angles	30	″
Initial velocity error	0.1	m/s
Initial position error	10	m
Gyro constant bias	0.01	°/h
Gyro random walk	0.001	°/h
Accelerometor constant bias	100	μg
Accelerometor random walk	10	μg
Scale factor error	10	ppm
Askew installation error	10	″

. The condition includes initial errors and measurement error. Note that this simulation only reveals the geomagnetic aiding process in the matching region, where the INS already has a large deviation before entering into the match regions.

4.1. The Comparison of ICCP and VICCP

Simulation results of introducing geomagnetic vector into VICCP algorithm shows that the vector matching method has better positioning precision and matching probability than the traditional ICCP when the indication trace of INS does not contain scale error, as shown in Figure 3 and

Table 3. Comparison of ICCP and VICCP with 100 times Monte Carlo simulation.

Statistical Quantity	Indication Trace of INS	VICCP with Rigid Transformation	VICCP with Affine Transformation
Mean (m)	679.05	267.66	127.4
Var (m)	530.49	265.65	483.8
Matching probability (percentage)	-	93%	98%

.

The matching probability means the percentage of average positioning errors less than the matching tolerance (200% of indication trace’s mean error, as shown in Table 1) in the Monte Carlo simulation. Actually, whenever the matching procedure fails, a considerable matching error will be generated. As shown in Figure 4, the mean positioning is around 5 km when the ICCP matching procedure fails. So the statistical quantities of positioning precision are given in the Tables below only when the matching procedure is successful.

4.2. The Validity of Affine Transformation Based VICCP

Simulation results in Figure 5 and

Table 4. Comparison of VICCP with different transformation for 100 times Monte Carlo simulation.

Statistical Quantity	Indication Trace of INS	VICCP with Rigid Transformation	VICCP with Affine Transformation
Mean (m)	1225.14	600.20	153.16
Var (m)	1726.78	1142.51	50.07
Matching probability (percentage)	-	90%	98%

show that when scale error exists, the traditional ICCP algorithm becomes invalid, and the positioning accuracy and matching probability have been significantly reduced. In addition, the affine transformation based VICCP increases the adaptability with positioning error characteristics of INS, and achieves positioning accuracy of less than 200 m instead.

In Figure 6, the effectiveness of proposed algorithm is evaluated with relative flat geomagnetic terrain, and the simulation results demonstrate the robustness of the proposed method. The geomagnetic entropy calculated with Equation (18) is much smaller than the matching region utilized in Figure 3.

$$H_X=-{\displaystyle \sum _{i=1}^N{p}_j\log p_i}, p_i=\left|h_i\right|/{\displaystyle \sum _{i=1}^N{h}_i}$$

(18)

where $h_i$ is the intensity of geomagnetic anomaly field.

4.3. The Comparison of VICCP and Bayesian Based VICCP

In order to further improve matching performance of vector matching method, a priori knowledge is required as a support. An improved algorithm based on the Bayesian statistical analysis can be derived with matching error expressed as:

$$\begin{array}{cc}{E}_j& ={\displaystyle \sum _{i=1}^N{d}_j\left(X_i-X_{i-1},H_i-H_{i-1}\right)}\hfill \\ & +T_A{\displaystyle \sum _{k=1}^3{\displaystyle \sum _{i=1}^N{p}_k{d}_j\left(X_i,\Omega \left(C_{i,k}\right)\right)/{\displaystyle \sum _{i=1}^N{p}_k}}}\hfill \end{array}$$

(19)

where $p_k$ is prior matching probability of each single element, which can obtain through experiment, and then $E_j$ in Equation (7) can be replaced with Equation (19). The Simulation result in Figure 7 and

Table 5. Evaluation of the performance of the Bayesian based algorithm with 100 times Monte Carlo simulation.

Matching Method		Mean (m)	Var (m)	Matching Probability (percentage)
ICCP	X	235.37	240.28	88%
	Y	531.31	662.51	64%
	Z	185.42	201.27	92%
VICCP		124.72	51.05	97%
Bayesian-based VICCP		88.36	34.96	99%

show that the Bayesian-based algorithm will further improve the positioning accuracy and matching probability.

5. Conclusions

This paper proposed a geomagnetic matching method that makes full use of the geomagnetic vector information to improve accuracy and robustness. The achievable accuracy limits for the traditional matching algorithm were discussed, the affine transformation was adopted to ICCP for the first time to increase adaptability with positioning error characteristics of INS, and the best achievable positioning accuracy for geomagnetic matching error was less than 200 m in case of existing scale error in the indication trace. For better results, it is necessary to use prior knowledge such as matching probability of each single geomagnetic element, and achieves positioning error less than 100 m. In future work, we plan to extend this work to multi matching regions during a complete long range trajectory, a specific INS error model will be introduced rather than using a rough indication trace error model, and we will try to achieve 3D geomagnetic matching without altimeter aiding.