CN105157704A - Bayesian estimation-based particle filter gravity-assisted inertial navigation matching method - Google Patents

Abstract

The invention provides a bayesian estimation-based particle filter gravity-assisted inertial navigation matching method. When solving non-linear problems, particle filter prevents linearization-caused errors and solves the problem that the traditional point matching Sandia algorithm easily produces divergence in a large-gravity anomalous change matching area. Through use of inertial navigation position information as quantity of state, a gravimeter measured value as observed quantity, bayesian estimation-based particle filter, and a random sample average replacing a probability density function conditional mean, inertial navigation system state variable is estimated.

Description

A kind of particle filter gravity based on Bayesian Estimation assists inertial navigation matching process

Technical field

The present invention relates to a kind of gravity and assist inertial navigation matching process, belong to gravity auxiliary inertial navigation system matching technique field.

Background technology

The most frequently used passive navigation mode of the long-time creeping of underwater vehicle is inertial navigation system, but ins error can along with accumulated time.For ensureing disguise and the independence of navigation, utilizing the passive navigation of geophysical character to correct ins error, therefore having carried out the researchs such as terrain match airmanship, Gravity-aided navigation technology, geomagnetic auxiliary navigation technology.For underwater navigation, the measurement of terrain data is more difficult, and the magnetic field of the earth is not very stable, and gravity field is stablized and gravimetric data can utilize satellite data inverting, and therefore application of weight navigation has very large advantage.Gravity matching algorithm is one of gravity core technology of assisting inertial navigation technique, and it is that the gravitation information utilizing gravity meter to measure in real time compares with certain algorithm with the gravimetric map information that prestores, thus estimates the position of inertial navigation.

Present stage uses for reference comparatively ripe terrain match, and Gravity matching algorithm is mainly divided into two large classes according to sample mode, sequences match and single-point coupling.Sequences match is a kind of posteriori estimation or batch processing method, mates each time and all will carry out after sampling enough counting, so real-time is poor, mainly with ICCP algorithm and relevant function method for representative.ICCP algorithm is developed by image registration algorithm at first, and be a kind of optimal alignment method found under global sense, precision is higher.Correlation analysis method is come by the terrain contour matching TERCOM algorithm development in terrain match, and its matching precision is by the impact of inertial navigation site error, but poor real and be difficult to improve theoretically.Single-point matching algorithm is mainly Sang Diya (SITAN) auxiliary navigation method proposed by U.S. Sang Diya laboratory, EKF technology is utilized to realize, this arithmetic accuracy high real-time is good, but non-linear due to gravity field feature, in the obvious region of gravity field feature, the error that EKF linearization brings is comparatively large, causes filtering divergence time serious, and coupling loses meaning.

Summary of the invention

The object of the invention is the deficiency existed to overcome prior art, propose a kind of particle filter gravity based on Bayesian Estimation and assist inertial navigation matching process, when solving nonlinear problem, particle filter avoids the error that linearization brings, and solves conventional point matching algorithm Sang Diya algorithm and changes in gravity anomaly the shortcoming that matching area greatly easily disperses.

The object of the invention is to be achieved through the following technical solutions.

Particle filter gravity based on Bayesian Estimation assists an inertial navigation matching process, comprises the steps:

Step one, the characteristics of motion according to underwater carrier, using the latitude and longitude information of carrier as state variable, GRAVITY ANOMALIES measured in real time by gravity meter is observed quantity, sets up gravity and assists inertial navigation single-point Matching Model:

Δx _k，k+1＝Δx _k-1，k+ΔU _k+e _k(1)

y _k＝h _k(x _k)+v _k(2)

In formula, Δ x _{k, k+1}represent the longitude and latitude difference of the submarine that k moment to the k+1 moment exports, Δ U _kfor inertial navigation system provides the offset increment in k moment, e _kthe error of inertial navigation system, y _krepresent the gravity anomaly information that the gravity meter in k moment is measured, h _k(x _k) represent at x _kbe in the GRAVITY ANOMALIES that gravity datum figure reads, v _krepresent gravity anomaly measuring error and gravity datum figure error;

Step 2, sequential importance sampling: suppose from posterior probability density p (x _k| Y _k) the N number of independent identically distributed random sample of middle extraction i=1 ..., N, then p (x _k| Y _k) be expressed as these random samples summation form, introduce the importance density function q (x of known, an easy sampling _k| Y _k), therefrom generate sampling particle, realize summation and approach posterior probability density function p (x _k| Y _k), then posterior probability density is expressed as the form of the weights sum of products corresponding to random sample point and each sample of sampling from the importance density function, particle weights are defined as the degree that the importance density function approaches posterior probability function, sampling particle is generated from the sampling of importance probability density, and try to achieve corresponding weights along with the arrival recursion successively of measured value, and then obtain state estimation;

Step 3: resampling: the particle weights obtained according to step 2, calculates the degree of degeneration that number of effective particles weighs particle weights;

Step 4, state x to inertial navigation system _kestimate: wherein measure Y according to existing observation _k, wherein Y _krepresent y ₁, y ₂..., y _k, estimate the positional information x that gravity assists inertial navigation positioning system _k.

Step 5: according to the Δ U in step one _kupgrade p (x _k| x _k-1), then according to step 3 more new particle weights, carry out the inertial navigation location estimation in k+1 moment.

Further, Bayesian Estimation comprises prediction and upgrades two stages, and forecasting process utilizes the priori probability density of the system model predictions state set up, and renewal process then utilizes up-to-date measured value to revise priori probability density, obtains posterior probability density.

Beneficial effect of the present invention:

Compared with existing single-point matching process, the present invention utilizes the particle filter method based on Bayesian Estimation to realize, when solving gravity field feature nonlinear problem, avoid the error that EKF linearization brings, overcome conventional point matching algorithm Sang Diya algorithm and change the shortcoming of easily dispersing in obvious region in gravity field feature, adapt to the change of various gravity field feature, widened the scope of application of traditional algorithm, and matching precision significantly improves, real-time is good.

Embodiment

Choose each matching area once of longitude and latitude span, in this region, between neighbouring sample point, gravity anomaly rate of change mean value is 57.413mgal, and gravity field feature is obvious.The data interpolating that gravity anomaly data used are 30 ' × 30 ' by the high resolution obtained of satellite survey comes, and after interpolation, gravimetric map resolution is 1 ' × 1 '.Gravity meter measured data adds random noise by actual value and forms.Simulated conditions: underwater hiding-machine headway 5 saves; Inertial navigation three direction gyros zero are 0.02 °/h partially, and random drift is 0.01 °/h; Three directional accelerations zero are 1 × 10 partially ^-4g, random drift is 5 × 10 ^-5g; Initial attitude angle error is 5 '; Initial position error is 1 '; Initial velocity error is 0.1m/s; The white Gaussian noise of gravity meter measurement noises to be variance be 2mgal, simulation time totally 10 hours.

Step one: according to the characteristics of motion of underwater carrier, using the latitude and longitude information of carrier as state variable, GRAVITY ANOMALIES measured in real time by gravity meter is observed quantity, sets up gravity and assists inertial navigation single-point Matching Model:

Δx _k，k+1＝Δx _k-1，k+ΔU _k+e _k(16)

y _k＝h _k(x _k)+v _k(17)

In formula, Δ x _{k, k+1}represent the longitude and latitude difference of the submarine that k moment to the k+1 moment exports, Δ U _kfor inertial navigation system provides the offset increment in k moment, e _kthe error of inertial navigation system, Y _krepresent the gravity anomaly information that the gravity meter in k moment is measured, h _k(x _k) represent at x _kbe in the GRAVITY ANOMALIES that gravity datum figure reads, v _krepresent gravity anomaly measuring error and gravity datum figure error.

Step 2: sequential importance sampling

State estimation is considered as a probability inference process by Bayesian Estimation, solves posterior probability density p (x by Target state estimator question variation for utilizing Bayesian formula _k| Y _k), and then integration obtains the optimal estimation of inertial navigation system.Bayesian Estimation needs integral operation, for the nonlinear system of gravity field feature, is difficult to the closed analytic expression obtaining posterior probability, then utilize particle filter Integral Problem to be converted into finite sample point summation problem.Particle filter is exactly that searching one group of random sample propagated in state space is to posterior probability density function p (x _k| Y _k) be similar to, p (x will be obeyed _k| Y _k) random sample that distributes is called particle.

In actual computation, usually directly cannot sample from Posterior distrbutionp, then introduce the importance density function q (x of known, an easy sampling _k| Y _k), therefrom generate sampling particle, realize summation and approach posterior probability density function p (x _k| Y _k), the present invention chooses the probability density function p (x of state variable _k| x _k-1) as importance density function, then posterior probability density can be expressed as

$p\left(x_k\right|Y_k)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^{\left(i\right)}\mathrm{\δ}(x_k-x_k^{\left(i\right)})---\left(18\right)$

Wherein δ () is Dirac function, for the corresponding weights of each particle

$w_k^{\left(i\right)}\∞\frac{p\left(x_k^{\left(i\right)}\right|Y_k)}{p\left(x_k^{\left(i\right)}\right|x_{k-1})}---\left(19\right)$

But based in the particle filter of importance sampling, estimate that posterior probability probability density needs to utilize all observation datas, each observation data that upgrades comes the importance weight all needing to recalculate whole state, and calculated amount is large, affects the real-time of algorithm.

For addressing this problem, the sequential analysis method in statistics being applied in monte carlo method, being called sequential importance sampling.Namely from the sampling of importance probability density, generate sampling particle, and try to achieve corresponding weights along with the arrival recursion successively of measured value, and then obtain state estimation.Importance density function can be decomposed into

p(x _0：k|y _1：k)＝q(x _0：k-1|y _1：k-1)q(x _k|x _{0：k-1y1：k})(20)

Particle weights recursive form can be expressed as

$\begin{array}{c}{w}_k^{\left(i\right)}\∞\frac{p\left(x_{0:k}^{\left(i\right)}\right|Y_k)}{q\left(x_{0:k}^{\left(i\right)}\right|Y_k)}\\ =\frac{p\left(y_k\right|x_k^{\left(i\right)})p\left(x_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)})p\left(x_{0:k}^{\left(i\right)}\right|Y_{k-1})}{q\left(x_k^{\left(i\right)}\right|x_{0:k}^{\left(i\right)},Y_k)q\left(x_{0:k-1}^{\left(i\right)}\right|Y_{k-1})}\\ =w_{k-1}^{\left(i\right)}\frac{p\left(y_k\right|x_k^{\left(i\right)})p\left(x_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)})}{q\left(x_k^{\left(i\right)}\right|x_{0:k}^{\left(i\right)},Y_k)}\end{array}---\left(21\right)$

Substitute into the importance density function p (x chosen _k| x _k-1) the weights of particle are

$w_k^{\left(i\right)}=w_{k-1}^{\left(i\right)}p\left(y_k\right|x_k^{\left(i\right)})$

The weights of particle are normalized, namely

${\tilde{w}}_k^{\left(i\right)}=\frac{w_k^{\left(i\right)}}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^{\left(i\right)}}---\left(22\right)$

Step 3: resampling

In order to obtain correct state estimation, wish that the variance of particle weights levels off to zero as far as possible.But in actual computation, through several iteration, only have the weights of minority particle comparatively large, the weights of all the other particles can be ignored.The variance of weights increased along with the time, and the number of effective particles of state space is less.Along with increasing of invalid sampling number of particles, a large amount of calculating is wasted in the distribution of estimation posterior probability density probability almost inoperative particle renewal, estimated performance is declined.Sampling number of effective particles weigh the degree of degeneration of particle weights, namely

When carrying out sequential importance sampling, if be less than the threshold value of setting, then the weights that the method for resampling should be adopted to overcome particle are degenerated, and method for resampling gives up the less particle of weights, replaces the particle that weights are larger.Resampling process meets under condition, by particle assembly be updated to in the present invention when time, adopt the residual error resampling more new particle based on importance sampling.Residual error resampling have efficiency high, realize feature easily.If wherein for floor operation.Remaining resampling adopts new weights select under remaining individual particle, the main process of residual error resampling is:

(1) the weights cumulative amount λ of residual particles is calculated _j,

(2) generate individual in [0,1] interval interior equally distributed random number

(3) for each μ ^l, find normalization weights cumulative amount and be greater than or equal to μ ^lminimum label m, i.e. λ _m-1< μ ^l< λ _m.Work as μ ^ldrop on interval [λ _m-1, λ _m] time, be replicated once.

Like this, each particle number after resampling is some number of particles of choosing in step (3) and N ⁱ.

Step 4: to the state x of inertial navigation system _kestimate

Bayesian Estimation utilizes the form of probability distribution to estimate the state x of nonlinear system _k.According to existing observed quantity Y _k(Y _krepresent y ₁, y ₂..., y _k), the positional information x that gravity assists inertial navigation positioning system be estimated _k.Have according to Bayesian formula:

$p\left(x_k\right|Y_k)=\frac{p\left(y_k\right|x_k)p\left(x_k\right|Y_{k-1})}{\&Integral;p\left(y_k\right|x_k)p\left(x_k\right|Y_{k-1})d{x}_k}---\left(24\right)$

From formula (16), denominator is one and x _kirrelevant normalization constant, it can ensure p (x _k| Y _k) integration be 1.By formula (17) known measuring error v _kprobability density function p (y _k| x _k), be priori probability density function:

p(y _k|x _k)＝∫δ(y _k-h _k(x _k)-v _k)p(v _k)dv _k(25)

Wherein δ () is Dirac function.

Due to observed reading y _konly with current state x _krelevant, then p (x _k| x _k-1, Y _k-1)=p (x _k| x _k-1), according to total probability formula:

p(x _k，x _k-1|Y _k-1)＝p(x _k|x _k-1，Y _k-1)p(x _k-1|Y _k-1)(26)

Abbreviation also carries out integration and can obtain CK equation:

p(x _k|Y _k-1)＝∫p(x _k|x _k-1)p(x _k-1|Y _k-1)dx _k-1(27)

Wherein, p (x _k-1| Y _k-1) suppose known, p (x _k| x _k-1) be the Markov process of state.

Namely according to Minimum Mean Square Error (MMSE) criterion, using conditional mean as system state x _koptimal estimation value, namely

Then above formula (24) (26) (27) constitute Bayesian Estimation system state x _kfundamental formular.Formula (26) is probability density function, and formula (24), (27) are respectively location updating and measurement updaue formula.From the above, Bayesian Estimation needs to carry out integral operation, and nonlinear system many employings approximate data solves Integral Problem, thus obtains the optimal estimation of state.Particle filter based on Bayesian Estimation utilizes the great amount of samples point in required state space to carry out the Posterior probability distribution of close approximation variable to be estimated, thus Integral Problem is converted to the summation problem of finite sample point.

Particle filter is exactly that searching one group of random sample propagated in state space is to posterior probability density function p (x _k| Y _k) be similar to, replace conditional mean E [x with sample average _k| Y _k].P (x will be obeyed _k| Y _k) random sample that distributes is called particle.

If for from posterior probability density function p (x _k| Y _k) the middle sampling particle obtained, then for the inertial navigation position x that will estimate _k, conditional mean E [x _k| Y _k] can approach with summing mode, namely

$E\left[x_k\right|Y_k]={\&Integral;x}_{k}p\left(x_k\right|Y_k)d{x}_k=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{x}_k^{\left(i\right)}---\left(28\right)$

Now carry out the sequential importance sampling in step (two), generate particle, when number of particles of sampling is very large, formula (13) is the real posterior probability density of programmable single-chip system just.The inertial navigation position x that therefore will estimate _kconditional mean E [x _k| Y _k] can be expressed as:

$E\left[x_k\right|Y_k]=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{x}}_k^{\left(i\right)}{\tilde{w}}_k^{\left(i\right)}---\left(29\right)$

Now carrying out step (three), the inertial navigation position x that estimate _kconditional mean E [x _k| Y _k] can be expressed as:

$E\left[x_k\right|Y_k]=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\hat{x}}_k^{\left(i\right)}{\hat{w}}_k^{\left(i\right)}---\left(30\right)$

Export inertial navigation position x now _k.

Step 5: according to the Δ U in step () _kupgrade p (x _k| x _k-1), then according to step (three) more new particle weights, carry out the inertial navigation location estimation in k+1 moment.

In order to effect of the present invention is described, Sang Diya algorithm is adopted to test under identical Setup Experiments, after adopting the present invention, matching track serves good corrective action to inertial navigation track in matching area, longitude and latitude error obviously reduces, through test of many times, the result of longitude and latitude AME of the present invention and traditional Sang Diya method comparison is as shown in table 1.

Table 1: error result

The above is only the preferred embodiment of the present invention; should be understood that; for those skilled in the art; under the premise without departing from the principles of the invention; some improvement can also be made; or carry out equivalent replacement to wherein portion of techniques feature, these improve and replace and also should be considered as protection scope of the present invention.

Claims (2)

1. the particle filter gravity based on Bayesian Estimation assists an inertial navigation matching process, it is characterized in that, comprises the steps:

Step one, the characteristics of motion according to underwater carrier, using the latitude and longitude information of carrier as state variable, GRAVITY ANOMALIES measured in real time by gravity meter is observed quantity, sets up gravity and assists inertial navigation single-point Matching Model:

△x _k,k+1＝△x _k-1,k+△U _k+e _k(1)

y _k＝h _k(x _k)+v _k(2)

In formula, △ x _{k, k+1}represent the longitude and latitude difference of the submarine that k moment to the k+1 moment exports, △ U _kfor inertial navigation system provides the offset increment in k moment, e _kthe error of inertial navigation system, y _krepresent the gravity anomaly information that the gravity meter in k moment is measured, h _k(x _k) represent at x _kbe in the GRAVITY ANOMALIES that gravity datum figure reads, v _krepresent gravity anomaly measuring error and gravity datum figure error;

Step 2, sequential importance sampling: suppose from posterior probability density p (x _ky _k) the N number of independent identically distributed random sample of middle extraction i=1 ..., N, then p (x _k| Y _k) be expressed as these random samples summation form, introduce the importance density function q (x of known, an easy sampling _k| Y _k), therefrom generate sampling particle, realize summation and approach posterior probability density function p (x _k| Y _k), then posterior probability density is expressed as the form of the weights sum of products corresponding to random sample point and each sample of sampling from the importance density function, particle weights are defined as the degree that the importance density function approaches posterior probability function, sampling particle is generated from the sampling of importance probability density, and try to achieve corresponding weights along with the arrival recursion successively of measured value, and then obtain state estimation;

Step 3: resampling: the particle weights obtained according to step 2, calculates the degree of degeneration that number of effective particles weighs particle weights;

Step 4, state x to inertial navigation system _kestimate: wherein measure Y according to existing observation _k, wherein Y _krepresent y ₁, y ₂..., y _k, estimate the positional information x that gravity assists inertial navigation positioning system _k;

Step 5: according to the △ U in step one _kupgrade p (x _k| x _k-1), then according to step 3 more new particle weights, carry out the inertial navigation location estimation in k+1 moment.

2. a kind of particle filter gravity based on Bayesian Estimation assists inertial navigation matching process as claimed in claim 1, it is characterized in that, further, Bayesian Estimation comprises prediction and upgrades two stages, forecasting process utilizes the priori probability density of the system model predictions state set up, renewal process then utilizes up-to-date measured value to revise priori probability density, obtains posterior probability density.