The Vector Matching Method in Geomagnetic Aiding Navigation

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.


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 Sensors 2016, 16, 1120; doi:10.3390/s16071120 www.mdpi.com/journal/sensors Sensors 2016, 16, 1120 2 of 12 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.

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]: where h E p3ˆ1q is the error-free geomagnetic field in the sensor frame, and whereas h M p3ˆ1q is the readings from the triad of magnetometer in the earth frame. The triangular matrix C O p3ˆ3q stands for the nonorthogonality of three-axis magnetometer. The diagonal matrix C S p3ˆ3q is scale factor and b s p3ˆ1q is the zero offset of the magnetometer. D H I p3ˆ1q and D SI p3ˆ3q stand for the hard magnetic iron and soft magnetic iron interference respectively. C e s p3ˆ3q is external attitude information of the vehicle. n S is Gaussian noise following~N(0, σ 2 n ). After introducing matrix C M p3ˆ3q and vector d M p3ˆ1q, the error model can be simplified as: 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.

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].
. xptq " F I NS xptq`wptq (4) omitting the time variable t, then x " r pφq T pδv n q T pδpq T pε b q where platform misalignment angles φ " δv n E δv n N δv n U ı T and δp " " δL δλ δh ı T with δL, δλ, δh represent latitude, longitude and altitude errors respectively. ε b " is accelerometer biases expressed in body frame. δK g can be represented as: δK g " " δk gxx δk gyx δk gzx δk gxy δk gyy δk gzy δk gxz δk gyz δk gzz ı T , where δk gii , pi " x, y, zq are gyro scale factor errors and δk gij , pi, j " x, y, z, i ‰ jq are gyro actual axis misalignment angles with respect to ideal body frame axis. δK a can be represented as: δk axx δk ayx δk azx δk ayy δk azy δk azz ı T , δk aii , pi " x, y, zq are accelerometer scale factor errors and δk aij , pi, j " x, y, z, i ‰ jq are accelerometer actual axis misalignment angles with respect to ideal body frame axis. The components ε b , ∇ b , δK g , δK a are all assumed to be constant vectors. wptq are white Gaussian noise sources including the first-order Gauss-Markov bias process for the accelerometer and gyro.
The F I NS in Equation (4) is expressed as: The specific formulae of F I NS are given in Appendix 5.
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 trace i and real trace trace t can be described as Equation (6) in the matching region. trace where f S is the scaling factor, f R is the rotating factor, and δ T is the translation.

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 tH i u pi " 1, 2,¨¨¨, Nq (N is the total number of points in matching region), which will be different from the actual trace tL i u pi " 1, 2,¨¨¨, Nq 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 tg i u pi " 1, 2,¨¨¨, Nq, and tC i u pi " 1, 2,¨¨¨, Nq is the corresponding set of geomagnetic field contour. If the actual trace tX i u pi " 1, 2,¨¨¨, Nq 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 tH i u and tX i u [17].
where dis pX, Hq " ||X´H|| 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 " TH 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 extraction of the closest contour point is discussed in [16]. The minimization process of j E is iterated so that in the j-th iteration the set of measured points is  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. 1 cos sin sin cos where q refers to the rotation angle. x t and y t are translation.

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 where k l is weighting coefficient, taking 1 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).
Then define corresponding contours extracted with the corrected geomagnetic vector as ( ) 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 Figure 1. Illustration of inertial navigation system (INS) indication trace, Estimated trace, and Real trace.
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.
where θ refers to the rotation angle. t x and t y are translation.

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 tH i u: where λ k is weighting coefficient, taking λ k " 1 without a priori knowledge. C i,k ( 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).
Then define corresponding contours extracted with the corrected geomagnetic vector as Ω`C i,k˘.
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).

of 12
The least squares solution of this transformation can be given by solving the Procrustes problem [18]. The cost function can be written as: where L 0 and H 0 denote the centroids of L i and H i , L 1 i and H 1 i is the relative vectors relative to their centroids. Then normalize L i and H i :  (14) when A is nonsingular.
In terms of the singular value decomposition of A as UΛV T , the optimal rotation matrix R can be expressed as: The scale factor S is S " tr pΛq¨b Then minimize the first term contribution in Equation (12) by

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 INS simulation in this paper is carried out for a route under the condition shown in Table 2. 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.

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.

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. 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.  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. 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.

The Validity of Affine Transformation Based VICCP
Simulation results in Figure 5 and Table 4 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.

The Validity of Affine Transformation Based VICCP
Simulation results in Figure 5 and Table 4 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. 1 1 log , where i h is the intensity of geomagnetic anomaly field.  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.
where h i is the intensity of geomagnetic anomaly field.  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. 1 1 log , where i h is the intensity of geomagnetic anomaly field.

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: 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 show that the Bayesian-based algorithm will further improve the positioning accuracy and matching probability.

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: where k p is prior matching probability of each single element, which can obtain through experiment, and then j E in Equation (7) can be replaced with Equation (19). The Simulation result in Figure 7 and Table 5 show that the Bayesian-based algorithm will further improve the positioning accuracy and matching probability.

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

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.