CN102818567A - AUV (autonomous underwater vehicle) integrated navigation method integrating Kalman filtering and particle filtering - Google Patents

Abstract

The invention discloses an AUV (autonomous underwater vehicle) integrated navigation method integrating Kalman filtering and particle filtering. The AUV integrated navigation method comprises the following steps of: 1) data collection: using a global positioning system to obtain initial position information of an AUV when the AUV is on the water surface, and collecting the information of speed, attitude angle and the like of the AUV by utilizing navigation sensors such as a Doppler log, an electronic compass and the like; and 2) filter positioning: fusing navigation information collected by the sensor by utilizing a filtering algorithm based on the combination of the Kalman filtering and the particle filtering, estimating to obtain the position and attitude change information of the AUV at every moment, and realizing overall positioning of the AUV. The invention provides the AUV integrated navigation method integrating the Kalman filtering and the particle filtering, which can improve the precision.

Description

The AUV Combinated navigation method that set Kalman filtering-particle filter combines

Technical field

The present invention relates to the oceanographic engineering field, especially a kind of autonomous submarine navigation device (Autonomous Underwater Vehicle, AUV) Combinated navigation method.

Background technology

Autonomous submarine navigation device is the focus of present oceanographic engineering art development, and environmental monitoring under water, offshore oil engineer operation, search under water obtain application more and more widely with military fields such as mapping and submarine mine antagonism and region of war warnings in real time.Airmanship is to realize that AUV realizes the key of autonomous navigation, and working time length, circumstance complication, information source are few because AUV exists, disguise requires high work characteristics, and this brings very big difficulty for stable, accurate AUV independent navigation and challenges.

Very active always for the research of AUV airmanship in the world in recent years.The underwater navigation technology need be carried out balance usually between precision, range of movement and automaticity etc., summarize and get up to mainly contain acoustics location, geophysics navigation and polytype airmanships such as dead reckoning and inertial navigation.

Existing AUV utilizes inertial navigation system (Inertial Navigation System usually; INS) attitude angle and the acceleration information gathered; Attitude angle information and Doppler log (Doppler Velocity Log that three-dimensional electronic compass (TCM) is gathered; The velocity information of DVL) gathering is carried out integrated navigation; Data fusion method commonly used comprise Kalman filtering (Kalman Filter, KF) EKF (Extended Kalman Filter, EKF), tasteless Kalman filtering (Unscented Kalman Filter; UKF), particle filter (Particle Filter; PF) and the set Kalman filtering (Ensemble Kalman Filter, EnKF) etc., wherein the common issue with of EKF and tasteless Kalman filtering is that convergence sharply descends even disperses when non-linear/non-Gauss's property is strong.And particle filter utilizes sequential monte carlo method; Has preferable performance for non-linear/non-Gauss's problem; If but to compare prior probability be peaking for up-to-date observation information is positioned at prior probability distribution in particle filter afterbody or likelihood function; Then can cause the blindness of particle selection, reduce estimated accuracy.

Summary of the invention

The purpose of this invention is to provide a kind of autonomous scale underwater vehicle combined navigation method that combines based on set Kalman filtering-particle filter, to improve the precision of AUV navigation algorithm.

The technical solution adopted for the present invention to solve the technical problems is:

A kind ofly gather the AUV Combinated navigation method that Kalman filtering-particle filter combines, said Combinated navigation method may further comprise the steps:

1) data acquisition: obtain the initial position message of AUV when the water surface, gather speed and the attitude angle information of AUV;

2) filtering location: with initial position message, speed and attitude angle information definition state vector, observation vector, state model equation and the observation model equation of step 1), filtering is following:

2.1) generate primary and its corresponding weight coefficient according to starting condition;

2.2) define each particle background set

according to formula (1) to background set predict

State model equation: X _k=f (X _K-1, w _k) (1)

X wherein _K-1, X _k4 state vector when representing k-1 and k respectively, w _kExpression state procedure noise, f () representes X _K-1And X _kBetween nonlinear relationship;

Calculate the average and the covariance of background set, and calculate corresponding kalman gain:

${\hat{x}}_k^{\left(i\right),b}=\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{x}_{k,j}^{\left(i\right),b}---\left(12\right)$

${\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}(x_{k,j}^{\left(i\right),b}-{\hat{x}}_k^{\left(i\right),b}){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(13\right)$

${\hat{P}}_{\mathrm{hh}}^{\left(i\right)k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right)){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(14\right)$

$K_k^{\left(i\right)}={\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}{({\hat{P}}_{\mathrm{hh}}^{\left(i\right),k}+v_k)}^{-1}---\left(15\right)$

Wherein

is the average of i particle in k background set constantly; is i particle gathered j sample point in k moment background value;

is that i particle is at the x of k background set constantly and the covariance matrix of h ();

is i particle at the h () of k background set constantly with the covariance matrix of h (), and

is that i particle is at the k kalman gain in the moment;

3) utilize formula (7) replacement analysis set,

$x_{k,i}^a=x_{k,i}^b+K_k(y_k-h\left(x_{k,i}^b\right)),i=\mathrm{1,2},...,n---\left(7\right)$

Y wherein _{K, i}Be with y _kBe average, v _kBe the sampling of the Gaussian distribution of variance,

Be that i particle gathered in k analysis constantly,

Be that i particle is in k background set constantly;

And average and the covariance of utilizing formula (8) and (9) computational analysis to gather:

${\hat{x}}_k^a=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{k,i}^a---\left(8\right)$

${\hat{P}}_k^a=\frac{1}{n-1}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(x_{k,i}^a-{\hat{x}}_k^a){(x_{k,i}^a-{\hat{x}}_k^a)}^T---\left(9\right)$

Wherein

is the average of k analysis set constantly;

is the value that i sample point gathered in k analysis constantly,

be the covariance of k analysis set constantly;

4) utilize analysis set structure suggestion distribution function, sampling obtains new particle, and the renewal weight coefficient:

${\hat{x}}_k^{\left(i\right)}~q\left(x_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)},z_k)=N({\hat{x}}_k^{\left(i\right),a},{\hat{P}}_k^{\left(i\right)},a)---\left(16\right)$

$w_k^{\left(i\right)}=w_{k-1}^{\left(i\right)}\frac{p\left(z_k\right|{\hat{x}}_k^{\left(i\right)})p\left({\hat{x}}_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)})}{q\left({\hat{x}}_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},z_{1:k})}---\left(17\right)$

5) judge whether to exist degradation phenomena,,, circulate again next time, finish up to filtering if having degradation phenomena then resample earlier if do not have degradation phenomena then circulate next time;

Filtering finishes the back and estimates to obtain each position and attitude change information constantly of AUV, realizes the overall situation location to AUV.

Technical conceive of the present invention is: in order to overcome the problem that particle filter exists; The present invention utilizes the set Kalman filtering to produce each suggestion distribution function constantly of particle filter; The novel navigation filtering algorithm of AUV that set Kalman filtering-particle filter (EnKPF) combines has been proposed; Make that the suggestion distribution function can be more near real posterior probability density, through the mutual supplement with each other's advantages of particle filter with two kinds of filtering algorithms of set Kalman filtering, to improve the precision of AUV navigation algorithm.

Beneficial effect of the present invention mainly shows: utilize the set Kalman filtering to produce each suggestion distribution function constantly of particle filter; Make the suggestion distribution function more to distribute near real posterior probability; Through the mutual supplement with each other's advantages of particle filter, thereby improve the precision of AUV navigation algorithm with set Kalman filtering two kinds of filtering algorithms.

Description of drawings

Fig. 1 is the synoptic diagram of EnKPF method of the present invention.

Fig. 2 PF algorithm flow chart.

Fig. 3 is the EnKF algorithm flow chart.

Fig. 4 is the EnKPF algorithm flow chart.

Fig. 5 is that PF, EnKF and EnKPF Combinated navigation method of the present invention carry out the navigator fix comparison diagram to AUV, and movement locus is that AUV turns over two circles around counter clockwise direction on the water surface, and navigation data derives from the AUV lake and tests.

Fig. 6 is that PF, EnKF and EnKPF Combinated navigation method of the present invention carry out the navigator fix comparison diagram to AUV, and movement locus is AUV makees similar S type on the water surface motion, and navigation data derives from the AUV lake and tests.

Fig. 7 is that PF, EnKF and EnKPF Combinated navigation method of the present invention carry out the navigator fix comparison diagram to AUV; Movement locus is that AUV runs the stretch journey earlier on the water surface, carries out dive then, carries out the motion of depthkeeping 15m under water; Again emerge at last, navigation data derives from the AUV lake and tests.

Embodiment

Below in conjunction with accompanying drawing the present invention is further described.

With reference to Fig. 1～Fig. 7, a kind ofly gather the AUV Combinated navigation method that Kalman filtering-particle filter combines, said Combinated navigation method may further comprise the steps:

1) data acquisition: utilize GPS to obtain the initial position message of AUV when the water surface, utilize navigation sensors such as Doppler log, electronic compass to gather the information such as speed and attitude angle of AUV;

2) filtering location: utilize the filtering algorithm that combines based on set Kalman filtering-particle filter with sensor acquisition to navigation information merge, estimate to obtain each position and attitude change information constantly of AUV, realize overall situation location to AUV.

Sensor acquisition to said velocity information be in pace information, transverse velocity information, the vertical speed information any or appoint several kinds.

Sensor acquisition to said attitude angle information comprise in angle of drift information, roll angle information, the trim angle information any or appoint several kinds.

The initial position message of autonomous underwater robot that GPS is obtained is longitude information and latitude information.

The course of work of EnKPF Combinated navigation method of the present invention is as shown in Figure 1.Navigation data derives from two kinds of approach, comprising: the attitude angle information that (1) INS or TCM gather; (2) the AUV velocity information of DVL collection.Be used in the filtering method that set Kalman filtering-particle filter combines, the AUV initial position message in conjunction with AUV goes up the GPS record of installing can obtain each positional information and attitude angle information constantly of autonomous submarine navigation device.It below is the concrete implementation method of EnKPF.

For a certain non-linear process, its state equation can be described by following equation with the measurement equation:

State model equation: X _k=f (X _K-1, w _k) (1)

Observation model equation: z _k=h (X _k, v _k) (2)

X wherein _K-1, X _kRepresent k-1 and k state vector constantly respectively, w _kExpression state procedure noise, f () representes X _K-1And X _kBetween nonlinear relationship, z _kExpression k observation vector constantly, v _kNoise is measured in expression, and h () representes X _kAnd z _kBetween nonlinear relationship.

Particle filter essence is to adopt sequential monte carlo method, and its basic thought is for independently to extract N sampled point from posterior probability density, and through weighted sum, the approximate representation posterior probability distributes.Its flow process is as shown in Figure 2, and it comprises following step: 1) generate primary and its corresponding weight coefficient according to starting condition; 2) upgrade each particle state vector; 3) utilize each state vector updating value and the observed reading in this moment that obtain to combine, upgrade the weight coefficient of each particle; 4) judge whether to exist degradation phenomena; If do not have degradation phenomena then carry out next renewal of state vector and weight coefficient thereof constantly; If have degradation phenomena then resample earlier, carry out next renewal of state vector and weight coefficient thereof constantly again, finish up to filtering.

The basic thought of set Kalman filtering is the state sampling set as a setting of initialization one group system, utilizes observation information to concentrate each individuality to upgrade through Kalman filtering to background data, obtains analyzing set.Analytic set share the true average and the variance of estimated state.Analyze set through the system model transmission, can obtain next background data set constantly.Use set to estimate true statistical value like this, improved estimated accuracy.Below for gathering the algorithm steps of Kalman filtering:

Definition set

is k background set constantly; It is transmitted by k-1 analysis set

constantly; N is the set sample number, and sampling average and covariance can be tried to achieve by following formula:

${\hat{x}}_k^b=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{k,i}^b---\left(3\right)$

${\hat{P}}_{\mathrm{xh}}^k=\frac{1}{n-1}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(x_{k,i}^b-{\hat{x}}_k^b){(h\left(x_{k,i}^b\right)-h\left({\hat{x}}_k^b\right))}^T---\left(4\right)$

${\hat{P}}_{\mathrm{hh}}^k=\frac{1}{n-1}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(h\left(x_{k,i}^b\right)-h\left({\hat{x}}_k^b\right)){(h\left(x_{k,i}^b\right)-h\left({\hat{x}}_k^b\right))}^T---\left(5\right)$

Kalman gain is:

$K_k={\hat{P}}_{\mathrm{xh}}^k{({\hat{P}}_{\mathrm{hh}}^k+v_k)}^{-1}---\left(6\right)$

V in the formula (6) _kExpression k measurement noise constantly.

Utilize up-to-date observation information, can upgrade, obtain k analysis set constantly the background set:

$x_{k,i}^a=x_{k,i}^b+K_k(y_k-h\left(x_{k,i}^b\right)),i=\mathrm{1,2},...,n---\left(7\right)$

Y in the formula (7) _{K, i}Be with y _kBe average, v _kBe the sampling of the Gaussian distribution of variance, corresponding, average and the covariance of analyzing set are:

${\hat{x}}_k^a=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{k,i}^a---\left(8\right)$

${\hat{P}}_k^a=\frac{1}{n-1}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(x_{k,i}^a-{\hat{x}}_k^a){(x_{k,i}^a-{\hat{x}}_k^a)}^T---\left(9\right)$

The filtering method that set Kalman filtering-particle filter combines has combined above two kinds of filtering methods exactly; Using EnKF to produce the suggestion distribution function of particle filter, mainly is to combine with it through Gauss's estimation of propagation posteriority distribution and with each nearest observation constantly.In other words, just being to use EnKF that following posterior probability density is carried out recurrence estimates:

$p\left(x_k\right|z_{1:k})\≈p_N\left(x_k\right|z_{1:k})=N({\hat{x}}_k,{\hat{P}}_k)---\left(10\right)$

In the framework of particle filter, to each particle use one independently EnKF produce and transmit Gauss and advise distributing:

$q\left(x_k^{\left(i\right)}\right|x_{1:k-1}^{\left(i\right)},z_{1:k})=N({\hat{x}}_k^{\left(i\right)},{\hat{P}}_k^{\left(i\right)})---\left(11\right)$

At k-1 constantly, use EnKF and nearest observation information that the average and the variance of the suggestion distribution of each particle are upgraded, from this distributes, sampling constantly at k obtains new particle.

The flow process of the filtering method that set Kalman filtering-particle filter combines is as shown in Figure 3, and it may be summarized to be following steps:

1) generates primary and its corresponding weight coefficient according to starting condition;

2) define each particle background set

according to formula (1) to background set predict; Calculate the average and the covariance of background set with following formula, and calculate corresponding kalman gain:

${\hat{x}}_k^{\left(i\right),b}=\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{x}_{k,j}^{\left(i\right),b}---\left(12\right)$

${\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}(x_{k,j}^{\left(i\right),b}-{\hat{x}}_k^{\left(i\right),b}){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(13\right)$

${\hat{P}}_{\mathrm{hh}}^{\left(i\right)k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right)){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(14\right)$

$K_k^{\left(i\right)}={\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}{({\hat{P}}_{\mathrm{hh}}^{\left(i\right),k}+v_k)}^{-1}---\left(15\right)$

Wherein

is the average of i particle in k background set constantly;

is i particle gathered j sample point in k moment background value;

is that i particle is at the x of k background set constantly and the covariance matrix of h ();

is that i particle is at the h () of k background set constantly and the covariance matrix of h ();

is i particle at k kalman gain constantly, and subscript T representes the transposition computing;

3) utilize formula (7) replacement analysis set, and utilize the average and the covariance of formula (8) and (9) computational analysis set;

4) utilize analysis set structure suggestion distribution function, sampling obtains new particle, and the renewal weight coefficient:

${\hat{x}}_k^{\left(i\right)}~q\left(x_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)},z_k)=N({\hat{x}}_k^{\left(i\right),a},{\hat{P}}_k^{\left(i\right)},a)---\left(16\right)$

$w_k^{\left(i\right)}=w_{k-1}^{\left(i\right)}\frac{p\left(z_k\right|{\hat{x}}_k^{\left(i\right)})p\left({\hat{x}}_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)})}{q\left({\hat{x}}_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},z_{1:k})}---\left(17\right)$

5) judge whether to exist degradation phenomena,,, circulate again next time, finish up to filtering if having degradation phenomena then resample earlier if do not have degradation phenomena then circulate next time.

Adopt the EnKPF method to merge the AUV speed and the attitude angle information of each sensor acquisition of AUV, the AUV initial position message in conjunction with AUV goes up the GPS record of installing can obtain each positional information and attitude angle information constantly of autonomous submarine navigation device.Below provide the integrated navigation example of bidimensional in the surface level.

For AUV is positioned, its state vector is shown below:

x(k)=[p _x(k)p _y(k)v _x(k)v _y(k)θ(k)w(k)] ^T （18）

P wherein _x(k) and p _y(k) be respectively in the horizontal shift of sampled point k moment AUV in x and y direction.v _x(k) and v _y(k) be respectively in sampled point k constantly pace and the transverse velocity of AUV.θ (k) is that AUV is in k moment angle of drift (with the angle of x axle).W (k) is the k angular acceleration of θ (n) constantly.

The observation vector of AUV is shown below

z(k)=[v _x(k)v _y(k)θ(k)w(k)] ^T （19）

Pace v wherein _x(k) and transverse velocity v _y(k) by the Doppler log collection.Angle of drift θ (k) can be by the electronic compass collection.Angular acceleration w (k) can be by the accelerometer collection.

According to the equation of motion, can obtain concrete state model equation by formula (18):

$x\left(k\right)=\left[\begin{array}{cccccc}1& 0& \cos \left(\mathrm{\&theta;}(k-1)\right)\mathrm{\&Delta;T}& \sin \left(\mathrm{\&theta;}(k-1)\right)\mathrm{\&Delta;<figure-callout\; id="0"\; label="T"\; filenames="HDA00001987477600032.png,HDA00001987477600041.png"\; state="\{\{state\}\}">T</figure-callout>}& 0& 0\\ 0& 1& -\sin \left(\mathrm{\&theta;}(k-1)\right)\mathrm{\&Delta;T}& \cos \left(\mathrm{\&theta;}(k-1)\right)\mathrm{\&Delta;<figure-callout\; id="0"\; label="T"\; filenames="HDA00001987477600032.png,HDA00001987477600041.png"\; state="\{\{state\}\}">T</figure-callout>}& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& \mathrm{\&Delta;<figure-callout\; id="0"\; label="T"\; filenames="HDA00001987477600032.png,HDA00001987477600041.png"\; state="\{\{state\}\}">T</figure-callout>}\\ 0& 0& 0& 0& 0& 1\end{array}\right]x(k-1)+w---\left(20\right)$

Can obtain concrete measurement model equation by formula (19):

$z\left(k\right)=\left[\begin{array}{cccccc}0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]x\left(k\right)+v---\left(21\right)$

Wherein △ T is the SI, and w and v are respectively the state procedure noise and measure noise.After having defined state vector, observation vector, state model equation and observation model equation, then can be according to above-mentioned EnKPF filtering method 1)-5) each positional information and attitude angle information constantly of step forecast updating AUV.

Embodiment

In the practical application, AUV vertical dimension estimation of Depth is usually through pressure transducer direct estimation depth information, and following examples utilize that the test navigation data carries out the location algorithm analysis of two-dimensional level face on the AUV lake.In the site test; At first utilize AUV to go up the initial position message that the GPS that installs obtains AUV; Utilize three-dimensional electronic compass to gather angle of drift information again, inertial navigation system is gathered the angular acceleration information of angle of drift, AUV pace and transverse velocity information that Doppler log is gathered; Can extrapolate each positional information constantly of AUV, obtain the movement locus of AUV.Fig. 5, Fig. 6 and Fig. 7 have provided three flight number data processed result utilizing EnKPF method of the present invention to carry out the AUV navigation, and have carried out analyzing relatively with PF and EnKF method.

Among Fig. 5, around counterclockwise having turned over two circles, average headway is 1.8m/s to AUV on the water surface,

The positional information that track 8 is obtained by GPS at the water surface for AUV; Each positional information constantly of AUV that track 9 obtains for utilizing the PF method; Each positional information constantly of AUV that track 10 obtains for utilizing the EnKF method; Each positional information constantly of AUV that track 11 obtains for utilizing EnKPF method of the present invention; The AUV final position information of point 12 for obtaining by GPS, point 13 is an AUV final position information of utilizing the PF method to obtain, the AUV final position information of point 14 for utilizing the EnKF method to obtain; The AUV final position information of point 15 for utilizing EnKPF method of the present invention to obtain can find out that by Fig. 5 and table 1 the AUV final position information of utilizing EnKPF method of the present invention to obtain is the highest near the AUV final position information navigation precision that is obtained by GPS.

Among Fig. 6; AUV does the motion of similar S type on the water surface; Average headway is 1.5m/s; The positional information that track 16 is obtained by GPS at the water surface for AUV, each positional information constantly of AUV that track 17 obtains for utilizing the PF method, each positional information constantly of AUV that track 18 obtains for utilizing the EnKF method; Each positional information constantly of AUV that track 19 obtains for utilizing EnKPF method of the present invention; The AUV final position information of point 20 for obtaining by GPS, point 21 is an AUV final position information of utilizing the PF method to obtain, the AUV final position information of point 22 for utilizing the EnKF method to obtain; The AUV final position information of point 23 for utilizing EnKPF method of the present invention to obtain can find out that by Fig. 6 and table 2 the AUV final position information of utilizing EnKPF method of the present invention to obtain is the highest near the AUV final position information navigation precision that is obtained by GPS.

Among Fig. 7, AUV carries out dive at overwater service after a period of time, and average headway is 1.5m/s; Depthkeeping 15m navigation, the navigation area depth of water be greater than 40m, the positional information that track 24 is obtained by GPS at the water surface for AUV; Path in graphs center section AUV is under water, does not therefore have GPS information.Each positional information constantly of AUV that track 25 obtains for utilizing the PF method; Each positional information constantly of AUV that track 26 obtains for utilizing the EnKF method; Each positional information constantly of AUV that track 27 obtains for utilizing EnKPF method of the present invention; Point 28 is the final positional information that floats of the AUV that is obtained by GPS; The final positional information that floats of AUV that point 29 obtains for utilizing the PF method; Point 30 is for utilizing the positional information of the final come-up of AUV that the EnKF method obtains, and point 31 is the positional information of the final come-up of AUV utilizing EnKPF method of the present invention and obtain, can find out that by Fig. 7 and table 3 to utilize the positional information of the final come-up of AUV that EnKPF method of the present invention obtains the highest near the positional information navigation accuracy of the final come-up of the AUV that is obtained by GPS.

Filtering algorithm	The final position absolute error
		PF	1.5315m
EnKF	1.2522m
		EnKPF	0.9329m

Table 1

Filtering algorithm	The final position absolute error
		PF	2.7958m
EnKF	1.9554m
		EnKPF	0.7726m

Table 2

Filtering algorithm	The final position absolute error
		PF	28.6549m
EnKF	26.9219m
		EnKPF	10.6352m

Table 3

The foregoing description shows and utilizes the present invention to gather the bearing accuracy that autonomous scale underwater vehicle combined navigation method that Kalman filtering-particle filter combines can improve integrated navigation system.

Claims (1)

1. gather the AUV Combinated navigation method that Kalman filtering-particle filter combines for one kind, it is characterized in that: said Combinated navigation method may further comprise the steps:

1) data acquisition: obtain the initial position message of AUV when the water surface, gather speed and the attitude angle information of AUV;

2) filtering location: with initial position message, speed and attitude angle information definition state vector, observation vector, state model equation and the observation model equation of step 1), filtering is following:

2.1) generate primary and its corresponding weight coefficient according to starting condition;

2.2) define each particle background set

according to formula (1) to background set predict

State model equation: X _k=f (X _K-1, w _k) (1)

X wherein _K-1, X _kRepresent k-1 and k state vector constantly respectively, w _kExpression state procedure noise, f () representes X _K-1And X _kBetween nonlinear relationship;

Calculate the average and the covariance of background set, and calculate corresponding kalman gain:

${\hat{x}}_k^{\left(i\right),b}=\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{x}_{k,j}^{\left(i\right),b}---\left(12\right)$

${\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}(x_{k,j}^{\left(i\right),b}-{\hat{x}}_k^{\left(i\right),b}){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(13\right)$

${\hat{P}}_{\mathrm{hh}}^{\left(i\right)k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right)){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(14\right)$

$K_k^{\left(i\right)}={\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}{({\hat{P}}_{\mathrm{hh}}^{\left(i\right),k}+v_k)}^{-1}---\left(15\right)$

Wherein

is the average of i particle in k background set constantly;

is i particle gathered j sample point in k moment background value;

is that i particle is at the x of k background set constantly and the covariance matrix of h ();

is that i particle is at the h () of k background set constantly and the covariance matrix of h ();

is i particle at k kalman gain constantly, and subscript T representes the transposition computing;

3) utilize formula (7) replacement analysis set,

$x_{k,i}^a=x_{k,i}^b+K_k(y_k-h\left(x_{k,i}^b\right)),i=\mathrm{1,2},...,n---\left(7\right)$

Y wherein _{K, i}Be with y _kBe average, v _kBe the sampling of the Gaussian distribution of variance,

Be that i particle gathered in k analysis constantly,

Be that i particle is in k background set constantly;

And average and the covariance of utilizing formula (8) and (9) computational analysis to gather:

${\hat{x}}_k^a=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{x}_{k,i}^a---\left(8\right)$

${\hat{P}}_k^a=\frac{1}{n-1}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}(x_{k,i}^a-{\hat{x}}_k^a){(x_{k,i}^a-{\hat{x}}_k^a)}^T---\left(9\right)$

Wherein is the average of k analysis set constantly;

is the value that i sample point gathered in k analysis constantly, be the covariance of k analysis set constantly;

4) utilize analysis set structure suggestion distribution function, sampling obtains new particle, and the renewal weight coefficient:

${\hat{x}}_k^{\left(i\right)}~q\left(x_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)},z_k)=N({\hat{x}}_k^{\left(i\right),a},{\hat{P}}_k^{\left(i\right)},a)---\left(16\right)$

$w_k^{\left(i\right)}=w_{k-1}^{\left(i\right)}\frac{p\left(z_k\right|{\hat{x}}_k^{\left(i\right)})p\left({\hat{x}}_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)})}{q\left({\hat{x}}_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},z_{1:k})}---\left(17\right)$

5) judge whether to exist degradation phenomena,,, circulate again next time, finish up to filtering if having degradation phenomena then resample earlier if do not have degradation phenomena then circulate next time;

Filtering finishes the back and estimates to obtain each position and attitude change information constantly of AUV, realizes the overall situation location to AUV.

Images