1. INTRODUCTION
Inertial Navigation Systems (INSs) are the primary navigation methods for Autonomous Underwater Vehicles (AUVs). However, the navigation error of INS increases with operating time during extended AUV missions. Therefore, external aiding information is necessary to limit the INS error accumulation. Contrary to the situation on the water surface, where satellite and some other radio navigation signals are easily available, the problem of underwater navigation is more complicated due to the requirement of continuous underwater operation for long periods. Sometimes it is unacceptable for the AUV to acquire the navigation updates by surfacing or nearly surfacing (Wu et al., Reference Wu, Tian, Ma and Tian2010; Hays et al., Reference Hays, Schmidt, Wilson, Campbell, Heckman and Gokhale2002; Satow, Reference Satow1951). However, gravimetry and magnetometry are totally non-emanating and passive, they do not need the AUV to surface. Gravity and geomagnetic fields vary from place to place around the Earth's surface and this is recognizable by matching algorithms (Liu et al., Reference Liu, Wu, Hu and Xie2007; Huang et al., Reference Huang, Sun and Hao2010). Thus gravity and geomagnetic information can be employed to construct the passive aided navigation systems for AUV.
The design of appropriate and accurate navigation algorithms is one of the most crucial parts in gravity/geomagnetic aided navigation systems. Presently, the most frequently used algorithms based on map matching technology are Terrain Contour Matching (TERCOM) and Iterated Closest Contour Point (ICCP) (Deng et al., Reference Deng, Ge, Guan and Han2010). Although both methods are able to achieve credible matching results, a sequence of gravity or geomagnetic measurements that are collected in advance is a necessary condition, so the matching process is by no means simultaneous (Zhao et al., Reference Zhao, Wang and Wang2009). In addition to map matching technology, an Extended Kalman Filter (EKF) is another choice to obtain an estimated position in underwater aided navigation. It handles the issue under nonlinear filter theory and can estimate the INS navigation error in real time. However, EKF is very sensitive to initial and process errors; small errors may lead to divergence, so the reliability and stability of this method cannot be ensured (Wu et al., Reference Wu, Tian, Ma and Tian2011).
The focus of previous researches has been on Gravity Aided Navigation (GravAN) or Geomagnetism Aided Navigation (GeomAN) individually (Feng et al., Reference Feng, Yang and Fang2008; Hays et al., Reference Hays, Schmidt, Wilson, Campbell, Heckman and Gokhale2002). The aided navigation result is much related to the gravity or geomagnetic features along the trajectory. In some regions of insufficient gravity or geomagnetism variation, if these two methods were combined together better navigation performance could be expected. Consequently, the Gravity and Geomagnetic Combined Aided Navigation (GGCAN) is proposed in this paper. There are two main procedures in GGCAN. First, Multiple Model Adaptive Estimation (MMAE) is employed as a navigation algorithm, in which multiple Kalman filters are formed and the optimal navigation positions are estimated separately from GravAN and GeomAN. Secondly, the results from GravAN and GeomAN are combined with the weighted average method derived from the Optimal Weight Allocation Principle (OWAP) and the final GGCAN position result can be obtained.
Weighted average methods were extensively used in multi-sensor data fusion, information retrieval and decision making (Hall and Llinas, Reference Hall and Llinas2001; Larsen, Reference Larsen2011). With simple weighted average methods like Equal Weighted (EW), the accuracy of fusion result may be less than that of one of the inputs. Thus a better weighted method is needed so that the accuracy of the GGCAN result can be better than either of GravAN and GeomAN. A weighted average method based on OWAP is explored in this study. This method is more effective and convenient. Moreover, the accuracy difference between GravAN and GeomAN is fully considered.
Two regions in the West Pacific Ocean were selected for the simulation and the performance and reliability of GGCAN is discussed. The improvement in the success rate and position accuracy show that OWAP is more appropriate for GGCAN than the equal weighted method, and GGCAN (OWAP) presented in this study is more accurate and robust than GravAN or GeomAN.
2. PRINCIPLES OF GGCAN
In GravAN and GeomAN, the gravity and geomagnetic data are collected point by point with onboard sensors, as the AUV moves. After pre-processing and calibrations, the measured anomalies are matched with the background anomaly prepared previously, and the optimal estimated positions are obtained with MMAE. Thereafter, the estimated positions are used to update the INS navigation information. In the proposed GGCAN method, both gravity anomaly and geomagnetic anomaly are involved. The GGCAN system consists of INS, aided navigation background databases, gravity/geomagnetic sensors, aided navigation algorithms (MMAE and OWAP), and a navigation processor. A block diagram of GGCAN is presented in Figure 1. The measured gravity/geomagnetic anomalies and background databases are sent to MMAE modules. Multi-module Kalman filters are employed here in the MMAE technology to ensure that reliable position estimations are obtained from GravAN or GeomAN. Then a weight function that is derived from the OWAP is adopted to compute the final GGCAN combined positions from the results of GravAN and GeomAN. Finally, the combined positions are used to modify the errors of the INS.
Block Diagram of GGCAN.
3. GRAVAN AND GEOMAN ALGORITHM
3.1. Gravity Anomaly Grid Map and Geomagnetic Anomaly Grid Map
Gravity and geomagnetic background databases are the most basic and fundamental elements of the GGCAN system. In this research, a gravity anomaly grid map and geomagnetic anomaly grid map are selected as the databases.
With the appearance and development of satellite altimetry, the resolution of the Gravity Anomaly Grid (GAG2) in ocean area from altimetry inversion has already reached 2′ × 2′. Therefore, a KMS2002 gravity anomaly grid map with 2′ × 2′ resolution is chosen for GGCAN in this paper (Andersen et al., Reference Andersen, Knudsen and Trimmer2005).
The Earth Magnetic Anomaly Grid (EMAG2) was released by National Geographic Data Center (NGDC) in March 2009; EMAG2 is compiled from satellite, continental, marine data and specified as a global 2′ × 2′ resolution grid of the anomaly of the total magnetic intensity (Maus et al., Reference Maus, Barckhausen, Berkenbosch, Bournas, Brozena, Childers, Dostaler, Fairhead, Finn, von Frese, Gaina, Golynsky, Kucks, Lühr, Milligan, Mogren, Müller, Olesen, Pilkington, Saltus, Schreckenberger, Thébault and Caratori Tontini2009). So the EMAG2 geomagnetic anomaly grid map with 2′ × 2′ resolution (EMAG2, 2011) was utilized for the GGCAN study.
3.2. GravAN and GeomAN Algorithm Based on MMAE
Kalman filters are widely used in integrated navigation systems. In this paper a Kalman filter and MMAE are utilized to construct GravAN and GeomAN algorithms. Firstly, a confidence area was formed and the size was determined by the INS position Circular Error Probability (CEP) and Rayleigh Distribution; it can be seen in Figure 2. Then, Kalman filters were constructed at every regularly arranged grid in the confidence area. The measurements for Kalman measurement equations are composed from the differences between the measured anomaly and the anomalies of the filters. The system model for the Kalman filter can be provided as follows:
$$ X = \left( \matrix{X_{1,k} \hfill \cr X_{2,k} \hfill} \right){\rm =} \phi _{k,k - 1} \left( \matrix{X_{1,k - 1} \hfill \cr X_{2,k - 1} \hfill} \right) + G_{k - 1} \left( \matrix{W_{1,k - 1} \hfill \cr W_{2,k - 1} \hfill} \right)$$
$$Z = \left( \matrix{Z_{1,k} \hfill \cr Z_{2,k} \hfill} \right) = H_k \left( \matrix{X_{1,k} \hfill \cr X_{2,k} \hfill} \right) + \left( \matrix{V_{1,k} \hfill \cr V_{2,k} \hfill} \right)$$The subscripts 1 and 2 indicate GravAN and GeomAN respectively in the whole paper, where:
X – Actual value of gravity/geomagnetic anomaly and it is treated as the state vector.
Z – The gravity/geomagnetic measurement vector.
W – System noise. W 1,k = N(0,q 1) and W 2,k = (0,q 2), Q=diag(q 1,q 2).
V – Measurement noise. V 1,k = N(0,r 1) and V 2,k = (0,r 2), R=diag(r 1,r 2).
Sketch Map of Confidence Interval and Validate Location.
The noises are assumed to be uncorrelated with each other. Five Kalman filter equations are listed as follows:
$$\left\{ \matrix{X_k^ - = \phi X_{k - 1} \hfill \cr P_k^ - = \phi P_{k - 1} \phi ^T + Q \hfill \cr K_k = P_k^ - H^T \hskip-2pt /(HP_k^ - H^T + R) \hfill \cr X_k = X_k^ - + K_k (Z_k - HX_k^ - ) \hfill \cr P_k = (I - K_k H)P_k^ - \hfill} \right.$$Kalman filter computations are executed in every filter individually. Thereafter, the filtering residuals are defined as:
$$\left( \matrix{\delta _{1,k + 1} \hfill \cr \delta _{2,k + 1} \hfill} \right) = \left( \matrix{Z_{1,k + 1} - X_{1,k + 1/k} \hfill \cr Z_{2,k + 1} - X_{2,k + 1/k} \hfill} \right)$$where:
δ 1,k + 1 = N(0,P 1,k/k − 1+r 1) and δ 2,k + 1 = N(0,P 2,k/k − 1+r 2) respectively.
P 1,k/k − 1 and P 2,k/k − 1 are the predicted covariance of the states of gravity and geomagnetic respectively.
The weighted residual and its smoothness value are defined (Hollowell, Reference Hollowell1990) as:
$$\left( \matrix{WRS_{1,k + 1} \hfill \cr WRS_{2,k + 1} \hfill} \right) = \left( \matrix{(\delta _{1,k + 1} )^2 /(P_{1,k + 1/k} + r_1 ) \hfill \cr (\delta _{2,k + 1} )^2 /(P_{2,k + 1/k} + r_2 ) \hfill} \right)$$
$$\left( \matrix{SWRS_{1,k + 1} \hfill \cr SWRS_{2,k + 1} \hfill} \right) = \left( \matrix{\alpha \cdot WRS_{1,k + 1} + (1 - \alpha )SWRS_{1,k} \hfill \cr \alpha \cdot WRS_{2,k + 1} + (1 - \alpha )SWRS_{2,k} \hfill} \right)$$where;
SWRS 1,0=SWRS 2,0 = 1.0
α is smooth factor.
α = 1.938p when p indicates the draft of the INS.
If SWRS is smaller, better position results can be achieved. So the smallest values of SWRS are sought, and the corresponding positions are regarded as the estimated results of positions for GravAN and GeomAN.
As the positions have been estimated, the following criterion is adopted to judge the reliability of the results:
$$H = \displaystyle{{SWRS_{\min} ^* - SWRS_{1/2}^{\min}} \over {SWRS_{1/2}^{\min}}} \gt H_t $$where:
H is the reliability.
H t is the threshold.
SWRS ½min is the smallest SWRS value, and SWRS min* is the second smallest SWRS value in the region around the corresponding position of SWRS ½min. For estimating the results of position for GravAN and GeomAN, the region is defined by nine grids, including the corresponding position of SWRS ½min and eight surrounding grids, and is illustrated in Figure 3. From Equation (7) it can be seen that the reliability H would increase if the differences between SWRS min* and SWRS ½min increased. When the reliability H is larger than threshold H t, the position results would be considered as valid, otherwise they would be invalid.
Region around SWRS ½min and OWAP for GGCAN.
As the position results of SWRS ½min are confirmed as valid, more accurate estimation of positions can be calculated with weighted methods. Equations (8) and (9) are presented here for position and weighted calculation respectively:
$$\left\{ \matrix{\lambda _1 = \sum\limits_{i = 1}^9 {(W_1^i} \cdot \lambda _1^i ) \hfill \cr \varphi _1 = \sum\limits_{i = 1}^9 {(W_1^i} \cdot \varphi _1^i ) \hfill} \right.,\left\{ \matrix{\lambda _2 = \sum\limits_{i = 1}^9 {(W_2^i} \cdot \lambda _2^i ) \hfill \cr \varphi _2 = \sum\limits_{i = 1}^9 {(W_2^i} \cdot \varphi _2^i ) \hfill} \right.$$
$$\left\{ \matrix{W_1^i = \displaystyle{{{\rm exp}( - 0.5 \times N_1^i )} \over {\sum\limits_{i = 1}^9 {{\rm exp}( - 0.5 \times N_1^i )}}}, N_1^i = \displaystyle{{SWRS_1^i} \over {SWRS_1^{{\rm min}}}}, 1 \leqslant i \leqslant 9 \hfill \cr W_2^i = \displaystyle{{{\rm exp}( - 0.5 \times N_2^i )} \over {\sum\limits_{i = 1}^9 {{\rm exp}( - 0.5 \times N_2^i )}}}, N_2^i = \displaystyle{{SWRS_2^i} \over {SWRS_2^{{\rm min}}}}, 1 \leqslant i \leqslant 9 \hfill} \right.$$Where (λ,φ) are the coordinates of the position, the weights are calculated with SWRS values at every grid. In Figure 3 point P 1(λ 1,φ 1) and P 2(λ 2,φ 2) are the more accurate estimation results.
4. OPTIMAL WEIGHT ALLOCATION PRINCIPLE (OWAP)
4.1. OWAP Algorithms
As the position results of GravAN and GeomAN have been obtained, OWAP is adopted to combine the gravity and geomagnetism aided navigation.
It is shown in Figure 3 that the position results of GravAN and GeomAN are P 1(λ 1,φ 1) and P 2(λ 2,φ 2), respectively. They are affected by various errors including measurement error, database error and the algorithm error. All the noises and errors are assumed to obey the law of Gaussian distribution (Ling et al., Reference Ling, Li and Zhou1998):
$$p(P_1 ) = N(\mu _1, \sigma _1^2 )$$
$$p(P_2 ) = N(\mu _2, \sigma _2^2 )$$After the following transformation, z 1 and z 2 obey the standard normal distribution:
$$z_1 = \displaystyle{{P_1 - \mu _1} \over {\sigma _1}} $$
$$z_2 = \displaystyle{{P_2 - \mu _2} \over {\sigma _2}} $$The weighted average method of the gravity and geomagnetism combined aided navigation (GGCAN) is then constructed as:
$$p = WP = [w_1, w_2 ][P_{1,} P_2 ]^T $$where:
W = [w 1,w 2] is the weight vector.
w 1 and w 2 are the weights of GravAN and GeomAN respectively.
P = [P 1,P 2]T.
Multiple random vectors Z obey the standard normal distribution after the transformation below:
$$Z = A(P - U)$$where:
Z = [z 1,z 2].
$A = diag\left[ {\displaystyle{1 \over {\sigma _1}}, \displaystyle{1 \over {\sigma _2}}} \right]$.U = [μ 1,μ 2]T.
From Equation (15) we obtain:
$$P = A^{ - 1} Z + U$$Substituting Equation (16) into Equation (14):
$$p = WP = W(A^{ - 1} Z + U) = WU + WA^{ - 1} Z$$According to the theory of n-dimensional multivariate statistics, the probability density function of p is:
$$\eqalign{\,f\,(\,p) & = (2\pi )^{{\textstyle{n \over 2}}} \left| {WP^{ - 1} (P^{ - 1} )'W'} \right|{\rm exp}\left\{ {\displaystyle{1 \over 2}(\,p - WU)(WP^{ - 1} (P^{ - 1} )'W')(\,p - WU)'} \right\} \cr & = (2\pi )^{ - {\textstyle{n \over 2}}} \left| {\sum\limits_{i = 1}^n {w_i^2 \sigma _i^2}} \right|{\rm exp}\left\{ { - \displaystyle{1 \over 2}\sum\limits_{i = 1}^n {w_i^2 \sigma _i^2} \left( {y - \sum\limits_{i = 1}^n {w_i^2 \sigma _i^2}} \right)^2} \right\}} $$This indicates that p obey normal distribution. In other words:
$$p = N\left( {\sum\limits_{i = 1}^n {w_i \mu _i}, \sum\limits_{i = 1}^n {w_i^2 \sigma _i^2}} \right)$$That is to say, the mathematical expectation of position result after fusing is the weighted expectation of GravAN and GeomAN. And the precision of GGCAN is:
$$\sigma _p = \sqrt {\sum\limits_{i = 1}^n {w_i^2 \sigma _i^2}} $$Apparently, if σ i are given, σ p is only influenced and determined by w i. As the minimum of σ p indicates the optimal accuracy of GGCAN. A mathematical model is constructed as follows:
$$f\,(w_1, w_2 ) = \sum\limits_{i = 1}^2 {w_i^2 \sigma _i^2} $$
$$\sum\limits_{i = 1}^2 {w_i} = 1(w_i \geqslant 0,i = 1,2),\sigma _i (i = 1,2)$$So the optimal problem is transformed into finding the conditional extreme value of this function f(w 1,w 2) (Ling et al., Reference Ling, Li, Chen, Gu and Li2000). This problem can be solved by the Lagrange Multiplier method.
4.2. The Solution of OWAP
The Lagrange Multiplier conditional extreme value equation can be written as below:
$$F = \sum\limits_{i = 1}^2 {w_i^2 \sigma _i^2} + \beta \left( {\sum\limits_{i = 1}^2 {w_i} - 1} \right)$$where β is the Lagrange Multiplier.
The partial differential of the F with respect to w i(i = 1,2) is assigned to 0, so a set of equations can be written as:
$$\left\{ \matrix{\displaystyle{{\partial F} \over {\partial w_1}} = 2w_1 \sigma _1^2 + \beta = 0 \hfill \cr \displaystyle{{\partial F} \over {\partial w_2}} = 2w_2 \sigma _2^2 + \beta = 0 \hfill \cr w_1 + w_2 = 1 \hfill} \right.$$Then the weight of GravAN and GeomAN could be obtained:
$$w_i = \displaystyle{1 \over {\sigma _i^2 \sum\limits_{i = 1}^2 {\displaystyle{1 \over {\sigma _i^2}}}}} $$Substituting Equation (25) into (20) the precision of GGCAN is:
$$\sigma _p = \sqrt {\sum\limits_{i = 1}^2 {w_i^2 \sigma _i^2}} = \displaystyle{1 \over {\sqrt {\sum\limits_{i = 1}^2 {\displaystyle{1 \over {\sigma _i^2}}}}}} $$4.3. Precision Analyses of OWAP
Let the precisions of GravAN and GeomAN be σ 1 and σ 2, respectively. Assuming that σ 1≤σ 2, the following inequality can be obtained:
$$\sigma _p = \displaystyle{1 \over {\sqrt {\displaystyle{1 \over {\sigma _1^2}} + \displaystyle{1 \over {\sigma _2^2}}}}} \leqslant \displaystyle{1 \over {\sqrt {\displaystyle{1 \over {\sigma _1^2}}}}} \leqslant \displaystyle{1 \over {\sqrt {\displaystyle{1 \over {\sigma _2^2}}}}} $$This inequality indicates that with OWAP the precision of GGCAN is improved with respect to either GravAN or GeomAN. This is the essential difference between OWAP and other fusion methods. So more precise position results can be gained with the combination of gravity and geomagnetism aided navigation after OWAP.
5. GGCAN
The flow chart of the GGCAN algorithm is shown in Figure 4. With the MMAE-based GravAN and GeomAN algorithms mentioned in Section 3, and OWAP method mentioned in Section 4, the GGCAN algorithm can be provided. The fusing algorithm OWAP only works when both GravAN and GeomAN are valid.
Flow Chart of GGCAN Algorithm Based on MMAE and OWAP.
As the criterion function, H in Equation (7) can, to a certain extent, be regarded as the precision of aided navigation. It is assumed that:
$$\left\{ \matrix{\sigma _1 = \displaystyle{{SWRS_1^* - SWRS_1^{\min}} \over {SWRS_1^{\min}}} \hfill \cr \sigma _2 = \displaystyle{{SWRS_2^* - SWRS_2^{\min}} \over {SWRS_2^{\min}}} \hfill} \right.$$Thereafter, the weight matrix is formed according to Equation (25). With the weights the position result P 1(λ 1,φ 1) of GravAN and the position result P 2(λ 2,φ 2) of GeomAN are taken to calculate the final GGCAN position result which is marked with a red dot in Figure 3.
The final GGCAN position result can be obtained from the following equation:
$$\lambda = w_{\;1} \cdot \lambda _{\;1} + w_{\;2} \cdot \lambda _{\;2}, \varphi = w_{\;1} \cdot \varphi _{\;1} + w_{\;2} \cdot \varphi _{\;2} $$6. SIMULATION RESULTS AND DISCUSSION
Gravity and geomagnetic anomaly maps for simulation are illustrated in Figure 5. Two special Regions A and B in the West Pacific Ocean are selected, which are marked with black rectangles in Figure 5. The statistical information of gravity and geomagnetic anomalies in the two regions are collected and listed in Table 1. From this table it can be found that the standard deviation of the geomagnetic anomaly is bigger than the gravity anomaly in Region A, and smaller than the gravity anomaly in Region B. So it perhaps means that there is more geomagnetic information in Region A and more gravity information in Region B.
GAG2 (top) and EMAG2 (bottom) Maps in West Pacific Ocean and the Simulation Two Regions.
Gravity and geomagnetic parameters in the simulation regions.
Simulation parameters including initial position errors, INS drift, measurement errors, AUV velocity, smooth factor and reliability threshold H t are assigned; their values are listed in Table 2. The actual trajectories are simulated form South West to North East in Regions A and B. Under the same conditions four methods, GravAN, GeomAN, GGCAN(EW) and GGCAN(OWAP) were used to provide position information in Regions A and B. The GGCAN(EW) was tested to prove the performance of OWAP. The trajectories of four aided navigation methods are showed in Figure 6. The position errors of these methods are showed in Figure 7.
The Trajectories of the Two Simulation Tests in Region A and B.
Four Types of Aided Navigation Errors in Region A and B.
Simulation parameters.
It can be seen from Figures 6 and 7 that INS position errors increase with time. With the four methods, GravAN, GeomAN, GGCAN(EW) and GGCAN(OWAP), the INS position errors can be corrected effectively by different amounts. The position errors of GGCAN(OWAP) were marked with red pentagrams in Figure 7 and, in these diagrams, GGCAN(OWAP) was lower and more steady than the three other methods.
For further quantitative analysis, the statistical parameters about navigation results are counted and listed in Table 3. In this table it can be seen that in Region A the performance of GeomAN is better than GravAN and in Region B this comparison is opposite. So, as mentioned before, it is apparent that there is more geomagnetic information than gravity in Region A, and more gravity information than geomagnetic in Region B.
Simulation results of four types of aided navigation systems.
In the four aided navigation methods, GGCAN(EW) and GGCAN(OWAP) have a distinct advantage in success rate and position error than GravAN and GeomAN, in either Region A or B. In Region A the success rate of GGCAN(OWAP) is 91·55%, much higher than 52·58% of GravAN and 85·92% of GeomAN. Similarly, in Region B the success rates are 93·43%, 79·34% and 69·48%, respectively. This means that GGCAN(OWAP) can be valid when GravAN or GeomAN fails. In Region A the mean position error of GGCAN(OWAP) is 1·51 n. miles, smaller than 1·81 n. miles of GravAN and 1·73 n. miles of GeomAN. In Region B these errors are 1·34, 1·53 and 1·99 n. miles. The improvement in success rate and position accuracy show that the GGCAN(OWAP) method presented in this study is more accurate and robust than GravAN or GeomAN.
Moreover, in both Regions A and B, the success rate of GGCAN(EW) and GGCAN(OWAP) are equal, but the position errors of GGCAN(OWAP) are smaller. This indicates that the proposed weighted method of OWAP is more suitable for GGCAN than the equal weighted method.
7. CONCLUSIONS
Some aided navigation methods are necessary to correct the INS position error in underwater passive navigation. Gravity aided navigation and geomagnetic aided navigation are two suitable methods for establishing aided navigation systems on Autonomous Underwater Vehicles (AUVs). But in some regions, gravity or geomagnetic navigation may fail due to the inconspicuous gravity/geomagnetic variation and some other errors. In this paper, the Gravity-Geomagnetism Combined Aided Navigation (GGCAN) is proposed. In this method a Kalman filter and Multiple Model Adaptive Estimation (MMAE) are utilized to get the position results of Gravity Aided Navigation (GravAN) and Geomagnetic Aided Navigation (GeomAN). With the weighted method of Optimal Weight Allocation Principle (OWAP), the two results are weighted and combined. Two regions in the West Pacific Ocean were selected in the simulation and the improved success rate and position accuracy show that OWAP is more suitable for GGCAN than the equal weighted method, and GGCAN(OWAP) presented in this study is more accurate and robust than GravAN or GeomAN.
ACKNOWLEDGEMENT
This work was supported by the National Natural Science Foundation of China (Grant nos. 41021003, 41074051, 40974044, and 41104050).
