1. INTRODUCTION
Strapdown Inertial Navigation Systems (SINS) develop navigation errors as time elapses, and the Global Positioning System (GPS) is typically used to update INS errors. For underwater vehicles, however, such as autonomous underwater vehicles, unmanned underwater vehicles, or submarines, their missions may be compromised without GPS data. Thus, some aided methods based on geophysical maps (Ingemar and Magnus, Reference Ingemar and Magnus2004; Liu and Tan, Reference Liu and Tan2011; Lin et al., Reference Lin, Yan and Tong2007) have been proposed to bound the INS errors. One such method which uses gravity field maps is gravity-aided navigation (Behzad and Behrooz, Reference Behzad and Behrooz1999). Although gravimeters and gradiometers can be used to sense local field amplitudes and gradients, respectively, cost often prohibits the use of gradiometers. Thus, in this paper, a triangle matching algorithm is proposed to correct navigation errors, with its key point being the transformation of position based on gravity amplitudes, namely the gravity anomalies. We must note that contours of the local gravity field are used for matching in this algorithm, and gravity-aided navigation is valid only when the local gravity field is not flat, and this is the basis of the assumptions made in this paper.
The gravity-aided navigation system for this triangle matching algorithm requires 1) local field grid maps that can provide gravity information with a high resolution, 2) a sensitive gravimeter to measure the gravity data in real time, and 3) algorithms that can localize the underwater vehicles on the local field maps using measurements (Hugh et al., Reference Hugh, Louis, Rovert and Daniel2000). The matching algorithms play a very important role in the system. At present there are two prevalent algorithms used by scholars: the Iterated Closest Contour Point (ICCP) algorithm and the Sandia Inertial Terrain Aided Navigation (SITAN) algorithm. The ICCP (Cheng et al., Reference Cheng, Xiu and Luo2009; Garner, Reference Garner2002; Zhao et al., Reference Zhao, Wang and Wang2009) is a sequence iterative matching algorithm, producing a matching path to correct the INS output path only after getting enough samples, which makes its real-time quality poor. SITAN (Wang and Bian, Reference Wang and Bian2008; Liu et al., Reference Liu, Li, Zhang and Hou2011; Larry and Ronald, Reference Larry and Ronald1983) is a real-time matching algorithm, but the position errors it requires are too small.
In this paper, a triangle matching algorithm using local gravity field maps is presented to bound the drift errors inherent in an INS for underwater vehicles. As the ICCP takes a long period of time to estimate the vehicle's position when the initial position error is too large, a concerning problem in a real-time navigation system is how to estimate the unknown initial position error quickly and effectively, so this triangle algorithm tries to solve this initial matching problem and improve real-time quality.
Section 2 gives a detailed description of the triangle matching algorithm. In Section 3, simulations are carried using data for the Bohai Sea and South China Sea areas to assess the effects of different initial errors on the matching solutions. In Section 4, experiments are implemented to evaluate the proposed algorithm. Finally, some suggestions are provided based on the results of simulations and experiments.
2. TRIANGLE MATCHING ALGORITHM
The triangle constraint, which utilizes triangular stability for matching, is widely used in fingerprint matching, global feature matching and computer vision (Guo and Cao, Reference Guo and Cao2012; Liu and An, Reference Liu and An2010). In this paper, however, the triangle constraint is introduced to localize the vehicle's position by using the local gravity field maps. As the INS can accurately give relative distance information in a short time, an accurate triangle can be generated to formulate the constraints in filtrating an established triangle on the local gravity map with the gravity measurements. The navigation errors (Cheng et al., Reference Cheng, Shi, Wang and Dong2010) can then be calculated by matching the established triangle with the accurate triangle.
The triangle matching algorithm has two matching stages, which are the initial match and the tracking match. The initial matching stage aims at providing the initial errors of INS position, especially in case of large initial errors, and the tracking matching stage uses the initial match solution to try to improve the real-time quality of sailing. The two matching stages have been divided into five phases to aid explanation, as shown in Figure 1.
Figure 1. Flow chart of the triangle matching algorithm.
The triangle matching algorithm is processed in the following way: firstly we define three INS indicated points as the original triangle. To accommodate the unknown initial errors of the INS position, a coarse search phase is employed to select a provisional triangle by matching the corresponding gravity measurements of the original triangle points with their nearby gravity contours coarsely and quickly. Then a fine search phase is employed to select an established triangle by matching the provisional triangle with the gravity measurements precisely. Afterwards, the initial position errors can be obtained by matching the established triangle with the original triangle based on the triangle congruent principle for translation and rotation in the matching solution phase. Whereas the tracking matching stage is based on the initial matching stage, the tracking search phase is almost the same as the fine search except for the matching triangles. The tracking matching stage can only be started when the matching reliability, which refers to the similarity of the matching triangles in the initial matching stage, reaches a reliable threshold. This reliable threshold is defined related to the efficiency and the local gravity anomalies information.
2.1. Coarse Match
To address the unknown initial errors, the coarse match is started when the vehicle gets three INS position points, which are not on the same line. Three steps are carried out to get a provisional triangle for the fine match phase, the detailed description is shown in Appendix I. Firstly some definitions are given: Let A,B,C be the three indicated points provided by the INS at the time t 1,t 2,t 3, and let g 1,g 2,g 3 be the corresponding gravity measurements, A 0,B 0,C 0 the actual position points, {P A},{P B},{P C} the matching point sets in the coarse search phase, and P 1, P 2, P 3 the three points of the provisional triangle in the coarse search phase.
Step I: Search three matching point sets, which are used to form a matching triangle set, by matching the gravity measurements of the INS position points with the nearby gravity contours coarsely.
As Figure 2 shows, a square matrix is taken based on the specific grids of a local gravity map to search the sets {P A},{P B},{P C}, and the matrix cell length in this paper occupies 10 grid cells of the gravity map.
Figure 2. The 4th square matrix used for searching point set.
Step II: Form a matching triangle set with the following constraints using the three matching point sets.
To form a matching triangle set {∆P APBPC}, the original triangle ∆ABC provides coarse constraints (Zheng and Gao, Reference Zheng and Gao2009):
$$\left\{ \matrix{\left| {P_A P_B - \left. {AB} \right| \lt r} \right. \cr \left| {P_B P_C - \left. {BC} \right| \lt r} \right. \cr \left| {P_P P_C - \left. {AC} \right| \lt r} \right. } \right.$$where r is the error threshold for the distance between two position points. This error threshold r is related to three sources: errors caused by the matrix cell length, errors caused by the grid resolution of the gravity map, and errors caused by the gravimeter. In the coarse match phase, however, errors caused by the matrix cell length, which are the biggest among the sources, are mainly considered, so r can be given as (Wang et al., Reference Wang, Dong and Wang2008):
$$r = e \times \sigma \times \displaystyle{\pi \over {180}} \times r_e $$where r e=6378165 m is the radius of the Earth, e=10 is the matrix cell length considering the computation, and σ is the grid resolution of the gravity map whose unit is deg/grid. Suppose the matrix rank is N A=N B=N C=3, the matching point sets is P A={P A,1,P A,2}, P B={P B,1,P B,2}, and P C={P C,1}. Thus, the matching triangles {∆P A,iP B,jP C,k} satisfying the coarse constraints are selected from the combinations of any three points in the point sets P A,PB,PC respectively, as Figure 3 shows.
Figure 3. The matching triangles in the coarse match phase.
Step III: To select a provisional triangle ∆P1P2P3 from the matching triangle set {∆P A,iP B,jP C,k}, the matching similarity is provided.
A space-mapping method (Sun et al., Reference Sun, Shan, Chen and Zhu2010), which describes a triangle with two elements in a limited plane space, is proposed to help estimate the similarity between triangles in the matching triangle set {∆P APBPC} and the original triangle ∆ABC. In Figure 4, the original triangle ∆ABC in the spatial domain is mapped to a single point Q(x,y) in the planar domain. Suppose P(x′,y′) is the mapping point of ∆P A,1P B,1P C,1, then a parameter ρ based on the location distance errors between Q(x,y) and P(x′,y′) can be used to estimate the similarity. Finally the provisional triangle ∆P1P2P3 with the largest similarity can be easily identified.
Figure 4. Space-mapping for the original triangle.
2.2. Fine Match
The fine match is employed to select an established triangle by matching the provisional triangle with the gravity measurements precisely. In this phase, the errors caused by the grid resolution of the gravity map and errors caused by the gravimeter are considered to select an established triangle ∆P′1P′2P′3 based on the provisional triangle ∆P1P2P3 information. Some definitions are given below: Let {P′A},{P′B},{P′C} be the matching point sets, and let ∆P′1,P′2,P′3, be the three points of the established triangle in the fine search phase.
Step I: Search three new matching point sets, which are used to form a new matching triangle set, by matching the gravity measurements g 1,g 2,g 3 with the nearby gravity contours around the established triangle points P′1,P′2,P′3, precisely.
Instead of the square matrix in the coarse match phase, a single matrix cell is taken to search the matching point set {P′A}, and the matrix cell is a 10×10 grid matrix on the gravity map. In the same way, the three new matching point sets {P′A},{P′B},{P′C} can be obtained easily.
Step II: Form a new matching triangle set with strict constraints using the new matching point sets.
In this step, the three main constraints in Equation (1) still hold, but the error threshold r must be replaced by r′, which is mainly related to errors caused by the grid resolution and errors caused by the gravimeter.
$$\left\{ \matrix{\left| {P'_{\hskip-2pt A} P'_{\hskip-2pt B} - \left. {AB} \right| \lt r'} \right. \cr \left| {P'_{\hskip-2pt B} P'_{\hskip-2pt C} - \left. {BC} \right| \lt r'} \right. \cr \left| {P'_{\hskip-2pt A} P'_{\hskip-2pt C} - \left. {AC} \right| \lt r'} \right. } \right.$$
$$r' = \left( {1 + \displaystyle{\zeta \over \tau}} \right) \times \sigma \times \displaystyle{\pi \over {180}} \times r_e $$ξ is the error from the gravity measurement whose unit is mGal, and τ is the gravity difference between two grids in the local gravity map whose unit is mGal / grid.
The new matching triangles {∆P′A,iP′B,jP′C,k} satisfying the strict constraints in Equation (3) are selected from the combinations of any three points in the point sets {P′A},{P′B},{P′C} respectively.
Step III: To select an established triangle ∆P′1P′2P′3 from the new matching triangle set, the matching similarity is also provided, as in step III of the coarse match phase.
In this step, an established triangle ∆P′1P′2P′3 with the largest similarity is identified. Meanwhile the similarity ρ′ between ∆P′1P′2P′3 and ∆ABC also plays a key role in dominating the whole matching process, so a reliable threshold ρ 0=0·8 is defined according to the matching precision. If ρ′ is larger than ρ 0 the program goes into the tracking matching stage; otherwise it goes back to the coarse match phase.
2.3. Matching Solution
To calculate the navigation errors, a value function is introduced to match the established triangle ∆P′1P′2P′3 with the original triangle ∆ABC based on the triangle congruent principle of translation and rotation. Let d be the translation vector, R be the rotation matrix, and ∆Q 1Q 2Q 3 be the original triangle ∆ABC for the sake of description.
As Figure 5 shows, suppose p′1,p′2,p′3 are the position vectors of ∆P′1P′2P′3 points, q1,q2,q3 are the position vectors of ∆Q 1Q 2Q 3 points, q′1,q′2,q′3 are the position vectors of ∆Q′1Q′2Q′3, which is the transformation result of the triangle ∆Q 1Q 2Q 3, and q0 is the position vector of Q 0, which is the barycentre of triangle ∆Q 1Q 2Q 3. The value function T(d,R), which is related to the translation vector d and the rotation matrix R, is defined with the sum of distance errors between p′1,p′2,p′3 and q′1,q′2,q′3.
$$T({\bi d},{\bi R}) = \sum\limits_{i = 1}^3 {\left\| {{\bi p}'_i} \right. -} \left. {{\bi q}'_i} \right\|^2 $$As the triangle ∆Q′1Q′2Q′3 is obtained by the triangle congruent principle of translation and rotation,
$${\bi q'}_{\hskip-3pt i} = {\bi d} + {\bi q}_0 + {\bi R}({\bi q}_i - {\bi q}_0 )$$
Figure 5. Triangle transformations by translating and rotating.
Where 
${\bi q}_0 = \displaystyle{1 \over 3}\mathop \sum \limits_{i = 1}^3 {\bi q}_i $ is the barycenter of the triangle ∆Q 1Q 2Q 3, so the value function can be expressed as
$$T({\bi d},{\bi R}) = \sum\limits_{i = 1}^3 {\left\| {{\bi p'}_{\hskip-3pt i} - {\bi d} - {\bi q}_0 - {\bi R}\left. {({\bi q}_i - {\bi q}_0 )} \right\|^2} \right.} $$Then, the translation vector d and the rotation matrix R can be calculated by minimizing the value function T(d,R) and approaching the singular value decomposition, the detailed process is shown in Appendix II. Let ∆λ be the longitude error, ∆L the latitude error, ∆θ the yaw error, then the navigation error can be obtained from vector d and matrix R.
$${\bi d} = \left[ \matrix{{\rm \Delta} \lambda \cr {\rm \Delta} L } \right],{\bi R} = \left[ {\matrix{ {\cos {\rm \Delta} \theta} & {\sin {\rm \Delta} \theta} \cr { - \sin {\rm \Delta} \theta} & {\cos {\rm \Delta} \theta} \cr}} \right]$$So ∆λ, ∆L are the position errors localized by the triangle matching algorithm, and ∆θ is the yaw error.
2.4. Tracking Match
The tracking match phase mainly aims to track the vehicle's path in real-time based on the initial matching results.
In this phase the two previous established points P′2, P′3 are fixed. Suppose that D is the running point provided by the INS following A,B,C. Then P 4 is its tracking point, which is revised with the navigation errors calculated in the initial stage; {P′D} is its tracking point set searched on the gravity map with the isolated gravimeter measurement g 4, and P′4 is the point used to form the established triangle ∆P′2P′3P′4.
Step I: Get the tracking point set using the initial navigation errors, in order to track the vehicle's path.
Let q4 be the position vector of D and p4 be the position vector of tracking point P 4, which is approximately revised with the navigation errors d, which is obtained from the initial matching stage.
$${\bi p}_4 = {\bi d} + {\bi q}_4 $$Then the tracking point set {P′D} can be obtained by matching the corresponding measurement g 4 with the gravity field map, and its searching range is in a matrix cell using P 4 as the central marker.
Step II: Form the tracking triangle set with the previous points.
Then the tracking triangle set {∆P′2P′3P′D} is formed by adding a tracking point set P′D={P′D,1,P′D,2,P′D,3}. r is the error threshold, then the constraints can be simplified as
$$\left\{ \matrix{\left| {P'_2} \right.P'_{\hskip-2pt D} - \left. {BD} \right| \lt r' \cr \left| {P'_3 P'_{\hskip-3pt D} - \left. {CD} \right| \lt r'} \right. } \right.$$Step III: To select the established triangle ∆P′2P′3P′4 from the tracking triangle set.
This step is the same as step III in the coarse match phase, and the established triangle ∆P′2P′3P′4 with the largest similarity is finally identified. Meanwhile, if the similarity ρ′ between ∆P′2P′3P′4 and ∆BCD is larger than threshold ρ 0, the triangle matching algorithm goes on in the tracking matching stage; otherwise the algorithm must be stopped or restarted.
2.5. Matching Solution
This phase is absolutely the same as the matching solution phase in the initial matching stage, and it calculates the navigation errors by matching the established triangle ∆P′2P′3P′4 with the original triangle ∆BCD in real-time. The INS position errors can be adjusted with the navigation errors.
3. SIMULATIONS
To assess the matching algorithms, a simulation system was configured. As Figure 6 shows, three types of data, the gravity field maps, the INS output path, and the gravity measurements, were used to implement the matching algorithms. In this paper, the Kriging algorithm (Raty and Kangas, Reference Raty and Kangas2012; Panagiotopoulou and Anastassopoulos, Reference Panagiotopoulou and Anastassopoulos2007) is employed to construct local gravity field maps of high resolution using sparse and regular grid data provided by the Scripps Institution of Oceanography. The actual path is generated by a track making software, which simulates the manoeuvring of underwater vehicles (Anthony and David, Reference Anthony and David1993; Li et al., Reference Li, Bian, Shi and Qin2007), and the output path is obtained by adding position errors to the actual path directly. Finally the gravity measurements are simulated by sampling gravity data from the gravity field maps along the actual path and adding the gravity noise from the gravimeter measurements.
Figure 6. The simulation system.
It is important to note that in this paper the vehicle is assumed to be an autonomous underwater vehicle (AUV), which is used to do some scientific tests, and all of the simulations ignore waves and current. Key assumptions are:
1) The grid resolution of the original gravity field maps, which are provided by the Scripps Institution of Oceanography, is just 0·016° per grid (0·016°/grid). We densified the original gravity field maps into 0·002°/grid using the Kriging algorithm.
2) The gravimeter is simulated by sampling the gravity data from the gravity field maps along the actual path and adding the gravity noise regardless of the influence from waves and current. The gravity noise added occupies the normal distribution with variance 0·01mGal, the speed of the AUV is 60 km/h, the time for sampling is 150 s, and all the simulations will be implemented for as long as 3600 seconds.
The simulations consist of two parts: the simulation of the triangle matching algorithm and the comparison of three different algorithms. The first simulation contains three tests and is mainly implemented in the Bohai Sea with the INS initial error changing from 0·05° to 0·002°, as 0·002° is the grid resolution of the gravity map, and 0·05° corresponds to an INS system with drift error 0·01°/h working for 5 hours without aided navigation. The comparison contains two groups of simulations, which are implemented in the Bohai Sea and the South China Sea with three different algorithms respectively.
3.1. Simulations of the Triangle Matching Algorithm
In this part, the simulations are all implemented in the Bohai Sea to evaluate the triangle matching algorithm. Here is some information on the actual path: the longitude of the initial point is 119°, and the latitude is 38°. Then the INS output path: the slope error is 0·01°/h, the random error is 0·001°, and the constant errors are 0·05°, 0·01° and 0·02° respectively.
Figure 7 shows all the paths on the gravity map for the triangle matching algorithm. Figure 8 shows all the results of the triangle matching algorithm in test one, Figure 8(a) plots the INS added errors and remainder errors, which are the differences between the added errors and the matching errors, of the triangle matching algorithm on longitude, Figure 8(b) plots both the errors on latitude, Figure 8(c) plots yaw errors, Figure 8(d) plots the processing time for each point. Similarly Figures 9 and 10 show the longitude and latitude errors of the triangle matching algorithm for tests two and three.
Figure 7. The matching results of the triangle matching algorithm.
Figure 8. Matching results for the triangle matching algorithm with initial error 0·05°.
Figure 9. Matching results for the triangle matching algorithm with initial error 0·01°.
Figure 10. Matching results for the triangle matching algorithm with initial error 0·002°.
From Figures 8 to 10, we can see that the remainder errors of longitude and latitude given by the triangle matching algorithm can be decreased to 20% when the INS initial error is 0·05°, and that it is the initial matching stage that plays a very important role in accommodating the unknown initial errors. As the INS initial errors changed from 0·05° to 0·002°, the remainder errors in the 0·002° share a larger part of the added errors; it is the measurement error of the gravimeter and the grid resolution of the gravity map that mainly affect the matching results.
Meanwhile the triangle matching algorithm gives useless yaw errors, which are calculated by approaching the singular value decomposition. In any case, the processing time in the whole process keeps very stable since the third sampling point is valid, because the triangle matching algorithm uses only three points of information for every matching process. Above all, the triangle matching algorithm has some compelling advantages: (1) It can work under a wide range of initial errors, from 0·05° to 0·002°, while the SITAN cannot work for 0·05°. (2) The processing time of this algorithm stays stable, which means the triangle matching algorithm could probably be utilized in a real-time navigation system.
3.2. Simulations for Comparing the Algorithm
In this part, two groups of simulations are implemented in Bohai Sea and South China Sea in order to compare the triangle matching algorithm with the prevalent ICCP and SITAN algorithms. The INS output path has a slope error of 0·01°/h, and the random error occupies the normal distribution with variance 0·001°. There are three tests with the INS initial error (Lon/deg, Lat/deg) changing from 0·05° to 0·002° in each group. In Tables 1 and 2, the remainder error ε(deg)=|e 1−e 2| represents the difference between the added position error e 1 and the matching result e 2 in degree, and η=ε/e 1 is the percentage of the remainder error ε(deg) related to the added position error. It is important to note that the results given in the tables are all mean values, as is the yaw error (deg), reliability (%), and processing time (ms) for each point. Otherwise, the SITAN algorithm, which cannot work when the initial error is 0·05°, is absent.
Table 1. Simulation results in the Bohai Sea district.
Table 2. Simulation results in the South China Sea district.
Group I: Simulations for comparison in the Bohai Sea.
Figure 11(a) gives the gravity anomalies in Bohai Sea, which is a 1448×1448 grid map with the longitude changing from 118° to 121°E and the latitude changing from 37° to 40°N. Figure 11(b) gives the actual path on the local gravity contour, the longitude of the initial point is 119°E, the latitude is 38°N, and the total simulation time is 3600 seconds.
Figure 11. Gravity field map in Bohai Sea and the actual path.
From Table 1 we can see that the triangle matching algorithm can decrease the position errors to 10–40%, as can the ICCP algorithm. However, the remainder errors given by the triangle matching algorithm are much smaller than the errors given by ICCP when the initial error is 0·05°, and the remainder errors given by SITAN are better than both the triangle and ICCP algorithms when the initial error is 0·002°. After all, when the initial error is 0·01° or 0·002°, the remainder errors ε(deg) for all three algorithms keep within 0·002°–0·005°, which means the distance error is limited to within 200–500 m.
Group II: Simulations for comparison in the South China Sea.
Figure 12(a) gives the gravity anomalies in the 3D model, which is a 1448×1448 grid map with the longitude changing from 112°to 115°E and the latitude changing from 12° to 15°N. Figure 12(b) shows the actual path on the local gravity contour, the longitude of the initial point is 113°E, the latitude is 13°N, and the total simulation time is 3600 seconds.
Figure 12. Gravity field map in South China Sea and the actual path.
From Table 2 we can see that the yaw errors given by the ICCP algorithm are much smaller than the errors given by the triangle matching algorithm, because the ICCP using a number of points in the process can result in precise and stable yaw errors. In terms of reliability, which is generated in different ways, we cannot make a comparison; the triangle matching algorithm gives its similarity information as the reliability, whereas ICCP and SITAN give their reliability by combining the difference between the actual path and the matching path. Regarding the processing time, which plays a very important role in assessing the algorithms, the triangle matching algorithm and SITAN are almost the same, whereas the ICCP algorithm costs too much.
4. EXPERIMENTS
To test the triangle matching algorithm, two groups of experiments were implemented. A land vehicle was used to run the path, and it carried a GPS receiver and the INS, which contains the Inertial Measurement Unit (IMU), the Processing Computer System (PCS), the PCS battery and a monitoring computer. The PCS collected the GPS data and the IMU data, then sent this to the Monitoring computer for storage, as shown in Figure 13.
Figure 13. The experimental instruments.
Firstly, the real gravity information is provided by the Scripps Institution of Oceanography, and its grid resolution is densified from 0·016°/grid to 0·002°/grid using the Kriging algorithm. Secondly, GPS single-point positioning is used to provide the actual path, as its positioning error is less than five metres, which is small enough to be negligible compared to the map grid resolution of 200 metres. Then, the INS position information is used to provide the output path, which contains the initial errors, constant errors (about 0·01°/h) and other real errors. Finally, the gravimeter is simulated by sampling along the actual path and adding the gravity noise (normal distribution with variance 0·01mGal) ignoring waves, current, and so on.
We employed the Extend Kalman Filter (EKF) (Zhang et al., Reference Zhang, Chen, Sun and Yan2011; Yuan et al., Reference Yuan, Zhang, Yuan and Tao2011; Gyung and Hang, Reference Gyung and Hang2006) to integrate the matching solution and the INS position information. So there are three kinds of integrated navigation results, such as ICCP/INS, Triangle/INS (TRI/INS) and SITAN/INS, otherwise the integrated navigation result of GPS/INS is used as a reference. In the following, two groups of experiments are implemented in two places, one running from Beijing to Chengde district for 120 km, the other running from Beijing to Xuanhua district for 180 km. There are three tests in each group with different INS initial errors. In Tables 3 and 4, the remainder error ε(m) represents the difference between the INS position error and the navigation results in metres, and η is the percentage of the remainder error ε(deg) related to the INS position error. p(m) is the remaining position error in metres. It is important to note that the INS position errors and the navigation results given in the table are all mean values.
Table 3. Navigation results for the Beijing-Chengde path.
Table 4. Navigation results for the Beijing-Xuanhua path.
Group I: Beijing to Chengde path.
We spent about 180 minutes to run from Beijing to Chengde at a speed of 70 km/h. In Figure 14(a), the broken line is the INS output path, and the red line is the navigation result of TRI/INS. In Figure 14(b), the broken line is the INS position errors, and the red line is the navigation errors of TRI/INS.
Figure 14. Beijing to Chengde navigation results of TRI/INS.
In Table 3, the whole Beijing to Chengde path is divided into three segments of about 60 min for each segment, to test the matching algorithms for different initial errors. We can see that the TRI/INS navigation gives position errors smaller than ICCP/INS and SITAN/INS navigations, mainly because of the coarse and fine match phase in the initial matching stage. After all, the triangle matching algorithm keeps the remainder errors of INS position within 200 m–700 m, decreasing to 5–25%.
Group II: Beijing to Xuanhua path.
The INS system takes ten minutes to finish its alignment, after that, we spent about 200 minutes to run from Beijing to Xuanhua at a speed of 70 km/h. Figure 15 shows the TRI/INS navigation results.
Figure 15. Beijing to Xuanhua navigation results of TRI/INS.
From Table 4, we can see that the GPS/INS navigation results keep η less than 1%. The TRI/INS keeps η around 10%, and its remaining position errors are around 200 m–400 m, which is meaningful considering the 0·002°/grid map resolution. The ICCP/INS and SITAN/INS are all bigger than 40%, so the TRI/INS behaves the best among the three navigation solutions.
5. CONCLUSION
Simulations show that the triangle matching algorithm can work under a wide range of initial errors, from 0·05° to 0·002°. The initial matching stage plays a very important role in accommodating the unknown initial errors. The comparison simulations show that the triangle matching algorithm gave matching solutions almost the same as with the ICCP algorithm, whereas the SITAN algorithm works much better than the other two in cases where the initial error is 0·002°. The processing time of the triangle matching algorithm remains stable, as with the SITAN algorithm, whereas the ICCP algorithm needs more time. In the experiments, the triangle matching algorithm behaves better than the prevalent ICCP and SITAN algorithm.
The study presented in this paper still uses simulated gravimeter data ignoring waves and current. Therefore, further study is needed to evaluate the triangle matching algorithm under the conditions of waves and current. Finally, we suggest that a combination of the triangle matching algorithm and the SITAN algorithm may be a feasible plan to improve the practicability of gravity-aided navigation. For example, the triangle matching algorithm captures the actual path when the INS position error is larger than 0·01°, otherwise the SITAN is used.
ACKNOWLEDGEMENTS
The authors thank the Scripps Institution of Oceanography and the University of California San Diego for providing the original gravity field maps. The work reported here has been supported by the Foundation for Innovative Research Groups of the National Natural Science Foundation of China under grant number 61121003.
APPENDIX I STEPS FOR THE COARSE MATCH IN THE INITIAL MATCHING STAGE
To address the unknown initial errors, the coarse match is started when the vehicle gets three INS position points, which are not on the same line. Three steps are carried out to get a provisional triangle for the fine match phase. Firstly some definitions are given: Let A, B, C be the three indicated points provided by the INS at the time t 1, t 2, t 3, and let g 1, g 2, g 3 be the corresponding gravity measurements, A 0, B 0, C 0 the actual position points, {P A},{P B},{P C} the matching point sets in the coarse search phase, and P 1, P 2, P 3 the three points of the provisional triangle in the coarse search phase.
Step I: Search three matching point sets, which are used to form a matching triangle set, by matching the gravity measurements of the INS position points with the nearby gravity contours coarsely.
Use the indicated point A, shown in Figure 2, as an example. A square matrix whose central point is A is taken to search the point set based on the grids of a local gravity map. Then the matrix rank N and the matrix cell length e tend to be key factors in addressing the unknown initial errors in the initial match coarsely by increasing the rank N until point set {P A} is composed or the maximum rank is reached. Figure 2 shows a square matrix with a rank of 4, the solid lines are iso-contours responding to g 1, the broken lines are other iso-contours on the gravity map, and the dashed points on the iso-contours are the middle points in their respective responding cells. The dashed points compose the point set {P A}.
The matrix cell length in this paper occupies 10 grid cells of the gravity map, considering the calculation, and the maximum rank N max is estimated referring to the INS system characteristic (Heung et al., Reference Heung, Jang and Chan2002).
$$N_{\max} = \left[ {2\displaystyle{{{\rm \Delta} t \times \delta} \over {e \times \sigma}}} \right] + 1$$Where ∆t is the navigation time since the INS navigates without any other bounding method, δ is the drift error of the INS system whose unit is °/h, σ is the grid resolution of the gravity map whose unit is °/grid, e is the matrix cell length, e=10, and [•] is an operation of rounding the elements to the nearest integer towards zero.
In the same way, the matching point sets {P A} {P B} {P C} responding to A, B, C can all be obtained.
Step II: Form a matching triangle set with the following constraints using the three matching point sets.
To form a matching triangle set {∆P APB PC}, the original triangle ∆ABC provides constraints (Wang et al., Reference Wang, Dong and Wang2008):
$$\left\{ \matrix{\left| {P_A P_B - \left. {AB} \right| \lt r} \right. \cr \left| {P_B P_C - \left. {BC} \right| \lt r} \right. \cr \left| {P_A P_C - \left. {AC} \right| \lt r} \right. } \right.$$where r is the error threshold for the distance between two position points. This error threshold r is related to three sources: errors caused by the matrix cell length, errors caused by the grid resolution of the gravity map, and errors caused by the gravimeter. In the coarse match phase, however, errors caused by the matrix cell length are mainly considered, so r can be given as (Sun et al., Reference Sun, Shan, Chen and Zhu2010):
$$r = e \times \sigma \times \displaystyle{\pi \over {180}} \times r_e $$where r e=6378165 m is the radius of the Earth, e=10 is the matrix cell length considering the computation, and σ is the grid resolution of the gravity map whose unit is °/grid. Suppose the matrix rank is N A=N B=N C=3, the matching point sets is P A={P A,1, P A,2}, P B={P B,1, P B,2}, and P C={P C,1}. Thus, the matching triangles {∆P A,iP B,jP C,k} satisfying the coarse constraints are selected from the combinations of any three points in the point sets P A,PB,PC respectively, as Figure 3 shows.
Step III: To select a provisional triangle ∆P1P2P3 from the matching triangle set {∆P A,iP B,jP C,k}, the matching similarity is provided.
To select a provisional triangle ∆P1P2P3 in this coarse search phase, a space-mapping method, which describes a triangle with two elements in a limited plane space, is proposed to help estimate the similarity between triangles in the matching triangle set {∆P APBPC} and the original triangle ∆ABC.
In Figure 4, the original triangle ∆ABC is mapped to a single point Q(x,y) in the plane space. Let AB=a, BC=b, AC=c, and suppose is the maximum one among the three sides. The mapping (Raty and Kangas, Reference Raty and Kangas2012) function can then be defined.
$$[x,y] = \Gamma (a,b,c)$$
$$x = \displaystyle{a \over c},\quad y = \displaystyle{b \over c}$$Suppose P(x′,y′) is the mapping point of ∆P A,1P B,1P C,1. Then Q(x,y) and P(x′,y′) must be the same when the two corresponding triangles are congruent, as the matching triangles contain variety of errors, a parameter ρ based on the location distance errors between Q(x,y) and P(x′,y′) can be used to estimate the similarity through a function f.
$$\rho = 1 - f\,(\varepsilon _x, \varepsilon _y )$$
$$f(\varepsilon _x, \varepsilon _y ) = \sqrt {\displaystyle{{\varepsilon _x^2 + \varepsilon _y^2} \over 2}} $$
$$\varepsilon _x = 2\displaystyle{{x - x'} \over {x + x'}},\quad\varepsilon _y = 2\displaystyle{{y - y'} \over {y + y'}}$$Finally the provisional triangle ∆P1P2P3 with the largest similarity can be easily determined.
APPENDIX II CALCULATION OF TRANSLATION VECTOR AND ROTATION MATRIX
To calculate the navigation errors, a value function is introduced to match the established triangle ∆P′1P′2P′3 with the original triangle ∆ABC based on the triangle congruent principle of translation and rotation. Let d be the translation vector, R be the rotation matrix, and ∆Q 1Q 2Q 3 be the original triangle ∆ABC for the sake of description. As Figure 5 shows, suppose p′1,p′2,p′3 are the position vectors of ∆P′1P′2P′3 points, q1,q2,q3 are the position vectors of ∆Q 1Q 2Q 3 points, q′1,q′2,q′3 are the position vectors of ∆Q′1Q′2Q′3, which is the transformation result of the triangle ∆Q 1Q 2Q 3, and q0 is the position vector of Q 0, which is the barycentre of triangle ∆Q 1Q 2Q 3.
Step I: The value function T(d,R), which is related to the translation vector d and the rotation matrix R, is defined with the sum of distance errors between p′1,p′2,p′3 and q′1,q′2,q′3.
$$T({\bi d},{\bi R}) = \sum\limits_{i = 1}^3 {\left\| {\left. {{\bi p'}_{\hskip -3pt i} - {\bi q'}_{\hskip -3pt i}} \right\|^2} \right.} $$As the triangle ∆Q′1Q′2Q′3 is obtained by the triangle congruent principle of translation and rotation,
$${\bi q'}_{\hskip -3pt i} = {\bi d} + {\bi q}_0 + {\bi R}({\bi q}_i - {\bi q}_0 )$$ Where 
${\bi q}_0 = \displaystyle{1 \over 3}\mathop \sum \limits_{i = 1}^3 {\bi q}_i $, so the value function can be expressed as
$$T({\bi d},{\bi R}) = \sum\limits_{i = 1}^3 {\left\| {\left. {{\bi p'}_{\hskip -3pt i} - {\bi d} - {\bi q}_0 - {\bi R}({\bi q}_i - {\bi q}_0 )} \right\|^2} \right.} $$Step II: Calculate the translation vector by minimizing the value function.
The translation vector is calculated by minimizing the value function Equation (7), which means
$$\displaystyle{{\partial T({\bi d},{\bi R})} \over {\partial {\bi d}}} = 0$$Then the result is
$${\bi d} = \displaystyle{1 \over 3}\sum\limits_{i = 1}^3 {({\bi p'}_{\hskip -3pt i}} - {\bi q}_0 - {\bi R}({\bi q}_i - {\bi q}_0 ))$$ Let 
${\bi p'}_{\hskip -2pt 0} = \displaystyle{1 \over 3}\mathop \sum \limits_{i = 1}^3 {\bi p'}_{\hskip -2pt i}, {\bi q}_0 = \displaystyle{1 \over 3}\mathop \sum \limits_{i = 1}^3 {\bi q}_i\hskip-3pt $. Then P′0,q0 are barycentres of ∆P′1P′2P′3 and ∆Q 1Q 2Q 3, respectively, and the translation vector is implied as
$${\bi d} = {\bi p'}_{\hskip -3pt 0} - {\bi q}_0 $$Step III: Calculate the rotation matrix by substituting the translation vector into the value function.
Let 
${\bi \tilde p'}_{\hskip -2pt i} = {\bi p'}_{\hskip -2pt i} - {\bi p'}_{\hskip -2pt 0} $, 
${\bi \tilde q}_i = {\bi q}_i - {\bi q}_0 $, 
$MC = \mathop \sum \limits_{i = 1}^3 \left(| {{\bi \tilde p'}_{\hskip -2pt i} \left\| {^2 +} \right.} \right.\left\| {{\bi \tilde q}_i \left\| {^2 )} \right)} \right.$, 
$MV = \mathop \sum \limits_{i = 1}^3 ({\bi \tilde p'}^T _{\hskip -2pt i} {\bi R\tilde q}_i )$. Then the value function can be
$$T({\bi d},{\bi R}) = \sum\limits_{i = 1}^3 {\left( {\left\| {\left. {{\bi \tilde p'}_{\hskip -3pt i}} \right\|^2 + \left\| {\left. {{\bi \tilde q}_{\bi i}} \right\|} \right.} \right.^2} \right)} - 2{\bi \tilde p'}^T _{\hskip -2pt i} {\bi R\tilde q}_i ) = MC - 2MV$$As MC is a constant value when the two matching triangles are determined, MV must be maximized to minimize the value function.
$$MV = \sum\limits_{i = 1}^3 {\left[ {Trace({\bi \tilde p'}_{\hskip -2pt i} {\rm}\, {\bi \tilde q}_{i}^T {\bi R}^T )} \right] = Trace\left[ {\sum\limits_{i = 1}^3 {({\bi \tilde p'}_{\hskip -2pt i\hskip2pt}} {\rm} {\bi \tilde q}_i^T ){\bi R}^T} \right]} $$ Let 
${\bi S} = \mathop \sum \limits_{i = 1}^3 ({\bi \tilde p'}_{\hskip -2pt i\hskip2pt} {\bi \tilde q}^T _i )$ and its singular value decomposition be S=UWVT; U and V are all orthogonal matrices, W=diag(w 1,w 2,w 3), w i⩾0. Then,
$$\eqalign{MV = & Trace({\bi SR}^T ) = Trace({\bi UWV}^T {\bi R}^T ) \cr = & Trace({\bi WV}^T {\bi R}^T {\bi U}) \leqslant Trace({\bi W})} $$Apparently MV can be maximized only if VTRTU=I, so the rotation matrix can be expressed as
$${\bi R} = {\bi UV}^T $$Step IV: Get the navigation error from the calculation results.
The navigation error of the INS system can be obtained from the translation vector d and rotation matrix R. Let ∆λ be the longitude error, ∆L the latitude error, ∆θ the yaw error. Then
$${\bi d} = \left[ {\matrix{ {\Delta \lambda} \cr {\Delta L} \cr}} \right],\quad{\bi R} = \left[ {\matrix{ {\cos \Delta \theta} & {\sin \Delta \theta} \cr { - \sin \Delta \theta} & {\cos \Delta \theta} \cr}} \right]$$So ∆λ, ∆L are the position errors localized by the triangle matching algorithm, and ∆θ is the yaw error.
