Point mass filter based matching algorithm in gravity aided underwater navigation

HAN Yurong,WANG Bo,*,DENG Zhihong,and FU Mengyin

1.School of Automation,Beijing Institute of Technology,Beijing 100081,China;

2.Nanjing University of Science and Technology,Nanjing 210094,China

1.Introduction

Nowadays,inertial navigation system(INS)is widely used in underwater autonomous vehicle(UAV)applications[1].However,the defect of INS is the accumulating errors.Therefore,various techniques have been developed to limit the navigation errors[2,3].Gravity-aided INS as a passive navigation method,can provide positioning data independent of global position system(GPS)and can operate in a wide range of conditions[4].The system consists of an INS,a priori gravitational field map,a gravity measurement unit and a matching algorithm.

The matching algorithm is the core technology in the gravity aided navigation system.There are two main kinds of matching algorithms,one is based on the correlation analysis method[5,6]and the other is based on the filter recursive principle[7,8].Besides,there are also other matching algorithms based on the limits of vehicle’s maneuver mode[9,10].The mostly practical matching algorithm based on the filtering recursive method is Sandia inertial terrain aided navigation(SITAN)algorithm[11].The essence of the SITAN algorithm is extended Kalman filter(EKF).Compared to the sequence matching algorithms,the SITAN algorithm possesses high real-time ability.However,the precise model of gravity anomaly is difficult to establish and the system cannot satisfy the linear assumptions or the local observability theory,thus its application is limited.

Motivated by the mentioned problems,the authors propose a matching algorithm based on the point mass filter(PMF)and the deterministic resampling method.Firstly,a simple system model is derived due to the digital reference map with low resolution[12].The Bayesian estimation approach gives a comprehensive framework for solving the recursive estimation problem in gravity matching navigation[13,14].Instead of constructing the observation model,the filter method computes a probability mass distribution of the vehicle position point and updates it recursively with each measurement[15,16].Moreover,when the algorithm is utilized,several evaluation strategies for optimal estimation are discussed and analyzed via simulation tests.Simulation result indicates that the locating precision on the order of the gravity anomaly map resolution can be achieved with sufficiently accurate measurements and the deterministic resampling strategy is more practical.

The rest of this paper is organized as follows:Section 2 introduces the gravity matching navigation system and the derived system model;in Section 3,the principle of PMF is discussed and the procedures are described in detail;the Verification through simulation tests is conducted in Section 4,which illustrates the usefulness of the proposed algorithm; finally,Section 5 summarizes this paper with some concluding remarks and offers the future work on this research topic.

2.Basic principles and models

2.1 Gravity-aided navigation system

In Fig.1,the gravity matching navigation system consists of four parts:INS,gravity anomaly reference database,gravity measurement sensor and matching algorithm.

Fig.1 Gravity matching navigation system

When the vehicle is traveling,the position information is provided by INS and the position information is utilized to search for a gravity reference value from the gravity anomaly base map.Meanwhile the gravity sensor offers a gravity measurement value in real time.Then the gravity information and position information is sent to the matching algorithm, finally the precise position information is obtained.The accuracy of the gravity-aided inertial navigation is related to the error of INS locating,the error of gravity anomaly map,the measurement error of gravity sensor and the error caused by the digital observation model.

2.2 System model and measurement model

Gravity-aided inertial navigation estimates position by comparing gravity anomaly measurements and digital gravity anomaly maps stored on a navigation computer.xkis the vehicle position variable at the k th time epoch,which is a two-dimensional vector and can be written in terms of latitude and longitude by

Estimates of relative movement are provided by INS.The movement of the vehicle is modeled by the following process:

where the updated position xk+1is written in terms of the previous position,the relative movement µkand the addi-tive white process noise wkwhich is assumed to be a white noise with Gauss distribution.

Gravity anomaly data are measured by the gravity measurement sensor.Assuming the additive noise,the gravity anomaly measurement is expressed as

where g(xk)represents the true gravity anomaly data of the true position point.vkis assumed to be a white noise with Gauss distribution.It is assumed that the noise processes wkand vkare independent and uncorrelated.

3.PMF based matching algorithm

3.1 Principle of the matching algorithm

The proposed algorithm combines the principle of correlation analysis and the process of recursive state estimation.The big resolution of gravity anomaly map and the INS short-term accuracy lead to the system model.Meanwhile,due to the sparse measurements,the particle filer has no advantage over the PMF.Besides,the optimal selection method is another important factor.The maximum a posteriori(MAP)method is aiming to pick up the grid point with the largest probability.The principle of minimum mean squared error(MMSE)is to get the estimation results according to the evaluation of all possible points.However,it would cause mismatch without resampling particles.Therefore,three mainly used resampling algorithms are introduced.

The fitting algorithm has the similar principle to the correlation analysis method,such as the terrain contour matching(TERCOM)algorithm and the iterative closest contour point(ICCP)algorithm.The probability calculation process is like a sequence matching.Compared to direct path registration,the algorithm is based on filtering using the historical information stored in state variables.In this way,the estimation result can be gained per sampling period,and it is more efficient than sequence matching methods.

In addition,compared to SITAN,the matching algorithm does not need linearization of the measure model.Thus,it avoids the error caused by heavy nonlinearity of the matching area.Since the probability density assumption depends not only on gravity anomaly distribution but also on the relative displacement of vehicle,the matching results are more reliable.

On the other hand,particle filtering generates particles according to the character of the system noise.In theory,the more the particles are,the more accurate the result is.However,the INS confidence radius is less than 10?when the vehicle has been less than 10 h with the medium inertial measurement unit(IMU)precision.It results in that the maximum number of the possible points is less than 100 in the base map with resolution 2?×2?.The conditions turn out that the particle filter loses its superiority.

Finally,the estimated value only involves vehicle position due to the simple filter model.In order to correct other outputs of INS,the Kalman filter with the INS error model is employed.Therefore,the latitude and longitude information of the matched point is delivered to the Kalman filter as observation data.In a word,the matching algorithm turns the nonlinear relationship between gravity anomaly and location to a linear form.

3.2 Flow of the matching algorithm based on PMF

In a 2D gravity matching navigation,p(xk/Yk)is approximated on N grid points in two dimensions:1,2,...,Nk.

The discrete probability assigned tois denoted by

Gridded filters have been devised for numerical approximation of the integrals.In this formulation,the posterior density p(xk/Yk)is approximated by discrete points on a suitably chosen grid.The update and propagation steps are then evaluated by numerical integration,and the resulting filter represents an approximation to the optimal Bayesian filter.

The PMF grids the state space and computes the posterior probability over this grid recursively.PMF can be applied to any nonlinear and the non-Gaussian model and is able to represent any posterior distribution.The main limiting factor is the dimensionality of the grid size in higher state dimensions and the algorithm of quadratic complexity in the grid size.

When the Bayesian estimation is applied in gravity aided navigation,the PMF scheme is illustrated in Fig.2.

Fig.2 Point mass filter for gravity-aided matching navigation

(i)Initialization:since the initial position error of INS is smaller than the resolution of the database map,the initial point set is assumed to have one point and the corresponding possibility is set to 1.

(ii)Possible-point set generation:according to the INS navigation circular error probable(CEP)and Rayleigh distribution,the true position of the vehicle is within the probable-region with a confidence degree of 99%.In theory,the probable-region should be a circular region.However,the map is a digital model with low resolution,the scope becomes a square area.In addition,the grid point of INS-indicated position is its center and 6σ is the length of its side(σ is the stand deviation of INS position error).The grids inside the scope are assumed to be the possible points.If the number of the possible-point set at the k th time epoch is Nk,the information of each grid is denoted by

(iii)Probability density function:the possibility of a point in the possible-point set is relative to real-time measurement and historical information.When measured gravity anomaly data are obtained,the comparison between the measured gravity anomaly data and the gravity anomaly data of the point in possible-point set results in the possibility and the possibility density function is presented as

Another probability density function is based on the relative displacement:

Though the error of position information provided by INS during a long navigation time is high,the INS short term output is reliable and it can be applied to limit matching error.For example,the ICCP algorithm is a recursive iteration with rigid transformation.The rigid transformation guarantees the invariability of relative location within a sequence,which is another form to import INS short term accuracy.It is concluded that the short-term parameter derived from INS output possesses a high reliability.Therefore,the μkshould be

(iv)Probability propagation:the procedure is a recursive process and the probability calculation is expressed as

(v)Optimal selection:there are two common selection rules for optimal position estimation:MMSE and MAP whose definitions are expressed as

The principle of MAP is that the optimal position is regarded as the grid point with the highest probability.In contrast,when MMSE is imported,the probability-set needs normalization and resampling.Otherwise,the final estimation may be contaminated by the mismatched position point.

Since the MMSE algorithm involves the re-sampling method,the commonly used re-sampling methods are studied:

(i)Multinomial re-sampling;

(ii)Residual re-sampling;

(iii)Deterministic resampling.

In theory,the MMSE method is more stable than the MAP method.The tests in the simulation section verify the performance of each re-sampling algorithm and the most appropriate one stands out.

4.Simulation and analysis

In this section,the gravity anomaly map contains gridded data of gravity anomaly and the data are not interpolated.It is known that the matching error and roughness of a matching field are negatively correlated[17].In order to verify the validation of the adaptive algorithm,simulation experiments are done in the field with relative high uniqueness.

4.1 Simulation condition

4.1.1 Gravity anomaly database

The gravity anomaly database is gained by a plenty of experiments from satellites,vessel and aviation measurements.The data precision is:grid length:2?×2?;gravity anomaly precision:2 mGal.Moreover,the gravity anomaly contour map and 3-D map of the matching area are shown in Fig.3.The gravity anomaly statistic parameters are:the numberof grid points:43×25;the maximum:34.0879 mGal;the minimum:-26.0559 mGal;the mean:5.741 8 mGal;the standard deviation:11.104 7 mGal.

Fig.3 Gravity anomaly matching area

4.1.2 INS

The test is carried out for a route under the conditions shown in Table 1.The conditions include navigation initial error and IMU measurement error.Moreover,the true trajectory,INS-indicated trajectory and the location error are shown in Fig.4.

Table 1 Simulation conditions

Fig.4 INS-indicated trajectory and position error

It is observed from Fig.4 that the position error increases with oscillation over time and it varies from less than 2?at beginning to more than 8?after 12 h.It is obvious that the INS position error is accumulating.Compared to latitude error,longitude error increases quickly.Then the INS short-term precision is tested.

Fig.5 shows the INS output accuracy within a sample period.It is obtained that the short-term latitude error and short-term longitude error do not increase as time and they vary in a small range.It is shown that the maximum of the short-term latitude error is 0.4?.Therefore,the parameter can achieve the accuracy less than a quarter of the map resolution.Considering the tests results and the resolution,σωis set to 2?,which is the system noise.

Fig.5 INS short-term error

The gravity anomaly observation error is related to the precision of the gravity anomaly map and the gravity measurement accuracy.According to practical experiences,the measurement error is about 1 mGal.Therefore,the variance of observation error in our application is set to 10.0 mGal2.

The sample period is supposed to be larger than the length of a grid. To be consistent with the gravity basemap,observations are measured at cruising intervals of 2?(about 2 n miles,for which the cruising time is approximately 12 min).Moreover,gravity-aided navigation is commonly applied to the underwater vehicle during the long sailing period,the total traveling time is set to 12 h.The experiment was executed on an Intel(R)Core(TM)i5-4200M,2.50 GHz computer with 4.00 G RAM in a Matlab environment.

4.2 Comparison of the proposed algorithm and other nonlinear filters

The matching algorithm based on MAP is conducted to examine the performance of the point mass filtering algorithm.Fig.6 shows the matching error.

Since the resolution of the gravity anomaly map is 2?×2?,the magnitude of INS latitude error is similar to the magnitude of latitude error from the PMF.In contrast,the PMF algorithm decreases the longitude error greatly.The position precision is improved in general.Moreover,it shows that the accuracy of the proposed algorithm is about 2 grids(about 4?).However,the matching errors of the PMF algorithm based on the MAP method with different time stamps vary strongly.

Fig.6 Matching errors

Under the same simulation conditions,due to the nonlinear observation model,the tests with the typical nonlinear filters are conducted.The nonlinear filters are EKF[18],unsent Kalman filter(UKF)[19]and particle filter(PF)[20].The quantitative comparison between PMF and others is shown in Table 2.

Table 2 Estimation error comparison

It is obvious that PMF gains much better estimation accuracy than other conventional nonlinear filters.It proves that though the system model of PMF is simple,it is more suitable than other filter base matching algorithms in our application.

4.3 Comparison of different optimal selection methods

The test in Section 1 is about the PMF matching algorithm whose optimal selection rule is the MAP method.Besides,there are other optimal selection rules mentioned in Section 3.In order to improve matching performance,the optimal selection rules are implemented respectively in this part.

Fig.7 shows the matched trajectories based on different optimal selection rules.The comparison validates the assumption in Section 4.1 that the resampling and weighted summation method possesses better stability.For the sake of obvious comparison,the error statistical result is shown in Table 3.

Fig.7 Matched trajectories based on different optimal selection rules

Table 3 Matching error comparison

It is observed that the deterministic resampling optimal selection method has the best result.Therefore,the deterministic resampling is selected as the optimal selection rule.

4.4 Simulation of navigation process with frequent correction

The optimal estimated position information is delivered to INS via the Kalman filter.The matched position is the feedback.The system state variable is

where δL and δλ are latitude error and longitude error respectively,δvEand δvNare velocity error in east and north directions,φE,φNand φUare pose angle errors,εE,εNand εUare gyroscope errors in three directions,then ΔEand ΔNare acceleration errors.Height and velocity in the up direction are discarded due to the error divergence in altitude channel.The system noise setting depends on simulation conditions show in Table 1.

The difference between the matched position and the INS-indicated position is the observation.The observation model is denoted by

Fig.8 shows the vehicle actual trajectory,INS-indicated trajectory,the matched trajectory and the trajectory with frequent feedback.In Fig.8,the black curve represents the real vehicle trajectory,the blue line presents INS-indicated trajectory and the green line expresses the matched trajectory.It is obtained that the matched trajectory is much closer to the true trajectory than INS trajectory.The red line represents navigation result with the feedback per sampling period.The comparison of all these trajectories indicates that the matched trajectory can limit INS position error efficiently and the trajectory with feedback gains the optimal result.

Fig.8 Trajectory after Kalman filter

5.Conclusions

The matching algorithm based on the PMF for gravity matching navigation is proposed.Simulation experiments are conducted on a gravity anomaly grid map with resolution 2?×2?.The result indicates that the real-time performance of the proposed matching algorithm is better than that of the correlation analysis matching algorithm.In addition,the method is a nonlinear filter that can avoid lin-earization error of a non-linear observation model.It proves that the proposed algorithm works well for better real-time performance and higher accuracy.

The accuracy of the matched results in gravity is mainly related to the precision and the resolution of the gravity anomaly base map,the gravity statistic feature in matching field,and the gravity measurement error.This paper discusses the principle of the matching algorithm.The influence of gravity characteristics on the precision of the matching algorithm should be considered in the future work.

[1]JIANG F,SWINDLEHURSTAL.Optimization of UAV heading for the ground-to-air uplink.IEEE Journal on Selected Areas in Communications,2012,30(5):993–1005.

[2]INDELMAN V,GURFIL P,RIVLIN E,et al.Real-time vision aided localization and navigation based on three-view geometry.IEEE Trans.on Aerospace and Electronic Systems,2012,48(3):2239–2259.

[3]WANG B,REN Q,DENG Z,et al.A self-calibration method for nonorthogonal angles between gimbals of rotational inertial navigation system.IEEE Trans.on Industrial Electronics,2015,62(4):2353–2362.

[4]BISHOP G C.Gravitational field maps and navigational errors unmanned underwater vehicles.IEEE Journal of Oceanic Engineering,2002,27(3):726–737.

[5]LIU M,WANG B,DENG Z,et al.Improved ICCP algorithm and its application in gravity matching aided inertial navigation system.Proc.of the 33rd Chinese Control Conference,2014:562–567.

[6]HAN Y,WANG B,DENG Z,et al.An improved TERCOM-based algorithm for gravity-aided navigation.IEEE Sensors Journal,2016,16(8):2537–2544.

[7]ZHAO L,GAO N,HUANG B,et al.A novel terrain-aided navigation algorithm combined with the TERCOM algorithm and particle filter.IEEE Sensors Journal,2015,15(2):1124–1131.

[8]HOSTETLER L,ANDREAS R.Nonlinear Kalman filtering techniques for terrain-aided navigation.IEEE Trans.on Automatic Control,1983,28(3):315–323.

[9]WANG H,WANG Y,FANG J,et al.Simulation research on a minimum root-mean-square error rotation fitting algorithm for gravity matching navigation.Science China Earth Sciences,2012,55(1):90–97.

[10]GAO W,ZHAO B,ZHOU G,et al.Improved artificial bee colony algorithm based gravity matching navigation method.Sensors,2014,14(7):12968–12989.

[11]DAI Z,KANG C.Geomagnetic field aided inertial navigation using the SITAN algorithm.Proc.of the2nd International Conference on Systems and Informatics,2014:79–83.

[12]GUSTAFSSON F.Particle filter theory and practice with positioning applications.IEEE Trans.on Aerospace and Electronic Systems,2010,25(7):53–82.

[13]BERGMAN N,LJUNG L,GUSTAFSSON F.Terrain navigation using Bayesian statistics.IEEE Control Systems,1999,19(3):33–40.

[14]BHAUMIK S,SRINIVASAN M,SADHU S,et al.Adaptive grid risk-sensitive filter for non-linear problems.IET Signal Processing,2011,5(2):235–241.

[15]COPP B,SUBBARAO K.Nonlinear adaptive filtering in terrain-referenced navigation.IEEE Trans.on Aerospace and Electronic Systems,2015,51(4):3461–3469.

[16]SIMANDL M,KRALOVEC J,SODERSTROM T.Anticipative grid design in point-mass approach to nonlinear state estimation.IEEE Trans.on Automatic Control,2002,47(4):699–702.

[17]WANG B,ZHU Y,DENG Z,et al.The gravity matching area selection criteria for underwater gravity-aided navigation application based on the comprehensive characteristic parameter.IEEE/ASME Trans.on Mechatronics,2016,21(6):2935–2943.

[18]KATRINIOK A,ABEL D.Adaptive EKF-based vehicle state estimation with online assessment of local Observability.IEEE Trans.on Control Systems Technology,2016,24(4):1368–1381.

[19]FANG Q,HUANG S X.UKF for integrated vision and inertial sensors based on three-view geometry.IEEE Sensors Journal,2013,13(7):2711–2719.

[20]LI J Q,ZHAO R H,CHEN J L,et al.Target tracking algorithm based on adaptive strong tracking particle filter.IET Science,Measurement&Technology,2016,10(7):704–710.