An Improved Method of Geomagnetic Aided Inertial Navigation Algorithm with Gyro and Accelerometer Error Corrected Online

. In consideration of the problem that traditional geomagnetic aided navigation method cannot reduce the scaling error of indication track in inertial navigation system (INS), which will further limit the error correction precision of gyro and accelerometer, an improved geomagnetic matching algorithm based on affine transformation is proposed in this paper. A geomagnetic matching algorithm led to the optimal affine transformation solution by Procrustes analysis is presented and develops latitude and longitude reference information. Then a 13-dimensional-state extended Kalman filter which estimates the attitude misalignment angles, the position error, the velocity error, the Gyro drift, and accelerometer error is introduced to continuously update the output of INS and remove the accumulative error. The results show that geomagnetic aided navigation based on improved algorithm has better location accuracy and correction accuracy of INS than the traditional method.


Introduction
The orientation and velocity error of pure Inertial Navigation System (INS) will accumulate and result in divergence over time due to the existence of gyro drift and accelerometer deviation, which will further affect the positioning precision. Geomagnetic aided navigation system (GANS) utilizes the matching algorithm to compare geomagnetic field profile acquired real-time with the reference geomagnetic map stored on board for matching. The matching results will be utilized to correct gyro drift and accelerometer error of INS. GANS has characteristics of strong autonomous, good concealment and working consistently over earth surface without reliance on external radio navigation signals. Besides, the navigation errors of GANS don't cumulate over time [1]. Due to these advantages, GANS has essential application value in underwater vehicle, unmanned aerial vehicle and indoor pedestrian navigation in recent years [2].
In the 1960s, American E-System Corporation proposed contour-based magnetic matching MAGCOM system [1]. Then magnetic navigation technology has a rapidly developed in various countries from then on. The simulation platform of GANS was established and validated the iterated closest contour point (ICCP) matching algorithm better than traditional contour matching algorithm [3]. Simulation of geomagnetic/inertial integrated navigation System was presented by Zhou [4], which validated a new geomagnetic matching algorithm based on MSD algorithm and ICCP algorithm is effective. The SITAN algorithm makes use of terrain elevation information in GANS and achieves reliable results [5]. Literature [6] presents a locating method of geomagnetic/inertial integrated navigation system to improve the accuracy of initial geomagnetic matching position.
In order to solve the problem that ICCP has a high requirement no scaling on the INS indication track and enhance the accumulative error compensation accuracy and onboard performance of INS, this paper designed an information fusion filter with the combination of the improved ICCP described in [7]. The paper is organized as follows. Section 2 addresses the principle of GANS, Section 3 is devoted to key technologies of GANS, Section 4 presents the simulation and the results. Conclusions are given in Section 5.

Principle of GANS
The process of the GANS is shown in Figure 1. The navigation processor is mainly composed of matching algorithm and information fusion filter. The geomagnetic reference maps along the track are stored in the vehicle firstly. There are two primary inputs, one is the geomagnetic intensity processed from three-axis fluxgate magnetometers, and the other is the indication track collected from INS. During the vehicle cruise along the track, magnetometers are detecting background geomagnetic field to form the real-time magnetic sequence, which will be compared with the stored map to determine the difference and position information about the vehicle with matching algorithm. Then EKF estimates

Matching algorithm
The iterated closest contour point (ICCP) algorithm is a kind of matching algorithm based on contour constraint. It was derived from the iterative closest point (ICP) algorithm of image registration and often used in terrain matching and gravity matching navigation [8]. The magnetometer provides the corresponding measured The extraction of the closest contour point is discussed in [8]. The minimization process of j E is iterated so that in the j-th iteration the set of measured points is 1 jj H TH   and the set of points j X is determined from the new points j H . The iteration process is continued until T becomes negligible.
The rigid transformation T consists of the rotation matrix R and the translation vector t as shown in (2), which can be solved with the quaternion method.
where  refers to the rotation angle. x t and y t are the translation.
The affine transformation T A is used in this paper to solve the problem that ICCP with rigid transformation cannot reduce the scaling error in INS indication track. Defining scale factor S, the matching process of the indication track i H to the real track L i of each iterative can be performed with (3).
The least squares solution of this transformation can be given by solving the Procrustes problem [9]. The cost function can be written as: Regarding the singular value decomposition of A as T UV  , the optimal rotation matrix R can be expressed as: The scale factor S is Then minimize the first term contribution in Equation (4) by 00 t L SRH  (9)

Information fusion
According to positioning requirements, the system variables are defined as follows: where ,, System state equation is followed as: where F(t) is System state matrix, G(t) is System noise matrix and W(t) is white noise in the gyros and accelerometers.
Measure equation is shown as: z(t) is the differences of latitude and longitude between the INS and matching output. h(t) is a zero matrix with dimension 2×13, except that h(1, 6) = 1 and h(2, 7) = 1. v(t) is observation noise matrix, which is white noise.

The INS Error Propagation Model
The INS error propagation model actually used is more complex than the state equation in (11), and can be expressed as follows, with omitting the time variable t.

Simulation
The effectiveness of the proposed matching algorithm and data fusion algorithm can be examined by this numerical simulation. The reference matching maps are generated with Enhanced Magnetic Model of order 720 and disturbed by Gaussian noise. The simulated magnetic measurement data are acquired based on the geomagnetic model above and disturbed by certain level external interferences, as shown in Table 1.
According to the simulation results in figure 3, the traditional ICCP algorithm cannot effectively rectify the indication track to the real track, and also cannot get the correct matching results when there is a scaling error between the INS indication track and the real track. However, the improved algorithm achieves accurate matching results. At the same time, the improved ICCP algorithm can also effectively estimated gyro and accelerometer error as shown in Figure 4. The improved algorithm not only solves the scaling problem in the indication track during the geomagnetic matching process, but also accurately correct gyro drift and accelerometer error of INS.

Conclusion
This paper proposed an improved method of geomagnetic aided inertial navigation algorithm with gyro and accelerometer error corrected, the affine transformation was adopted to ICCP to increase adaptability with indication track error characteristics of INS, and then the gyro drift and accelerometer deviation of INS were accurate estimated with EKF. The best achievable positioning accuracy was less than 300 m and velocity was less than 2 m/s in case of existing scale error.