Geomagnetic matching navigation algorithm based on robust estimation

The outliers in the geomagnetic survey data seriously affect the precision of the geomagnetic matching navigation and badly disrupt its reliability. A novel algorithm which can eliminate the outliers influence is investigated in this paper. First, the weight function is designed and its principle of the robust estimation is introduced. By combining the relation equation between the matching trajectory and the reference trajectory with the Taylor series expansion for geomagnetic information, a mathematical expression of the longitude, latitude and heading errors is acquired. The robust target function is obtained by the weight function and the mathematical expression. Then the geomagnetic matching problem is converted to the solutions of nonlinear equations. Finally, Newton iteration is applied to implement the novel algorithm. Simulation results show that the matching error of the novel algorithm is decreased to 7.75% compared to the conventional mean square difference (MSD) algorithm, and is decreased to 18.39% to the conventional iterative contour matching algorithm when the outlier is 40nT. Meanwhile, the position error of the novel algorithm is 0.017° while the other two algorithms fail to match when the outlier is 400nT.


Introduction
With the rapid development of science and technology, navigation technology has been widely applied in military and civil field in recent decades. Nowadays there are many kinds of navigation technology such as Inertial Navigation System (INS), Global Position System (GPS) [1], Celestial Navigation System [2], and Terrain Contour Matching (TERCOM) [3] and so on. But these systems all have their respective shortcomings. As we all know, INS is a kind of autonomous navigation method. However, its location error accumulates along with the elapse of time. GPS technology is with high positioning accuracy, and it can output all-weather navigation information. Yet it is unable to locate when the GPS receivers are in the remote areas where the GPS signals are weak, and it is vulnerable to the interference of the enemy. Celestial Navigation System can't be used when the weather is terrible. TERCOM can't work well when the vehicle moves on the water or the flat terrain.
The geomagnetic field has certain magnitude and direction, which is consistent to all kinds of vector field. It is a kind of solid source of the navigation information. More importantly, it nearly exists on the entire earth's surface [4] . Therefore the geomagnetic navigation can be used in the places where the other navigation methods can't work. Moreover, the geomagnetic navigation has many other advantages such as strong independence, low cost and good concealment. Thus it is favored by more and more scholars in recent decades. Reference [5] presents the process of geomagnetic matching navigation. MSD algorithm is used in [6] to implement the geomagnetic matching. In [7], a numerical iterative method takes the place of the traversal method in MSD algorithm. Reference [8] considers the initial longitude, latitude and heading errors, and it applies the iterative method to realize the fast geomagnetic matching. Reference [9] utilizes affine model transposition to realize geomagnetic matching with higher precision. However, some false data (namely the outliers) are usually measured because of the external disturbance influence and the performance limitations of the geomagnetic sensor when geomagnetic field is surveyed. It can seriously affect the geomagnetic matching navigation precision and badly disrupt its reliability. Based on [8], a fast geomagnetic matching algorithm that can remove the influence of the outliers is realized in this paper. This paper will be structured as follows. Section II introduces the geomagnetic matching navigation principle briefly. Section III introduces the robust estimation theory, and in this section, how to design the weight function is also showed. Section IV describes the mathematical principle of the new algorithm in detail. Section V summarizes the processes of the novel navigation algorithm based on robust estimation. Section VI shows the simulation results. The last section is conclusions. Figure 1 shows the geomagnetic matching navigation system. This system consists of three modules: INS module, geomagnetic map module and geomagnetic sensor module. Then the information of the three modules is fused together by matching algorithm, and the result is acquired. The concrete processes are as follows [5].  (1) Obtain the geomagnetic map of the matching area before the vehicle moves, and save them in the vehicle computer.

The Principle of Geomagnetic Matching Navigation
(2) As the vehicle passes through the matching area, the carried geomagnetic sensor gathers a series of real-time geomagnetic data.
(3) Output the INS information, the geomagnetic map and the real-time geomagnetic data to the matching algorithm and calculate matching result.

Robust Estimation
As mentioned earlier, there are some outliers in the surveyed geomagnetic data sometimes. But traditional algorithms such as MSD algorithm are all based on the principle of the mean square error. These algorithms can work effectively only when the measured data follow normal distribution, and they can't remove the affection of the outliers. Therefore this paper employs the robust estimation theory in statistics to design new algorithm which can eliminate the influence of the outliers.
The main task of the robust estimation [10] is to design the weight function. The measured data [11] can be usually divided into the normal data, the available data and the outliers in the robust estimation theory. The independent variable of the weight function is correspondingly divided into the protected area, the reducing area and the refusal area. The basic efficiency of the proposed algorithm is guaranteed by the measured data in the protected area, which are in the majority in the survey data. The robustness of the proposed algorithm is guaranteed by the measured data in the reducing area. And the measured data in the refusal area can enhance the algorithm robustness. In this paper, the weight function is selected as follows: where  is an adjustable parameter, and it is determined by the value of the outliers. Then the derivative of equation (1) is as follows: Figure 2 presents the weight function curve and Figure 3 shows its derivative curve. It can be seen that both the weight function and its derivative provide a gradual transition between the normal data and the outliers.

The Mathematical Principle of the Proposed Algorithm
Because of the INS deficiencies, there are displacement and heading errors between the reference trajectory which is calculated by INS and the real trajectory. Figure 4 is the sketch of the trajectories.
Rf Figure 4. The sketch of the trajectories.
In Figure 4, Rf represents the trajectory which is calculated by INS and it is called the reference trajectory. M is the trajectory which is calculated by geomagnetic matching algorithm and it is called the matching trajectory.
Rf ' is parallel to M .  is the heading error between the reference trajectory and the matching trajectory. The displacement error consists of the longitude and latitude errors. The longitude error is denoted by x  and the latitude error is denoted by y  .
A suitable choice for geomagnetic matching algorithm is the correlation criterion. The correlation criterion is selected as follows: where () I u v , represents the geomagnetic field intensity of the matching point is actual magnetic field intensity measured by magnetic sensor when the carrier is located at reference point T ab ( , ) . The matching trajectory is located nearby the reference trajectory. Thus the geomagnetic field intensity can be expanded in a Taylor series around the reference trajectory.
where () I a b , is the geomagnetic field intensity of the reference point () T ab , which can be obtained from geomagnetic map.
represents the geomagnetic field intensity gradient along the longitude at the point is the geomagnetic field intensity gradient along the latitude at the point () T ab , .
2 O represents higher order term. Substituting equation (5) into equation (4) and neglecting the higher order term 2 O , we can obtain equation (6).
where () Then substitute equation (3) into equation (6) and discretize it, thus equation (9) is acquired as follows: where nn xn a a b b 1 nn a a a   (12) and n represents the ordinal of the sampling points. The robust target function is acquired by combining equation (1) with equation (9). where p represents the total number of the sampling points and 2 p  . The geomagnetic matching problem is changed into searching for the values of x  , y  and  to make the robust target function minimum, therefore the first-order partial derivatives of F function to x  , y  and  are obtained as follows: Then nonlinear equations can be acquired as follows: Newton iteration [12] is employed to solve nonlinear equations in this paper. Then the solutions x  , y  and  of the nonlinear equations are substituted into equation (3), and the matching trajectory can be acquired. It should be noted that the large initial errors can disturb the matching precision for neglecting the higher order term of the Taylor series. So the proposed algorithm can be iterated many times and the first iteration matching result is regarded as the second iteration reference trajectory. Because the first iteration matching result eliminates most of the displacement and heading errors, the second iteration will acquire the matching result which is closer to the real trajectory, and the matching precision will be further improved.

The Process of the Proposed Algorithm
By employing Newton iteration to solve the nonlinear equations mentioned earlier, the concrete process of the proposed algorithm based on robust estimation is as follows: Step 1: Obtain the coordinates Step 3: Initialize the longitude, latitude and heading errors: Step 4: Calculate L and G as follows: Step 5: Calculate M  as follows: Step 6: Update the longitude, latitude and heading errors: Step 7: If the process meets the end condition (the iterative number reaches the predetermined number or the 2-norm of M  is less than the predetermined value), go to Step 8, and otherwise go to Step 4.
Step 8: Substitute the values of x  , y  and  into equation (3), and output the matching result.

Simulation and Results
In the simulations, international geomagnetic anomaly data is regarded as the geomagnetic map. The reference trajectory is generated by the strap-down inertial navigation system (SINS) [13]. Table I shows the simulation parameters. Geomagnetic sensor noise(nT) 3 In the simulations, the proposed algorithm in this paper is compared with traditional MSD algorithm [6] and the geomagnetic contour matching algorithm based on iterative method [8]. The different outliers are added on the real geomagnetic data in two simulations.
In simulation case 1, the outlier of 40nT is added on the eighth geomagnetic data measured by geomagnetic sensor. Figure 5 represents the matching trajectories, and Figure 6 shows the matching errors. It can be seen that the matching trajectory of the proposed algorithm is very close to the real trajectory and the matching error of the novel algorithm is the smallest. The maximum error of MSD algorithm is 0.100°. The maximum error of the geomagnetic contour matching algorithm based on iterative method is 0.042°. The maximum error of the proposed algorithm is 0.007°. Therefore, the proposed algorithm can effectively eliminate the influence of the outliers. In simulation case 2, the outlier of 400nT is added on the eighth geomagnetic data measured by geomagnetic sensor. Figure 7 and Figure 8 show the results. It can be seen that the maximum error of the proposed algorithm is 0.017° while the other two algorithms fail to match. Therefore, the proposed algorithm still can realize the high precision matching when the outlier is big.

Conclusion
A novel geomagnetic matching navigation algorithm which can eliminate the influence of the outliers is proposed in this paper. By constructing the robust target function, the robustness of the proposed algorithm is guaranteed. The matching result is calculated by Newton iteration. The simulation results indicate that the matching error of the proposed algorithm is decreased to 7.75% compared to the conventional MSD algorithm, and is decreased to 18.39% to the conventional iterative contour matching algorithm when the outlier is 40nT. Meanwhile, the maximum error of the proposed algorithm is 0.017° while the other two algorithms fail to match when the outlier is 400nT. Therefore, the proposed algorithm can effectively eliminate the influence of the outliers, and realize the high precision matching.
Further research will investigate better robustness of shape constraints against large inertial component drift.

Acknowledgments
This work is partially supported by the National Natural Science Foundation of China (61403095, 61375046, and 61640301).