An INS/Geomagnetic Integrated Navigation Algorithm Based on Matching Strategy and Hierarchical Filtering

Abstract

To improve the positioning accuracy of an inertial/geomagnetic integrated navigation algorithm, a combined navigation method based on matching strategy and hierarchical filtering is proposed. First, the PDA-ICCP geomagnetic matching algorithm is improved. On basis of evaluating the distribution of magnetic measurements, a number of controllable magnetic values are regenerated to participate in the geomagnetic matching algorithm (GMA). As a result, accuracy of the matching algorithm is ensured and its efficiency is improved. Secondly, the integrated navigation filter is designed based on the hierarchical filtering strategy, in which the navigation information of the geomagnetic matching module and inertial navigation module are respectively filtered and fused in the main filter. In this way, the shortcoming that GMA is unable to provide continuous and real-time navigation information is overcome. Meanwhile, precision of the inertial/geomagnetic integrated navigation algorithm is improved. Finally, the feasibility and validity of the proposed algorithm are verified by simulation and physical experiments. Compared with the integrated filtering algorithm which directly uses the error equation of inertial navigation system (INS) as the state equation, the proposed hierarchical filtering algorithm can achieve higher positioning precision.

1. Introduction

An inertial navigation system (INS) can provide continuous and real-time navigation information for the carrier, and it is characterized by strong autonomy and good concealment. However, the navigation error of INS will accumulate over time, therefore it is hard to adapt to a long endurance mission. On the other hand, geomagnetic navigation is based on Earth’s geophysical field, which is passive, radiation-free and displays good stealth. Most importantly, the location error of geomagnetic navigation does not accumulate over time [1,2,3,4]. Therefore, constructing an inertial/ geomagnetic integrated navigation system can not only overcome the disadvantages of INS, such as accumulated navigation errors and poor long-term stability, but also can realize all-weather and all-region navigation.

According to their different combinations, inertial/geomagnetic integrated navigation algorithms can be divided into two categories, namely the loose and tight navigation algorithm. Generally speaking, the loose navigation algorithm mainly consists of the geomagnetic matching algorithm (GMA) and integrated navigation algorithm (INA), in which the locations evaluated by the GMA are used as observations of the integrated filter to estimate INS’s error [5]. The structure of loose navigation is beneficial for fault diagnosis, and is convenient for adding other navigation subsystems, but its filtering accuracy is largely affected by the location precision of GMA. As for the tight navigation algorithm, the measured magnetic values are directly used as observations of the integrated navigation filter [6]. As the measurement equation of tight navigation system is always nonlinear [7,8], the Extended Kalman Filter (EKF) [9], Unscented Kalman Filter (UKF) [10] and Particle Filter (PF) [11,12,13] are most commonly used. A linear approximation of the geomagnetic map is needed in EKF and UKF, but not for PF. Considering the non-linearity of the geomagnetic map, the EKF may easily diverge. Secondly, when other navigation subsystems are added, the loose INA is easier to implement.

It should be noted that the GMA is the core of loose INA, which estimates the carrier’s position by matching the geomagnetic characteristics measured in real time with the geomagnetic map previously stored in the navigation computer. Once the accuracy of the magnetic map is fixed, GMA is mainly affected by the magnetic measurements. Generally, the geomagnetic measurement error includes the instrument error, which is caused by the structure and materials of the magnetometer [14] and the interference field error, which is caused by the superposition of external interference field. Among them, the external interference field is mainly led by the hard and soft magnetic material of the carrier, and the random magnetic field which is difficult to describe using the magnetic model. In general, the non-random interference field in geomagnetic measurement can be processed by model compensation algorithms, but for the random interference field and other unknown interference factors that may appear in practice, the compensation result is always unsatisfactory. Currently, there are two ways to solve the problem: (1) In order to reduce the influence of interference field, process the magnetic measurements, such as Hilbert-Huang transform (HHT) [15], wavelet transform [16] and so on. (2) To study new GMA with good robustness.

The Iterated Closest Contour Point (ICCP) algorithm is a common method in geomagnetic matching, whose basic principle is shown in Figure 1. As is illustrated, suppose the matching length of ICCP algorithm is 5; C₁ to C₅ are five geomagnetic contour lines of a navigation area; M₁ to M₅ denote the real trajectory of the carrier; the carrier’s coordinates offered by the reference navigation system (usually the INS) at the sampling points are denoted by I₁ to I₅.

If there is no magnetic interference, point M_n (n = 1, 2, …, 5) must be located on a geomagnetic contour line, but on which point of the contour is unknown. At the first step of geomagnetic ICCP algorithm, it finds the points closest to INS’s trajectory on the contour lines. The closest points are represented by N₁ to N₅ in Figure 1. Then ICCP algorithm will calculate the rigid transformation T (including rotation and transformation) between I_n and N_n (n = 1, 2, …, 5). Afterwards, the transformation T is applied to INS’s trajectory, and the new trajectory is denoted by N’₁ to N’₅. In next interaction, a new transformation will be used to minimize the distance between N’_n (n = 1, 2, …, 5) and their closest points. The criterion to measure the distance between the points is in Formula (1). The above process will be iterated many times. Finally, I_n will continuously approximate the real trajectory of the carrier.

$$D={\displaystyle \sum _{i=1}^5{d}^2(C_n,\mathit{T}{I}_n)}$$

(1)

where d measures the distance of I_n and its corresponding contour line C_n.

The ICCP algorithm assumes that the magnetic sensor has no measurement error, and the real position of the carrier is located on or very close to the magnetic contour that corresponds to the magnetic measurement [17]. If the hypothesis is not satisfied, the accuracy of GMA would be hard to guarantee. Wu proposed a GMA based on the tree searching algorithm. It does not need to consider the distribution of measurement errors and has a certain anti-interference ability [18]. Xiao combined the probability data association (PDA) algorithm with the ICCP algorithm [19,20] and proposed a PDA-ICCP algorithm. It regarded all measurements of the magnetometer within a certain confidence range in interference environment as the effective measurements of a position, and finally evaluated the carrier’s location with the PDA fusion algorithm. The algorithm comprehensively considered the effects of measurement error and interference field and improved the GMA’s adaptability to a practical environment.

To guarantee the accuracy of the integrated inertial/geomagnetic navigation under interference environment, the PDA-ICCP algorithm is improved in our study and on basis of it, a new integrated navigation filter is designed. Finally, the feasibility and effectiveness of the proposed algorithm are verified through simulation physical and experiments.

2. Improvement of PDA-ICCP Algorithm

2.1. RM-PDA-ICCP Geomagnetic Matching Algorithm

In practice, due to the influences such as residuals of geomagnetic compensation, diurnal variation of magnetic field and measurement error of magnetometer, the magnetic measurement may not be equal to the corresponding value read from the geomagnetic map. If these magnetic measurements with noise are directly applied in GMA, the matching precision will be reduced. Even worse, it may lead to matching failure. To solve the problem, a PDA-ICCP algorithm is presented. Its basic idea is shown in Figure 2.

As is illustrated in the figure, it is supposed that the matching length is 5. m_k0, which is usually read directly from the magnetic map, is the ideal magnetic value at the kth matching point. m_k1, m_k2, …, m_kn are the data measured n times at the kth point when interference field exists. m_k-1, m_k-2, …, m_k-L+1 are the magnetic values of the other points of the geomagnetic matching sequence (GMS). Assuming the interference field follows the Gaussian distribution with mean 0, variance σ², then according to the 3σ criteria, the magnetic measurements will fall within the circle of radius 3σ centered at m_k0 with the probability of 99.74%. If the n measurements at the kth matching point are all within this circle (the upper circle in Figure 2), and each of them constitutes a GMS with m_k-1, m_k-2, …, m_k-4, then there will be n matching results at kth point (denote as P_{k, i,} i = 1, 2, …, n) when the ICCP algorithm is executed. Considering the constraints of carrier’s kinematic performance, such as the heading and velocity constraints, its position should be within a reasonable scope. We denote the matching results in the scope as the valid estimations of the carrier’s location. Otherwise, the matching results are invalid. Finally, by fusing all valid estimations with PDA algorithm, the carrier’s final position can be estimated.

The essence of the method above is to select the positions where the carrier is most likely to appear and fuse them by PDA algorithm. In a statistical sense, if the magnetic field is measured enough times, there will always be a measurement close to the ideal magnetic value, thus the fusion effect of PDA algorithm is guaranteed. But, the more the ICCP algorithm is conducted, the worse the real-time performance it shows. In fact, if the magnetometer’s measurements are far away from the corresponding values read from the magnetic map, mismatches will appear in the GMA. These mismatched results are very likely to be eliminated due to the constraint of carrier’s motion in the fusion process of PDA. In other words, if number of the effective measurements is controllable and they are very close to the ideal geomagnetic value of the corresponding position, both the algorithm’s precision and real-time performance can be guaranteed. Based on this, an improved fusion algorithm based on regenerated measurements is put forward, which modified the first process of the PDA-ICCP algorithm to generate magnetic measurements. Its principle is shown in Figure 3:

In Figure 3, the definition of m_k-1, m_k-2, …, m_k-L+1 are the same with that in Figure 2, ${\widehat{m}}_{k0}$ is the estimation of the ideal magnetic value at the kth matching point, $\widehat{\sigma }$ is the estimated standard deviation of the magnetic measurements. Three circles, which are centered at ${\widehat{m}}_{k0}$ with the radius of $\widehat{\sigma }$, 2$\widehat{\sigma }$ and 3$\widehat{\sigma }$, divide the confidence region of the magnetic measurements into three areas. RM-PDA-ICCP algorithm respectively regenerates several possible measurements in each area, which are denoted as m’_k1, m’_k2, …, m’_km. They reconstitute the GMSs with the former L-1 matching points and participate the subsequent geomagnetic matching and fusion. To distinguish from the magnetic measurements in PDA-ICCP algorithm, these regenerated magnetic values are recorded as the regeneration measurements (RMs).

Rather than directly using the measured magnetic values for matching, the RM-PDA-ICCP algorithm regenerates a number of controllable RMs based on the statistical characteristics of the magnetometer’s measurements, which limits the number to execute the algorithm and enhances its real-time performance to a large extent. In addition, the RMs in r₁, r₂ and r₃ can be adjusted to ensure the algorithm’s accuracy and the stability of the algorithm.

It can be concluded that the RM-PDA-ICCP algorithm contains the following steps:

• Firstly, estimate ${\widehat{m}}_{k0}$ and ${\widehat{\sigma }}^2$, and regenerate the RMs.
• Secondly, conduct the ICCP algorithm with all the RMs.
• Thirdly, select effective matching results with the constraints of the kinematic performance of the carrier.
• Finally, calculate the associated probability and fuse all effective matching results.

Obviously, evaluating ${\widehat{m}}_{k0}$ and ${\widehat{\sigma }}^2$_, determining the constraints of carrier’s kinematic performance and calculating the associated probability of PDA algorithm are the key points in the RM-PDA-ICCP algorithm, which are explained respectively in the following parts.

2.2. Calculation of ${\widehat{m}}_{k0}$ and ${\widehat{\sigma }}^2$

m_k0 is the ideal magnetic value at a position in the PDA-ICCP algorithm, which is hard to achieve when the position error is very large. It is also challenging to measure the magnetic field of a position for many times while the carrier is moving. Nevertheless, measurement of the magnetometer is a random variable that follows a certain probability distribution. In addition, the carrier cannot move for a long distance within a short period, so the corresponding changes of the magnetic field are relatively small. In this sense, the magnetometer’s measurements within a short period can be considered as the random quantity of the measurements at a position, and the parameter estimation method can be used to evaluate the statistical characteristics of the magnetic measurements.

Suppose m_k1, m_k2, …, m_kn are the values measured n times at the kth matching point, and they follow the Gaussian distribution, then according to the moment estimation method, the estimated value of m_k0 and σ² can be expressed as:

$$\{\begin{array}{c}{\widehat{m}}_{k0}=\frac{1}{n}{\displaystyle \sum _{i=1}^n{m}_{ki}}\\ {\widehat{\sigma }}^2=\frac{1}{n}{\displaystyle \sum _{i=1}^n(m_{ki}-{\widehat{m}}_{k0})}\end{array}$$

(2)

2.3. Constraints of Carrier’s Kinematic Performance

The position of the carrier at the kth matching point can be estimated by its kinematic performance at the (k-1)th matching point. As is shown in Figure 4, (x_k-1, y_k-1) and (x_k, y_k) respectively denote the carrier’s position at the (k-1)th and kth matching point. Considering the influence of velocity error and course error, and supposing the carrier’s velocity and course interval at (k-1)th point is [$V_{k-1}^{\min }$,$V_{k-1}^{\max }$] and [${\theta }_{k-1}^{\min }$,${\theta }_{k-1}^{\max }$], then at the kth point the carrier is restricted within the concentric circles with the radius of $T{V}_{k-1}^{\min }$ and $T{V}_{k-1}^{\max }$ by the velocity error, and in a fan-shaped area by the course error. Here, variable T measures the time between (k-1)th and kth point. Finally, the shadow part in Figure 4 shows the possible position of the carrier, which can be described by Formula (3).

$$\{\begin{array}{c}\mathrm{arc}\tan \frac{y_k-y_{k-1}}{x_k-x_{k-1}}\in [{\theta }_{k-1}^{\min },{\theta }_{k-1}^{\max }]\\ \sqrt{{(y_k-y_{k-1})}^2+{(x_k-x_{k-1})}^2}\in T[V_{k-1}^{\min },V_{k-1}^{\max }]\end{array}$$

(3)

2.4. Calculation of the Associated Probability

Associated probability determines the fusion coefficient of every effective position restricted by Formula (3). Suppose the n measurements (m_k1, m_k2, …, m_kn) at the kth matching point follow the normal distribution with the mean of m_k0 and variance of σ₀², then the probability Pr_ki that the ith measurement derives from its ideal magnetic value can be expressed as:

$$P{r}_{ki}=1-erf(\frac{m_{ki}-m_{k0}}{\sqrt{2}{\sigma }_0}),i=1,2,\dots ,n.$$

(4)

where erf(x) is the error function and is defined as:

$$erf(x)=\frac{\sqrt{2}}{\mathsf{\pi }}{\displaystyle {\int }_0^x{e}^{-{\gamma }^2}d\gamma }$$

(5)

Define a matrix (A)_1×n, where n is the measurement number at the kth matching point. Once the matching result of the ith measurement satisfies Formula (3), the ith element in A is set to 1. Otherwise, the corresponding element is 0. As a result, the associated probability of each effective matching result, denoted by $P{r}_{ki}^{\prime }$ (i = 1, 2, …, n) can be represented as:

$$[P{r}_{k1}^{\prime },P{r}_{k2}^{\prime },\cdots ,P{r}_{kn}^{\prime }]={[P{r}_{k1},P{r}_{k2},\cdots ,P{r}_{kn}]}^{\mathrm{T}}\times \mathit{A}$$

(6)

Normalize $P{r}_{ki}^{\prime }$, then the associated probabilities are:

$$P{r}_{ki}^{\prime \prime }=\frac{Pr{\prime }_{ki}}{{\displaystyle \sum _{i=1}^{n}Pr{\prime }_{ki}}},i=1,2,\cdots ,n.$$

(7)

Finally, the position of the carrier is estimated as:

$${\widehat{P}}_k={\displaystyle \sum _{i=1}^n{P}_{k,i}P{r}_{ki}^{\prime \prime }}$$

(8)

where ${\widehat{P}}_k$ is the final position estimated by the RM-PDA-ICCP algorithm, P_k,i is the matching result of ICCP algorithm corresponding to the ith magnetic measurement m_ki.

3. Design of the Integrated Navigation System

According to different state variables, the loose integrated navigation algorithm can be divided into two types: the one based on position error [2] and the one based on location (and speed) [5]. The former usually takes the error equation of INS as the state equation of the integrated navigation filter, and the position error of GMA as the observations to estimate the attitude error, velocity error and position error of the carrier. While the latter directly takes the motion equation of the carrier as state equation, and updates the position and speed immediately. The above two integrated algorithms have been applied in integrated navigation system that based on geophysical fields such as geomagnetism, gravity and terrain. In our study, a hierarchical filter is put forward. The inertial/geomagnetic integrated navigation system is designed in Figure 5, which adopts the carrier’s location as the state variable. As is shown, the hierarchical filter is divided into two layers, the first layer consists of two subfilters, and the second one includes a main filter.

The above system consists of two sub-filters and one main filter. First, the geomagnetic and inertial navigation subsystems independently execute the Kalman filtering algorithm and give an estimate of the carrier’s position. Then the main filter integrates the position information of two subsystems to evaluate the position error and velocity error of the carrier. Ultimately, the INS can be corrected. In the following, the state equation, measurement equation and filtering algorithm of the integrated navigation system are described in detail.

3.1. State Equation and Observation Equation of the Subsystem

Assume the carrier is in the uniform rectilinear motion, the matching results of GMA are taken as observations of the filter, and the position and velocity in longitude and latitude are taken as the state, namely $\mathit{X}(k)={[\lambda (k)V_{\mathrm{E}}(k)L(k)V_{\mathrm{N}}(k)]}^{\mathrm{T}}$, then the state equation and observation equation of the geomagnetic filter subsystem can be expressed as:

$${\mathit{X}}_{k+1}=\mathit{\Phi }{\mathit{X}}_k+\mathit{\Gamma }{U}_k$$

(9)

$${\mathit{Z}}_k=\mathit{H}{\mathit{X}}_k+V_k$$

(10)

where $\mathit{\Phi }=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]$, $\mathit{\Gamma }=\left[\begin{array}{ll}\frac{T^2}{2}\hfill & 0\hfill \\ T\hfill & 0\hfill \\ 0\hfill & \frac{T^2}{2}\hfill \\ 0\hfill & T\hfill \end{array}\right]$, $\mathit{H}=\left[\begin{array}{llll}1\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 1\hfill & 0\hfill \end{array}\right]$, T is the time between two sampling points, U_k is the random disturbance during the movement of the carrier, V_k is the location estimation noise of GMA.

The RM-PDA-ICCP algorithm executes once a magnetic measurement is obtained. In the regions where the geomagnetic fluctuation is not very obvious, the RM-PDA-ICCP algorithm is prone to mismatching. If the mismatched results are sent to the main filter, the precision of integrated navigation system will greatly reduced. In this sense, the geomagnetic filter subsystem will evaluate a proper result for the main filter. Finally, the accuracy and real-time performance of the integrated navigation algorithm are ensured.

The state and observation equation of the inertial subsystem are the same as the geomagnetic filter subsystem, which are not discussed to keep our study concise.

3.2. State Equation and Observation Equation of the Main Filter

In the main filter, the position difference between two subsystems is taken as the observation. Its state equation and observation equation are:

$$\mathit{\delta }{\mathit{X}}_{k+1}=\mathit{\Phi }\mathit{\delta }{\mathit{X}}_k+{\mathit{w}}_k$$

(11)

$$\left[\begin{array}{c}{\lambda }_{\mathrm{GMNS}}-{\lambda }_{\mathrm{INS}}\\ L_{\mathrm{GMNS}}-L_{\mathrm{INS}}\end{array}\right]=\mathit{H}\mathit{\delta }{\mathit{X}}_k+{\mathit{n}}_k$$

(12)

where $\mathit{\delta }\mathit{X}={[\delta \lambda \delta V_{\mathrm{E}}\delta L\delta V_{\mathrm{N}}]}^{\mathrm{T}}$, w_k is the system noise; λ_GMNS and λ_INS are the matching position of the geomagnetic filter subsystem and inertial subsystem in longitude respectively, L_GMNS and L_INS are the positions in latitude. n_k is the location estimation noise, $\mathit{H}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]$, $\mathit{\Phi }=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]$.

3.3. Integrated Filtering Algorithm

As the state and observation equations of the integrated navigation system are all linear, a standard KF algorithm can be adopted. The difference between the subfilters and main filter is that a threshold is set in the geomagnetic filter subsystem to restrict the matching results whose error is very large. The filtering and updating process of the geomagnetic filter subsystem is shown in Figure 6, in which Q and R stand for the variance of the process noise U_k and measurement noise V_k; Z_k is the observation, which is corresponding to the matching results of RM-PDA-ICCP algorithm. P₀ and ${\hat{\mathit{X}}}_0$ are the initial states of filtering; P is the covariance matrix, K is the filtering gain matrix, and $\hat{\mathit{X}}$ is the state estimation of the subsystem. Other variables are the same with that in Formula (9)–(12).

In Figure 6, the covariance matrix, gain matrix and filtering state are updated during the filtering process. If the observation Z_k exceeds the presupposed threshold, the estimated state ${\hat{\mathit{X}}}_{k,k-1}$ should be taken as current state ${\hat{\mathit{X}}}_k$, otherwise ${\hat{\mathit{X}}}_k$ should be updated with the KF algorithm. The filtering process of the main filter is a standard KF.

In essence, the algorithm above belongs to the loose integrated navigation algorithm. Furthermore, only minor changes are needed when other auxiliary navigation methods are added to the integrated navigation system. In addition, introducing filtering algorithm to the geomagnetic matching module is beneficial to improve the adaptability to the geomagnetic environment.

In a word, the proposed algorithm can be summarized as follows:

• Step 1: To execute the RM-PDA-ICCP algorithm and obtain the estimated position of the carrier;
• Step 2: The result in Step 1 is taken as the observation of the geomagnetic filter subsystem;
• Step 3: The information offered by the inertial system is used in the INS filter subsystem;
• Step 4: The results of Step 2 and Step 3 are fused by the main filter. Finally, the positions of the carrier are continuously calculated.

4. Simulation Experiments

A local area of the magnetic anomaly field is randomly selected from the Y component of the NGDC-720 model to verify the proposed algorithm, its latitude and longitude respectively ranging from [49.8°, 50.8°] N and [36.5°, 37.5°] E. Then the Kriging interpolation method is used to establish the detailed magnetic map, as is shown in Figure 7. After interpolation, the resolution of the geomagnetic map is about 100 m.

4.1. Effectiveness of the Proposed Algorithm

It is assumed the carrier is in uniform rectilinear motion, and the angle between its movement direction and X-axis is 135°. It’s velocity along the East and north are both 15 m/s. Suppose the proposed integrated navigation algorithm is conducted from the point [37.4° E, 49.86° N]. INS’s accumulated error is simulated by adding different rigid transformations to the real trajectory [21]. The measurement error is Gaussian white noise. Its original position errors in longitude and latitude are both 300 m, and the bias of gyroscope is 1 °/h. Suppose the magnetic interference follows the normal distribution, whose average is 0 nT and variance is 1 nT².

Evaluate m_k0 and σ² by the method in Section 2.2, and then ten RMs at ${\widehat{m}}_{k0}$ ± 1/4$\widehat{\sigma }$, ${\widehat{m}}_{k0}$ ± 1/2$\widehat{\sigma }$, ${\widehat{m}}_{k0}$ ± $\widehat{\sigma }$, ${\widehat{m}}_{k0}$ ± 2$\widehat{\sigma }$ and ${\widehat{m}}_{k0}$ ± 3$\widehat{\sigma }$ could be generated respectively. Assume the matching length of GMS is 5, and the allowable velocity error and course error of the carrier are respectively ±5 m/s and ±20°, then the RM-PDA-ICCP algorithm is conducted. If all matching results corresponding to the RMs do not meet the constraint of carrier’s kinematic performance, the algorithm does not output.

Afterwards, Formula (9) and (10) are used in the geomagnetic filter subsystem, where the matching results of RM-PDA-ICCP algorithm are taken as the observations, and the position and velocity are the filter’s state. The sampling interval is T = 2 s, and the location estimation noise covariance R and process noise covariance Q are set as follows:

$$\mathit{R}=\left[\begin{array}{cc}10& 0\\ 0& 10\end{array}\right],\mathit{Q}=\left[\begin{array}{cccc}10& 0& 0& 0\\ 0& 0.5& 0& 0\\ 0& 0& 10& 0\\ 0& 0& 0& 0.5\end{array}\right].$$

where the unit of R is (m)². In matrix Q, units of the elements representing the position are (m)², units of the elements representing velocity is (m/s)².

Since the matching length is 5, namely the matching result of RM-PDA-ICCP algorithm is output from the 5th sampling point, therefore the filtering initial value of the geomagnetic subsystem is set to X(4), and the initial covariance is:

$${\mathit{P}}_0=\left[\begin{array}{cccc}0.5& 0& 0& 0\\ 0& 0.5& 0& 0\\ 0& 0& 0.5& 0\\ 0& 0& 0& 0.5\end{array}\right]$$

where the unit of P₀ is defined the same as that in matrix Q.

In order to quantitatively describe the effect of GAM and filtering algorithm, the total position error is defined as:

$$e_k=\sqrt{{({\widehat{\lambda }}_k-{\lambda }_k)}^2+{({\widehat{L}}_k-L_k)}^2}$$

(13)

where (${\widehat{\lambda }}_k$, ${\widehat{L}}_k$) is the estimated location at sampling point k, and (λ_k, L_k) is the real position of the carrier.

Figure 8. shows the filtering results of the geomagnetic navigation subsystem. It can be seen that although the matching results of RM-PDA-ICCP algorithm are effective at most sampling points, the matching errors are relatively large. When the filtering algorithm is conducted, the positioning accuracy of the geomagnetic filter subsystem is significantly improved. As is shown in Figure 8c, all positioning error is within 250 m, whereas the accuracy still needs to be improved.

In addition, it is obvious that the outputs of RM-PDA-ICCP algorithm in Figure 8a are not continuous, and the position errors at some sampling points in Figure 8c are not displayed (such as the 45th sampling point). This is because the matching results corresponding to all RMs are too large to meet the constraint of carrier’s kinematic performance.

For the inertial subsystem, $\mathit{R}=\left[\begin{array}{cc}100& 0\\ 0& 100\end{array}\right]$ m. Other simulation parameters are the same as the geomagnetic filter subsystem. Figure 9 shows its filtering results.

As is shown in the illustration, filtering results of the inertial subsystem change with the outputs of INS. However, considering the influence of INS’s accumulated error, the filtering result will diverge over time. Therefore, even if the filtering algorithm is applied, the geomagnetic filter subsystem still calls for correction.

Based on the filtering results of the geomagnetic and inertial subsystem, the main filter is established according to Formula (11) and (12). Other simulation parameters are the same as the geomagnetic filter subsystem. Figure 10 gives the filtering results of the integrated navigation system:

In Figure 10a, the filtering results of two subsystems are further integrated in the main filter. Obviously, the position accuracy is greatly improved. In Figure 10b, it shows the total error at each sampling point is less than 120 m, which is much better than the subsystem.

Table 1. Results of simulation experiment (Unit: m).

Location Error	Maximum Error	Minimum Error	Mean
Geomagnetic filter subsystem	241.7	10/8	113.6
INS filter subsystem	467.8	347.2	394.6
Integrated filter system	108.7	49.3	87.9

lists some statistical characteristics of the location error of each system. The results are consistent with the above analysis.

4.2. Algorithm Comparison

To further validate the proposed integrated navigation algorithm, comparison experiments are conducted in this section. First, set the original error of INS in latitude and longitude to 200 m, its total error is 282.8 m (about 2.82 magnetic grids). R and Q are set according to the actual equipment: the magnetic measurement error is based on the performance of the magnetometer; the error of INS is referenced to the parameters of a certain type of unmanned aerial vehicle (UAV). For the sake of privacy, the values are not exactly the same with actual equipment, but by experiment comparison, the conclusion still holds. Other simulation parameters are the same as that in Section 4.1. Then, the proposed algorithm is compared with the other loose integrated navigation algorithm, which takes the error equation of INS as state equation and the matching error as observation [2]. Its state and observation equation are shown below:

$$\mathit{\delta }{\mathit{X}}_{k+1}=\mathit{F}\mathit{\delta }{\mathit{X}}_k+{\mathit{m}}_k$$

(14)

$$\mathit{\delta }{\mathit{r}}_k=\mathit{H}\mathit{\delta }{\mathit{X}}_k+{\mathit{V}}_k$$

(15)

where $\mathit{\delta }\mathit{X}={[{\varphi }_{\mathrm{N}}{\varphi }_{\mathrm{U}}{\varphi }_{\mathrm{E}}\delta V_{\mathrm{N}}\delta V_{\mathrm{E}}\delta L\delta \lambda ]}^{\mathrm{T}}$, ${\varphi }_{\mathrm{N}}$, ${\varphi }_{\mathrm{U}}$ and ${\varphi }_{\mathrm{E}}$ stand for the attitude angle error in north, up and east, $\delta V_{\mathrm{N}},\delta V_{\mathrm{E}}$ corresponds to the velocity error in north and east, $\delta L$ and $\delta \lambda$ are the position error in longitude and latitude; F is the state transfer matrix of INS’s error equation; m_k is the system noise. F is the state transition matrix when the error is taken as the state variable, and a traditional Kalman filtering method is adopted. The detailed principle can refer to reference [22]; δr_k represents for the location error of RM-PDA-ICCP algorithm, $\mathit{H}=\left[\begin{array}{ccccccc}0& 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 1& 0\end{array}\right]$.

Figure 11 shows the comparative results:

As is illustrated in the figure, the filtering results of different algorithms are located near the real position of the carrier, indicating that both algorithms are effective. But comparatively speaking, the total error of the proposed integrated navigation algorithm is less than 200 m (2 magnetic grids) when the filter is stable, and its positioning result is much closer to the real position of the carrier.

5. Flight Experiment

Considering the magnetic field in the air is relatively pure, a quadrotor is used in our study to test the proposed integrated navigation algorithm. The experiment system is shown in Figure 12.

As is shown, the inertial/geomagnetic integrated navigation system mainly includes the following parts: the measurement and storage module, the quadrotor and the ground control station. Among them, the measurement module is a 10-axis high precision attitude-angle sensor made by Wit Smart Company, which contains the RM3100 geomagnetic sensor, inertial element and GPS module. Its main parameters are listed in

Table 2. Main parameters of the sensor module.

Main Parameters	Acceleration	Angular Velocity	Geomagnetic Field	Angle
Measurement dimension	3	3	3	3
Measurement range	±16 g	±2000 °/s	±4800 μT	±180°
Measurement error and precision	(1) Measurement error in X and Y axis is 0.05°. In Z axis, it is 1°. (2) Measurement precision of geomagnetic sensor is 0.1 μT.

. The storage module uses a serial port to SD card to storage data. To avoid the influence of carrier’s material on the magnetic measurement, the frame of the quadrotor is made of carbon.

In the system, the real position of the carrier is given by GPS; the ground control station is used to communicate with the quadrotor, to load the planed trajectory and to conduct the integrated navigation algorithm.

The total magnetic values of the sensor are applied in navigation. Finally, a measured geomagnetic field with the size of 1400 m × 600 m is taken as the background, whose grid resolution is 25 m. The quadrotor is in the uniform rectilinear motion at the speed of 5 m/s with the starting point being (199.3 m, 201.5 m) and altitude 40 m. Suppose the baud rate of sensor module is 9600, its output frequency is 10 Hz. Limited by the endurance time of the quadrotor, the INS element has worked for a while before the flight experiment, so its accumulated error is obtained. Assume the original position errors of INS in longitude and latitude are both 40 m, namely the total error of INS is 56.57 m (about 2.26 magnetic grids), bias of the gyro is 1 °/h.

Then the proposed integrated navigation algorithm is executed. In the geomagnetic filter subsystem, the state and observation equation follow Formula (9) and (10), where T = 5 s. The variance matrix of the observation noise $\mathit{R}=\left[\begin{array}{cc}10& 0\\ 0& 10\end{array}\right]$, and the process noise $\mathit{Q}={10}^{-2}\times \left[\begin{array}{llll}10\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0.5\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 10\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0.5\hfill \end{array}\right]$.

In the main filter, the process noise $\mathit{Q}=\left[\begin{array}{llll}10\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0.5\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 10\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0.5\hfill \end{array}\right]$. Other parameters are unchanged. Figure 13, Figure 14, Figure 15 and Figure 16 respectively show the filtering results of the geomagnetic filter subsystem and the integrated navigation system, and

Table 3. Results of flight experiment (Unit: m).

Location Error	Maximum Error	Minimum Error	Mean	Standard Deviation
Geomagnetic filter subsystem	45.24	6.61	28.20	11.44
Integrated filter system	38.73	4.28	13.58	8.11

lists some statistical characteristics of the location error of each system.

In Figure 14, as all matching results corresponding to the RMs are beyond the threshold of effective matching, the RM-PDA-ICCP algorithm has no output at some sampling points. Besides, the matching errors at some points are too large, for example the 5th and the 28th sampling points. However, the total location error of the subsystem after filtering is within 50 m (about 2 magnetic grids). As a result, the locations of the carrier could be output continuously by the geomagnetic filter subsystem. Comparing the final filtered trajectory with the carrier’s real trajectory in Figure 13a, it is shown that the observations have a great influence on the filtering algorithm.

In Figure 15 and Figure 16, the filtering trajectory of integrated navigation system is very close to carrier’s real trajectory, and all the total filtering errors are less than 40 m (about 1.6 magnetic grids). Compared with the geomagnetic filter subsystem, the filtering results are obviously improved. This result proves the effectiveness of proposed combined navigation algorithm.

6. Conclusions

Filtering accuracy of the integrated navigation system is greatly impacted by the results of GMA. Due to the precision limitation of geomagnetic measurements and mapping, and the magnetic information in some areas being insufficient, the GMA is easy to mismatch. The RM-PDA-ICCP algorithm fuses each possible matching within a certain confidence region and improves the reliability of the location result. In addition, the hierarchical filtering and refusion strategy is able to fuse the results of each navigation subsystem, which guarantees the advantages of loose integrated navigation system and improve its accuracy at the same time.

In order to reduce the influence of the magnetic field of the carrier on magnetic measurements, the frame of the quadrotor is carbon. However, there are a lot of ferromagnetic materials and electromagnetic equipment in the actual system and as a result, the proposed integrated navigation algorithm will be further verified based on magnetic measurement compensation.