CN103968839A - Single-point gravity matching method for improving CKF on basis of bee colony algorithm - Google Patents

Abstract

The invention relates to a single-point gravity matching method for improving CKF on the basis of bee colony algorithm. The method comprises the steps: acquiring an initial value X0 of the quantity of state and an initial value P0 of a covariance matrix; updating the time by adopting a CKF method to obtain a state one-step prediction value Xk and a one-step predicted covariance matrix Pk, and obtaining corresponding intensity of gravity Fk in a gravity reference map according to longitude lambda k and latitude phi k of a carrier output by an initial navigation system; carrying out the iterative search on an indicated position of the initial navigation system by adopting a manual bee colony algorithm according to the intensity Fk of the gravity and the carrier speed Vk output by a Doppler, and thus obtaining optimal estimated values lambda k' and phi k' of the longitude and latitude; measuring and updating the state estimated covariance value Pk and an optimal estimated value Xk of a state vector by adopting the CKF method according to the optimized estimated value lambda k' and phi k'.

Description

Single-point gravity matching method for improving CKF (CKF) based on swarm algorithm

Technical Field

The invention relates to a single-point gravity matching method for improving CKF based on a swarm algorithm.

Background

Inertial Navigation Systems (INS) mainly based on inertial navigation devices (gyros and accelerometers) have become navigation systems widely applied in the current navigation field, and are widely applied in the fields of navigation, aerospace and aviation. The gravity-assisted navigation can effectively make up for the defects of inertial navigation, is not limited by time, can provide real-time gravity data, and can analyze gravity intensity data on a gravity reference map by combining with an inertial navigation system. At present, a CKF (cubature Kalman filtering) method adopted in navigation has the defects of inaccuracy in estimation and the like due to the fact that the CKF method is easily influenced by environmental interference and errors.

Disclosure of Invention

The invention aims to provide a single-point gravity matching method for improving CKF based on a swarm algorithm, which can effectively improve the stability and precision of the CKF and reduce the influence of environment and deviation.

The technical scheme for realizing the aim of the invention is as follows:

a single-point gravity matching method for improving CKF based on a swarm algorithm is characterized by comprising the following steps:

step 1: collecting gravity signal F measured by gravimeter to obtain longitude lambda and latitude of carrier position informationVelocity information V_xAnd V_yCarrier attitude informationAnd

step 2: by carrier position error informationSum δ λ, speed error information δ V_xAnd δ V_yCarrier attitude informationAndtaking the drift information of the gyroscope and the accelerometer as state quantity to obtain a state vector X; establishing a system state equation by taking external noise of a system as a system disturbance vector W;

and step 3: according to the information collected in the step 1, obtaining an initial value X of the state quantity₀Initial value P of sum-covariance matrix₀(ii) a Updating time by adopting a CKF method to obtain a state one-step predicted valueSum-step prediction covariance matrix

And 4, step 4: carrier longitude lambda from inertial navigation system output_kAnd latitudeObtaining corresponding gravity intensity F in the gravity reference diagram_k(ii) a According to the strength of gravity F_kAnd the carrier velocity V of the Doppler output_kIterative search is carried out at the indicated position of the inertial navigation system by adopting an artificial bee colony algorithm to obtain the optimal estimated value lambda of the longitude and the latitude_k' and

and 5: according to longitude lambda output by inertial navigation system in real time_kAnd latitudeObtaining the relevant gravity intensity from the gravity reference mapThe observation value of the gravity intensity is obtained by the measurement of a gravimeterThe difference between the two is used to obtain the system observation vector

Step 6: according to the optimal estimated value lambda of the longitude and the latitude obtained in the step 4_k' andadopting CKF method to carry out measurement updating to obtain measurement predicted valueInnovation variance P_xz,kCovariance matrix P_zz,kFilter gain K_kState estimation covariance matrix P_kOptimal estimate of the state vector

And 7: and (6) repeating the steps 4 to 6, and estimating the state vector and the state covariance matrix in the next step.

In step 2, the state vector X is represented as follows:

wherein,is the system attitude angle; epsilon_di(i ═ x, y, z) is tourbillonThe drift error of the screw instrument constant value and satisfiesε_ri(i ═ x, y, z) is the gyroscope first order Markov drift error, which satisfiesWhere T represents the time constant of the first order Markov process of the gyroscope, w_ri(i ═ x, y, z) is the first order noise of the gyroscope,for accelerometer zero order drift, satisfy a first order Markov process, anIn the formula T_aTime constant, w, representing the first order Markov process of an accelerometer_i(i ═ x, y) is accelerometer first order noise, [ … … { (i ═ x, y) } is accelerometer first order noise]^TRepresents a transposition of the vector;

the perturbation vector W of the system is represented as follows:

W＝[w_gx w_gy w_gz w_rx w_ry w_rz w_ax w_ay]^T

wherein, w_giAnd i is white noise and satisfies w_gi～N(0,σ²),i＝x,y,z；

The state equation of the system is expressed as follows:

<math> <mrow> <mover> <mi>X</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <mi>FX</mi> <mo>+</mo> <mi>GW</mi> </mrow> </math>

wherein, F is the system matrix, G is the system control matrix, obtains the formula discretization:

<math> <mrow> <msub> <mover> <mi>X</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>X</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>W</mi> <mi>k</mi> </msub> </mrow> </math>

wherein,

<math> <mrow> <msub> <mover> <mi>&Phi;</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mi>F</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </math>

<math> <mrow> <msub> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <munderover> <mo>&Integral;</mo> <mi>k</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </munderover> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>.</mo> <mi>k</mi> </mrow> </msub> <mi>G</mi> <mrow> <mo>(</mo> <mi>&tau;</mi> <mo>)</mo> </mrow> <mi>d&tau;</mi> </mrow> </math>

in step 3, the initial value X of the state quantity₀Initial value P of sum-covariance matrix₀Expressed as:

$P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]({\hat{x}}_0=E\left(x_0\right))$

when the time updating is carried out by adopting the CKF method, the nonlinear system state equation and the measurement equation with additive Gaussian noise are considered to have the following forms

x_k＝f(x_k-1,μ_k-1)+ω_k-1

z_k＝h(x_k,μ_k)+ν_k

Wherein x is_k∈RⁿIs the state of the system at time k,is an input of the system, z_k∈R^mIs a measure of the system, ω_k-1V and v_kIs uncorrelated zero-mean white Gaussian noise with covariance matrices of Q_k-1And R_k；

A weighted Gaussian score of (x) for any function f

<math> <mrow> <msub> <mi>I</mi> <mi>N</mi> </msub> <mrow> <mo>(</mo> <mi>f</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mo>&Integral;</mo> <msup> <mi>R</mi> <mi>n</mi> </msup> </msub> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mi>N</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>;</mo> <mi>&mu;</mi> <mo>,</mo> <mi>P</mi> <mo>)</mo> </mrow> <mi>dx</mi> <mo>&ap;</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <mi>f</mi> <mrow> <mo>(</mo> <mi>&mu;</mi> <mo>+</mo> <msqrt> <mi>P</mi> </msqrt> <msub> <mi>&xi;</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> </mrow> </math>

Volume point set { ξ) with 2n elements_iIs as

$\sqrt{n}\{\left[\begin{array}{c}1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right]...\left[\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ 1\end{array}\right]\left[\begin{array}{c}-1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right]...\left[\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ -1\end{array}\right]\}$

Wherein, the volume point xi_iIs an n-dimensional column vector, n being the state dimension;cholesky decomposition for covariance P and satisfiesUsing the volume rule, under the Bayesian estimation framework, the known posterior density function at the k-th time is assumed $p\left(x_{k-1}\right)=N(x_{k-1};{\hat{x}}_{k-1},P_{k-1});$

The time update by the CKF method is as follows:

(1) calculating volume points

<math> <mrow> <msub> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msqrt> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <msub> <mi>&xi;</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <mi>j</mi> <mo>=</mo> <mn>1,2</mn> <mo>,</mo> <mo>.</mo> <mo>.</mo> <mo>.</mo> <mn>2</mn> <mi>n</mi> </mrow> </math>

(2) Propagation of volume points

<math> <mrow> <msubsup> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> <mo>*</mo> </msubsup> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>&mu;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>,</mo> <mi>j</mi> <mo>=</mo> <mn>1,2</mn> <mo>,</mo> <mo>.</mo> <mo>.</mo> <mo>.</mo> <mn>2</mn> <mi>n</mi> </mrow> </math>

(3) State prediction valueSum-covariance matrix

<math> <mrow> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> <mo>*</mo> </msubsup> </mrow> </math>

<math> <mrow> <msub> <mover> <mi>P</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> <mo>*</mo> </msubsup> <msup> <msubsup> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> <mo>*</mo> </msubsup> <mi>T</mi> </msup> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <msup> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </math>

Wherein Q_k-1And (4) inputting a covariance matrix for the system.

In the step 4, the method specifically comprises the following steps:

step 4.1: initializing parameters; randomly generating n numbers of data points with respect to a given longitude λ_kLatitude and longitudeInitial solution X in the vicinity of velocity V_iPosition ofIs given currentlyInitializing a solution group according to the following formula;

X_i＝X_i0+rand*R

wherein, X_i(i ═ 1,2, … n) is a two-dimensional vector, X_i0Is composed ofThe initial solution obtained, rand, is [0,1 ]]Any number in between, R is the neighborhood radius;

step 4.2: searching a new position solution in the range of the current solution according to the following formula, and calculating the fitness of the new solution; when the fitness of the new solution is better, the position solution is updated to a new position, otherwise, the original solution is kept unchanged, and the number s of times of non-update is added with 1;

<math> <mrow> <msubsup> <mi>V</mi> <mi>i</mi> <mi>j</mi> </msubsup> <mo>=</mo> <msubsup> <mi>X</mi> <mi>i</mi> <mi>j</mi> </msubsup> <mo>+</mo> <msubsup> <mi>&phi;</mi> <mi>i</mi> <mi>j</mi> </msubsup> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mi>i</mi> <mi>j</mi> </msubsup> <mo>-</mo> <msubsup> <mi>X</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </math>

wherein k is 1,2, … n, j is 1,2, and i is not equal to j;is [ -1,1 [ ]]Any number in between;

calculating the current fitness function value fitness:

fitness＝1/(1+f_i)

wherein,

wherein m is₁,m₂Is a factor, and satisfies m₁+m₂＝1，λ_k-1|k-1Andrespectively representing the optimal estimated values of the longitude and the latitude after the filtration at the k-1 moment, wherein F is the measured value of the current gravitational field strength, and F is the measured value of the current gravitational field strength_kIs a current longitude lambda output according to an inertial navigation system_kAnd latitudeObtaining the strength of the gravitational field, V, in a gravity reference map_kThe carrier velocity as the doppler output;

step 4.3: the probability of each lead bee being selected is calculated as follows:

<math> <mrow> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>=</mo> <mi>fithess</mi> <mo>/</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mi>fitness</mi> </mrow> </math>

P_ithe fitness function value representing the proportion of the fitness function value of a specific solution to the sum of the fitness function values;

step 4.4: selecting a leading bee by the following bee in a roulette mode, namely following the updated solution in the step 4.2, updating the position information in the step 4.1, if the position is more optimal, updating to a new solution, and if the position is not more optimal, adding 1 to the number s of times of non-updating;

step 4.5: when the number of times of non-update is greater than a given value s in the vicinity of the i-th solution_maxIf no solution more suitable than the current solution is found, leading the bees to abandon the current position solution, changing the solution into the detection bees, and initializing a new solution group according to the method of the step 4.1;

step 4.6: if the iteration times reach the maximum value, the algorithm iteration is ended, the optimal solution at the moment is output, and if not, the step 4.2 is returned to continue the circulation.

In step 6, the measurement update is performed by using the CKF method as follows:

(1) calculating volume points

<math> <mrow> <msub> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msqrt> <msub> <mover> <mi>P</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> </msqrt> <msub> <mi>&xi;</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> </mrow> </math>

(2) Volumetric point propagation

Z_j,k＝h(X_j,k,μ_k),j＝1,2,…2n.

(3) Estimating a measurement prediction value at time k

<math> <mrow> <msub> <mover> <mi>Z</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msub> <mi>Z</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </math>

(4) Estimating innovation variance

<math> <mrow> <msub> <mi>P</mi> <mrow> <mi>zz</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msub> <mi>Z</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msup> <msub> <mi>Z</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mi>T</mi> </msup> <mo>-</mo> <msub> <mover> <mi>Z</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <msup> <msub> <mover> <mi>Z</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> </mrow> </math>

(5) Estimating a covariance matrix

<math> <mrow> <msub> <mi>P</mi> <mrow> <mi>xz</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msub> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msubsup> <mi>Z</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <msup> <msub> <mover> <mi>Z</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mi>T</mi> </msup> </mrow> </math>

(6) Estimating filter gain

K_k＝P_xz,kP_zz,k ^-1

(7) State estimation value at time k

<math> <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>x</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mover> <mi>Z</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> </mrow> </math>

(8) Covariance value of state estimation at time k

<math> <mrow> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>P</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>K</mi> <mi>x</mi> </msub> <msub> <mi>P</mi> <mrow> <mi>zz</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msup> <msub> <mi>K</mi> <mi>k</mi> </msub> <mi>T</mi> </msup> </mrow> </math>

The invention has the following beneficial effects:

aiming at the problem that the mathematical model is difficult to accurately establish due to the nonlinearity and noise interference of the system, the method adopts a single-point gravity matching mode, avoids the error caused by model approximation, and improves the accuracy of the matched filtering result.

Aiming at a single-point gravity matching mode, the invention adopts a CKF method and a Bayesian framework based on Gaussian assumption to resolve nonlinear filtering into an integral problem of a product of a nonlinear function and Gaussian probability density, and has the advantages of simple realization, high estimation precision, no need of calculating a Jacobian matrix like an EKF algorithm and no need of pre-assuming parameters like a UKF algorithm.

Aiming at the problem of filter performance reduction caused by inaccurate filter models, the invention adopts an artificial bee colony algorithm to improve the CKF. The artificial bee colony algorithm has the advantages of simple flow, strong robustness, clear division of labor of multiple roles in a colony, good cooperative work mechanism and easy combination with other algorithms. According to the invention, the artificial bee colony algorithm is adopted to carry out secondary matching on the CKF one-step prediction information, and the matched state quantity is used for the measurement updating process of the CKF, so that the stability and the precision of the CKF are improved, the influence of environment and deviation is reduced, and the precision of matching positioning is improved.

Drawings

FIG. 1 is a flow chart of a method of the present invention;

fig. 2 is a schematic block diagram of the single-point gravity matching method for improving CKF based on bee colony algorithm according to the present invention.

Detailed Description

As shown in fig. 1, the single-point gravity matching method for improving CKF based on bee colony algorithm of the present invention includes the following steps:

step 1: collecting gravity signal F measured by gravimeter, and obtaining carrier position information lambda and lambda output by inertial navigation systemVelocity information V_xAnd V_yCarrier attitude informationAnd

step 2: by carrier position error informationSum δ λ, speed error information δ V_xAnd δ V_yCarrier attitude informationAndtaking the drift information of the gyroscope and the accelerometer as state quantity to obtain a state vector X; and (3) taking the external noise of the system as a system disturbance vector W, and further establishing a system state equation.

In particular, using carrier position error informationSum δ λ, speed error information δ V_xAnd δ V_yCarrier attitude informationAndthe drift information of the gyroscope and the accelerometer is used as a state vector, and a system state vector is obtained through definition

Wherein,is the system attitude angle; epsilon_di(i ═ x, y, z) is the gyro constant drift error and satisfiesε_ri(i ═ x, y, z) is the gyroscope first order Markov drift error, which satisfiesWhere T represents the time constant of the first order Markov process of the gyroscope, w_ri(i ═ x, y, z) is the first order noise of the gyroscope,for accelerometer zero order drift, satisfy a first order Markov process, anIn the formula T_aTime constant, w, representing the first order Markov process of an accelerometer_i(i ═ x, y) is accelerometer first order noise, [ … … { (i ═ x, y) } is accelerometer first order noise]^TRepresenting the transpose of the vector. The perturbation vector of the system can be expressed as follows:

W＝[w_gx w_gy w_gz w_rx w_ry w_rz w_ax w_ay]^T

wherein, w_giAnd i is white noise and satisfies w_gi～N(0,σ²) And i is x, y, z. The state equation of the system can be expressed as

<math> <mrow> <mover> <mi>X</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <mi>FX</mi> <mo>+</mo> <mi>GW</mi> </mrow> </math>

Wherein, F is the system matrix, G is the system control matrix, obtains the formula discretization:

<math> <mrow> <msub> <mover> <mi>X</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>X</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msub> <mi>W</mi> <mi>k</mi> </msub> </mrow> </math>

wherein,

<math> <mrow> <msub> <mover> <mi>&Phi;</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mi>F</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </math>

<math> <mrow> <msub> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <munderover> <mo>&Integral;</mo> <mi>k</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </munderover> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>.</mo> <mi>k</mi> </mrow> </msub> <mi>G</mi> <mrow> <mo>(</mo> <mi>&tau;</mi> <mo>)</mo> </mrow> <mi>d&tau;</mi> </mrow> </math>

and step 3: according to the information collected in the step 1, obtaining an initial value X of the state quantity₀Initial value P of sum-covariance matrix₀(ii) a Updating time by adopting a CKF method to obtain a state one-step predicted valueSum-step prediction covariance matrix

Initial value X of state quantity₀Initial value P of sum-covariance matrix₀Expressed as:

wherein,is the system attitude angle; epsilon_di0(i ═ x, y, z) is the constant drift of the gyroscopeShift error and satisfyε_ri0(i ═ x, y, z) is the gyroscope first order Markov drift error, which satisfiesIn the above formula, T represents the time constant of the first-order Markov process of the gyroscope, w_ri(i ═ x, y, z) is the first order noise of the gyroscope;for accelerometer zero order drift, satisfy a first order Markov process, anIn the above formula, T_aTime constant, w, representing the first order Markov process of an accelerometer_i(i ═ x, y) is accelerometer first order noise, [ … … { (i ═ x, y) } is accelerometer first order noise]^TRepresenting the transpose of the vector. Initial value of covariance matrix, i.e.

$P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]({\hat{x}}_0=E\left(x_0\right))$

Specifically, the initial values of the state quantity and the covariance matrix are obtained as

$P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]({\hat{x}}_0=E\left(x_0\right))$

CKF time updates are made. Consider a nonlinear system state equation with additive Gaussian noise and a metrology equation having the form

x_k＝f(x_k-1,μ_k-1)+ω_k-1

z_k＝h(x_k,μ_k)+ν_k

Wherein x is_k∈RⁿIs the state of the system at time k,is an input of the system, z_k∈R^mIs a measure of the system, ω_k-1V and v_kIs uncorrelated zero-mean white Gaussian noise with covariance matrices of Q_k-1And R_k。

The kernel problem of nonlinear Gaussian filtering is that the integral of the product of multivariable nonlinear function and Gaussian density function is solved, the weighted problem of Gaussian integral is replaced by the summation of 2n volume points by the third-order volume integral rule, and the weighted Gaussian score of any function f (x) is

<math> <mrow> <msub> <mi>I</mi> <mi>N</mi> </msub> <mrow> <mo>(</mo> <mi>f</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mo>&Integral;</mo> <msup> <mi>R</mi> <mi>n</mi> </msup> </msub> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mi>N</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>;</mo> <mi>&mu;</mi> <mo>,</mo> <mi>P</mi> <mo>)</mo> </mrow> <mi>dx</mi> <mo>&ap;</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <mi>f</mi> <mrow> <mo>(</mo> <mi>&mu;</mi> <mo>+</mo> <msqrt> <mi>P</mi> </msqrt> <msub> <mi>&xi;</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> </mrow> </math>

Volume point set { ξ) with 2n elements_iIs as

$\sqrt{n}\{\left[\begin{array}{c}1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right]...\left[\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ 1\end{array}\right]\left[\begin{array}{c}-1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right]...\left[\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ -1\end{array}\right]\}$

Wherein, the volume point xi_iIs an n-dimensional column vector, n being the state dimension;cholesky decomposition for covariance P and satisfiesUsing the volume rule, under the Bayesian estimation framework, the known posterior density function at the k-th time is assumed $p\left(x_{k-1}\right)=N(x_{k-1};{\hat{x}}_{k-1},P_{k-1}).$

The CKF time updates are as follows:

(1) calculating volume points

<math> <mrow> <msub> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msqrt> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <msub> <mi>&xi;</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <mi>j</mi> <mo>=</mo> <mn>1,2</mn> <mo>,</mo> <mo>.</mo> <mo>.</mo> <mo>.</mo> <mn>2</mn> <mi>n</mi> </mrow> </math>

(2) Propagation of volume points

<math> <mrow> <msubsup> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> <mo>*</mo> </msubsup> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>&mu;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>,</mo> <mi>j</mi> <mo>=</mo> <mn>1,2</mn> <mo>,</mo> <mo>.</mo> <mo>.</mo> <mo>.</mo> <mn>2</mn> <mi>n</mi> </mrow> </math>

(3) State prediction values and covariance matrices

<math> <mrow> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> <mo>*</mo> </msubsup> </mrow> </math>

<math> <mrow> <msub> <mover> <mi>P</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> <mo>*</mo> </msubsup> <msup> <msubsup> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> <mo>*</mo> </msubsup> <mi>T</mi> </msup> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <msup> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </math>

Wherein Q is_k-1And (4) inputting a covariance matrix for the system.

And 4, step 4: from longitude λ output by inertial navigation system_kAnd latitudeObtaining the strength F of the gravity field in the gravity reference diagram_k. Doppler output carrier velocity of V_k. Adopting artificial bee colony algorithm to carry out iterative search at the indicated position of the inertial navigation system, continuously updating longitude and latitude information and defining a cost function f_i

Wherein m is₁,m₂Is a factor, and satisfies m₁+m₂＝1，λ_k-1|k-1Andrespectively representing the optimal estimated values of the longitude and the latitude after filtering at the moment k-1, F is a measured value of the gravity field strength output by the gravimeter,representing the square of the two norms. V_kThe carrier velocity of the doppler output. Due to the cost function f_i>0, further defining a fitness function fitness

fitness＝1/(1+f_i)

After initial values and iteration times are given, optimal estimated values lambda of longitude and latitude can be obtained_k' and

specifically, the current longitude λ is output according to the inertial navigation system_kAnd latitudeObtaining the strength F of the gravity field in the gravity reference diagram_kDoppler output carrier velocity of V_k. Adopting an artificial bee colony algorithm to carry out iterative operation, and specifically comprising the following steps:

step 4.1: initializing parameters: randomly generating n numbers of data points with respect to a given longitude λ_kLatitude and longitudeInitial solution X in the vicinity of velocity V_iInitial positionIs given currentlyInitializing a solution group according to the following formula;

X_i＝X_i0+rand*R

wherein, X_i(i ═ 1,2, … n) is a two-dimensional vector, X_i0Is composed ofThe initial solution obtained. rand is [0,1 ]]Any number in between, R is the neighborhood radius。

Step 4.2: according to the following formula, a new position solution is searched in the range of the current solution, and the fitness of the new solution is calculated. When the fitness of the new solution is better, the position solution is updated to a new position, otherwise, the original solution is kept unchanged, and the number s of times of non-update is added with 1;

<math> <mrow> <msubsup> <mi>V</mi> <mi>i</mi> <mi>j</mi> </msubsup> <mo>=</mo> <msubsup> <mi>X</mi> <mi>i</mi> <mi>j</mi> </msubsup> <mo>+</mo> <msubsup> <mi>&phi;</mi> <mi>i</mi> <mi>j</mi> </msubsup> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mi>i</mi> <mi>j</mi> </msubsup> <mo>-</mo> <msubsup> <mi>X</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </math>

wherein k is 1,2, … n, j is 1,2, and i is not equal to j;is [ -1,1 [ ]]Any number therebetween.

Calculating the current cost function value f_i：

Wherein m is₁,m₂Is a factor, and satisfies m₁+m₂＝1，λ_k-1|k-1Andrespectively representing the optimal estimated values of the longitude and the latitude after the filtration at the k-1 moment, wherein F is the measured value of the current gravitational field strength, and F is the measured value of the current gravitational field strength_kIs based on the current output of the inertial navigation systemLongitude λ_kAnd latitudeThe gravitational field strength from the gravity reference map. Due to the cost function f_i>0, further defining a fitness function fitness

fitness＝1/(1+f_i)

And calculating the current fitness function value fitness.

Step 4.3: the probability of each lead bee being selected is calculated as follows:

<math> <mrow> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>=</mo> <mi>fitness</mi> <mo>/</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mi>fitness</mi> </mrow> </math>

wherein, P_iThe fitness function value representing the ratio of the fitness function value for a particular solution to the sum of the fitness function values.

Step 4.4: selecting a leading bee by the following bee in a roulette mode, namely the updated solution in the step 4.2 for following, updating the position information in the step 4.1, if the position is more optimal, updating to a new solution, and if the position is not more optimal, adding 1 to the number s of times of non-updating;

step 4.5: when the number of times of non-update is greater than a given value s in the vicinity of the i-th solution_maxIf no solution more suitable than the current solution is found, leading the bee to abandon the current position solution, changing the solution into a scout bee, and searching a new solution according to the method in the step 4.2;

step 4.6: if the iteration times reach the maximum value, the algorithm iteration is ended, the optimal solution at the moment is output, and if not, the step 4.2 is returned to continue the circulation.

And 5: navigation by inertiaLongitude information lambda output by system in real time_kAnd latitude informationObtaining the relevant gravity strength value according to the gravity reference mapThe observed value of the gravity intensity is obtained by the measurement of a gravimeterThe difference between the two is used to obtain the system observation vector

Specifically, the longitude information lambda is output in real time according to the inertial navigation system_kAnd latitude informationObtaining the relevant gravity strength value according to the gravity reference mapThe observation value of the gravity intensity is obtained by the measurement of a gravimeterThe difference between the two is used to obtain the system observation vector

Wherein,δg_min order to be a reference map error,δg_ggmeasuring errors for a gravimeter;

step 6: longitude lambda of the optimal estimate obtained in step 4_k' and latitudeCarrier attitude information output by inertial navigation systemAndadopting CKF to carry out measurement updating to obtain a measurement predicted valueInnovation variance P_xz,kCovariance matrix P_zz,kFilter gain K_kState estimation mean square error value P_kOptimal estimate of the state vector

Specifically, the longitude λ of the optimal estimation obtained in step 4 is applied_k' and latitudeAnd Doppler output carrier velocity V_kThe measurement update is performed on the CKF, which specifically includes the following steps:

(1) calculating volume points

<math> <mrow> <msub> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msqrt> <msub> <mover> <mi>P</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> </msqrt> <msub> <mi>&xi;</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> </mrow> </math>

(2) Volumetric point propagation

Z_j,k＝h(X_j,k,μ_k),j＝1,2,…2n.

(3) Estimating a measurement prediction value at time k

<math> <mrow> <msub> <mover> <mi>Z</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msub> <mi>Z</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </math>

(4) Estimating innovation variance

<math> <mrow> <msub> <mi>P</mi> <mrow> <mi>zz</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msub> <mi>Z</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msup> <msub> <mi>Z</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mi>T</mi> </msup> <mo>-</mo> <msub> <mover> <mi>Z</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <msup> <msub> <mover> <mi>Z</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> </mrow> </math>

(5) Estimating a covariance matrix

<math> <mrow> <msub> <mi>P</mi> <mrow> <mi>xz</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msub> <mi>X</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msubsup> <mi>Z</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <msup> <msub> <mover> <mi>Z</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mi>T</mi> </msup> </mrow> </math>

(6) Estimating filter gain

K_k＝P_xz,kP_zz,k ^-1

(7) State estimation value at time k

<math> <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>x</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mover> <mi>Z</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> </mrow> </math>

(8) Covariance value of state estimation at time k

<math> <mrow> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>P</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>K</mi> <mi>x</mi> </msub> <msub> <mi>P</mi> <mrow> <mi>zz</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msup> <msub> <mi>K</mi> <mi>k</mi> </msub> <mi>T</mi> </msup> </mrow> </math>

And 7: using new initial values and variances obtained by CKF filtering, optimal estimated values of longitude and latitude and velocity V_kAnd (6) repeating the steps 4 to 6, and carrying out the next filtering and the processing of the artificial bee colony algorithm. As can be seen from the above embodiments, for single-point gravity matching, compared with the existing matching method, the single-point gravity matching method for improving the CKF based on the artificial bee colony algorithm provided by the invention solves the problem of a susceptible loop in the CKF filtering processAnd the estimation is inaccurate due to environmental interference and error influence. The bee colony algorithm has the advantages of clear multi-role division work in the colony, good cooperative work mechanism and easy combination with other algorithms. And performing secondary matching on the CKF one-step prediction information by adopting an artificial bee colony algorithm, and using the matched state quantity in the measurement updating process of the CKF filtering, so that the stability and the precision of the CKF filtering are improved, the influence of the environment and the deviation is reduced, and the matching positioning precision is improved.

Claims (5)

1. A single-point gravity matching method for improving CKF based on bee colony algorithm is characterized in that: the method comprises the following steps:

step 1: collecting gravity signal F measured by gravimeter to obtain longitude lambda and latitude of carrier position informationVelocity information V_xAnd V_yCarrier attitude informationAnd

step 2: by carrier position error informationSum δ λ, speed error information δ V_xAnd δ V_yCarrier attitude informationAndtaking the drift information of the gyroscope and the accelerometer as state quantity to obtain a state vector X; establishing a system state equation by taking external noise of a system as a system disturbance vector W;

and step 3: according to the information collected in the step 1, obtaining an initial value X of the state quantity₀Initial value P of sum-covariance matrix₀(ii) a Updating time by adopting a CKF method to obtain a state one-step predicted valueSum-step prediction covariance matrix

And 4, step 4: carrier longitude lambda from inertial navigation system output_kAnd latitudeObtaining corresponding gravity intensity F in the gravity reference diagram_k(ii) a According to the strength of gravity F_kAnd the carrier velocity V of the Doppler output_kIterative search is carried out at the indicated position of the inertial navigation system by adopting an artificial bee colony algorithm to obtain the optimal estimated value lambda of the longitude and the latitude_k' and

and 5: according to longitude lambda output by inertial navigation system in real time_kAnd latitudeObtaining the relevant gravity intensity from the gravity reference mapThe observation value of the gravity intensity is obtained by the measurement of a gravimeterThe difference between the two is used to obtain the system observation vector

Step 6: according to the optimal estimated value lambda of the longitude and the latitude obtained in the step 4_k' andadopting CKF method to perform measurement update to obtain the measurement prediction valueInnovation variance P_xz,kCovariance matrix P_zz,kFilter gain K_kState estimation covariance matrix P_kOptimal estimate of the state vector

And 7: and (6) repeating the steps 4 to 6, and estimating the state vector and the state covariance matrix in the next step.

2. The bee colony algorithm-based single point gravity matching method for improving CKF according to claim 1, wherein:

in step 2, the state vector X is represented as follows:

wherein,is the system attitude angle; epsilon_di(i ═ x, y, z) is the gyro constant drift error and satisfiesε_ri(i ═ x, y, z) is the gyroscope first order Markov drift error, which satisfiesWhere T represents the time constant of the first order Markov process of the gyroscope, w_ri(i ═ x, y, z) is the first order noise of the gyroscope,for accelerometer zero order drift, satisfy a first order Markov process, anIn the formula T_aTime constant, w, representing the first order Markov process of an accelerometer_i(i ═ x, y) is accelerometer first order noise, [ … … { (i ═ x, y) } is accelerometer first order noise]^TRepresents a transposition of the vector;

the perturbation vector W of the system is represented as follows:

W＝[w_gx w_gy w_gz w_rx w_ry w_rz w_ax w_ay]^T

wherein, w_giAnd i is white noise and satisfies w_gi～N(0,σ²),i＝x,y,z；

The state equation of the system is expressed as follows:

wherein, F is the system matrix, G is the system control matrix, obtains the formula discretization:

wherein,

。

3. the bee colony algorithm-based single point gravity matching method for improving CKF according to claim 2, wherein: in step 3, the initial value X of the state quantity₀Initial value P of sum-covariance matrix₀Expressed as:

when the time updating is carried out by adopting the CKF method, the nonlinear system state equation and the measurement equation with additive Gaussian noise are considered to have the following forms

x_k＝f(x_k-1,μ_k-1)+ω_k-1

z_k＝h(x_k,μ_k)+ν_k

Wherein x is_k∈RⁿIs the state of the system at time k,is an input of the system, z_k∈R^mIs a measure of the system, ω_k-1V and v_kIs uncorrelated zero-mean white Gaussian noise with covariance matrices of Q_k-1And R_k；

A weighted Gaussian score of (x) for any function f

Volume point set { ξ) with 2n elements_iIs as

Wherein, the volume point xi_iIs an n-dimensional column vector, n being the state dimension;cholesky decomposition for covariance P and satisfiesUsing the volume rule, under the Bayesian estimation framework, the known posterior density function at the k-th time is assumed

The time update by the CKF method is as follows:

(1) calculating volume points

(2) Propagation of volume points

(3) State prediction valueSum-covariance matrix

Wherein Q_k-1And (4) inputting a covariance matrix for the system.

4. The method of claim 3 for improving single point gravity matching of CKF based on bee colony algorithm, wherein the method comprises the following steps: in the step 4, the method specifically comprises the following steps:

step 4.1: initializing parameters; randomly generating n numbers of data points with respect to a given longitude λ_kLatitude and longitudeInitial solution X in the vicinity of velocity V_iPosition ofIs given currentlyInitializing a solution group according to the following formula;

X_i＝X_i0+rand*R

wherein, X_i(i ═ 1,2, … n) is a two-dimensional vector, X_i0Is composed ofThe initial solution obtained, rand, is [0,1 ]]Any number in between, R is the neighborhood radius;

step 4.2: searching a new position solution in the range of the current solution according to the following formula, and calculating the fitness of the new solution; when the fitness of the new solution is better, the position solution is updated to a new position, otherwise, the original solution is kept unchanged, and the number s of times of non-update is added with 1;

wherein k is 1,2, … n, j is 1,2, and i is not equal to j;is [ -1,1 [ ]]Any number in between;

calculating the current fitness function value fitness:

fitness＝1/(1+f_i)

wherein,

wherein m is₁,m₂Is a factor, and satisfies m₁+m₂＝1，λ_k-1|k-1Andrespectively representing the optimal estimated values of the longitude and the latitude after the filtration at the k-1 moment, wherein F is the measured value of the current gravitational field strength, and F is the measured value of the current gravitational field strength_kIs a current longitude lambda output according to an inertial navigation system_kAnd latitudeObtaining the strength of the gravitational field, V, in a gravity reference map_kThe carrier velocity as the doppler output;

step 4.3: the probability of each lead bee being selected is calculated as follows:

P_ithe fitness function value representing the proportion of the fitness function value of a specific solution to the sum of the fitness function values;

step 4.4: selecting a leading bee by the following bee in a roulette mode, namely following the updated solution in the step 4.2, updating the position information in the step 4.1, if the position is more optimal, updating to a new solution, and if the position is not more optimal, adding 1 to the number s of times of non-updating;

step 4.5: when the number of times of non-update is greater than a given value s in the vicinity of the i-th solution_maxIf no solution more suitable than the current solution is found, leading the bees to abandon the current position solution, changing the solution into the detection bees, and initializing a new solution group according to the method of the step 4.1;

step 4.6: if the iteration times reach the maximum value, the algorithm iteration is ended, the optimal solution at the moment is output, and if not, the step 4.2 is returned to continue the circulation.

5. The method for improving single-point gravity matching of CKF based on bee colony algorithm as claimed in claim 4, wherein in step 6, the measurement update is performed by using the CKF method as follows:

(1) calculating volume points

(2) Volumetric point propagation

Z_j,k＝h(X_j,k,μ_k),j＝1,2,…2n.

(3) Estimating a measurement prediction value at time k

(4) Estimating innovation variance

(5) Estimating a covariance matrix

(6) Estimating filter gain

K_k＝P_xz,kP_zz,k ^-1

(7) State estimation value at time k

(8) Covariance value of state estimation at time k

。