CN104215259B - A kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter - Google Patents

Abstract

The invention belongs to the field of underwater geomagnetic aided navigation and positioning, and in particular relates to an inertial navigation error correction method based on geomagnetic modulus gradient and particle filter. The invention includes: the inertial navigation system calculates the position of the carrier according to the accelerometer information on the submarine; predicts the position error of the carrier according to the state equation of the geomagnetic modulus gradient/inertial integrated navigation system; The gradient measurement device obtains the measured value of the geomagnetic modulus gradient of the submarine at the real position in real time; obtains the difference between the predicted modulus gradient value and the observed modulus gradient value; the particle filter estimation algorithm based on particle dynamics mimic physics optimization optimizes the system Estimation of the state: error compensation for the inertial navigation system. The invention corrects the carrier track according to the estimation result, and at the same time estimates and compensates the drift of the inertial navigation gyro. It provides an ideal way for underwater vehicles to realize precise autonomous navigation.

Description

A Method of Inertial Navigation Error Correction Based on Geomagnetic Modulus Gradient and Particle Filter

technical field

The invention belongs to the field of underwater geomagnetic aided navigation and positioning, and in particular relates to an inertial navigation error correction method based on geomagnetic modulus gradient and particle filter.

Background technique

In order to ensure the normal operation of the underwater carrier, the carrier must have long-term underwater high-precision navigation and positioning capabilities, which puts forward high requirements for underwater navigation technology. As the core equipment of the underwater navigation system, the positioning error of the inertial navigation system accumulates over time and must be readjusted and corrected. The geomagnetic field is the inherent physical field of the earth. Underwater geomagnetic navigation and positioning has the characteristics of passive, non-radiation, all-time, and all-regional. It is one of the ideal ways to realize real-time, continuous and accurate underwater autonomous navigation of underwater vehicles. , the research on underwater geomagnetic navigation theory and technology is of great significance and practical value.

In recent years, research institutions and scholars at home and abroad have carried out extensive research on error correction methods for inertial navigation systems. At present, the relatively successful underwater inertial navigation error correction methods are mainly divided into two types, namely track geometry matching algorithm and Kalman filter estimation method. Track geometry matching algorithms such as correlation matching, ICCP, etc. all require small initial position errors, and cannot meet the requirements of large initial errors. The Kalman filter method for estimating inertial navigation error requires precise measurement equations, and the measurement equations are highly nonlinear, which will lead to large errors when performing linear approximation at the measurement points. Other filtering methods exist that also require measurement equations.

The present invention proposes an inertial navigation error correction method based on the geomagnetic modulus gradient and particle filter. The geomagnetic anomaly data is used to construct the underwater geomagnetic modulus gradient reference map and stored in the integrated navigation computer. When the submarine sails underwater, the geomagnetic The modulus gradient measurement device obtains the measured value of the geomagnetic modulus gradient in the sea area it passes in real time, and obtains the predicted value by using the geomagnetic modulus gradient reference map stored in the computer in advance, and the difference between the measured value and the predicted value of the geomagnetic modulus gradient is used as Observation information, the inertial navigation error estimation is performed on the observation information through the particle filter technology, and the error compensation is performed on the inertial navigation system according to the estimation result. The invention does not need measurement equations, solves many problems such as the inability to establish practical geomagnetic gradient models and measurement equations, the usability of matching filter algorithms in large initial position errors, and realizes high-precision compensation for inertial navigation system errors.

Contents of the invention

The purpose of the present invention is to provide an inertial navigation error correction method based on geomagnetic modulus gradient and particle filter.

The purpose of the present invention is achieved like this:

Step 1. The inertial navigation system calculates the position of the carrier according to the accelerometer information on the submarine represents the obtained carrier latitude, Represents the carrier longitude obtained from the solution;

Step 2. Predict the position error of the carrier according to the state equation of the geomagnetic modulus gradient/inertial integrated navigation system The inertial navigation position is corrected by the carrier position error to obtain the predicted value of the real position Find the predicted true position in the Geomagnetic Modulus Gradient Benchmark The corresponding geomagnetic modulus gradient at The calculated value and the real geomagnetic modulus gradient at the position The relationship is:

e _m is the error of the geomagnetic modulus gradient reference map;

Step 3. When the submarine sails underwater, the real position of the submarine is obtained in real time by the geomagnetic modulus gradient measurement device on it. The measured value of the geomagnetic modulus gradient at True Geomagnetic Modulus Gradient with geomagnetic modulus gradient measurements The relationship is:

where e _s is the measurement noise of the geomagnetic modulus gradient measurement device;

Step 4. Obtain the difference between the predicted modulus gradient value and the observed modulus gradient value from steps 2 and 3, namely

Step 5. Estimate the state of the system based on the particle filter estimation algorithm based on particle dynamics mimic physics optimization: use the predicted value of the geomagnetic modulus gradient obtained in step 4 and geomagnetic modulus gradient observations The difference between the particle weights is updated to obtain an estimate of the system state; the particle filter estimation algorithm based on particle dynamics mimic physics optimization estimates the system state:

5.1 Initialization;

5.2 Forecast; from Sampling a new set of particles and calculating the particle weights;

5.3 Optimize the particle distribution; use the particle dynamics mimic physics optimization process to optimize the particle distribution and obtain a new particle set,

5.4 Iterative optimization ends;

Calculate the weight of the new particle and normalize it;

5.5 Resampling; if the number of effective particles is less than the set threshold, resampling is performed and returns

5.6 State Estimation,

The filter continuously estimates the inertial navigation position error through updating and recursion, and corrects the system position output, so that the position error gradually tends to zero; at the same time, it estimates the gyro drift and filters to obtain the current time drift;

Step 6. Perform error compensation on the inertial navigation system according to the estimation result in step 5.

The geomagnetic modulus gradient reference map is constructed as follows: the existing geomagnetic anomaly data measured by aviation and sea surface are extended to the underwater reference plane by one-step wave number domain iteration method, and the frequency domain expression, the cosine inverse transform is performed on the frequency domain expression of the geomagnetic anomaly to obtain the spatial domain expression of the geomagnetic modulus gradient, and the underwater geomagnetic modulus gradient reference map is obtained from the spatial domain expression of the geomagnetic modulus gradient, and the obtained The geomagnetic modulus gradient reference map is stored in the integrated navigation computer.

The beneficial effects of the present invention are: the proposed inertial navigation error correction method based on geomagnetic modulus gradient and particle filter adopts the method of geomagnetic anomaly conversion modulus gradient and cosine transformation to construct the geomagnetic modulus gradient reference map, which can largely Developing and utilizing the existing geomagnetic anomaly data, using the cosine transform to carry out forward modeling on the geomagnetic anomaly data can reduce the Gibbs boundary effect, and solve the current problem of lack of underwater geomagnetic modulus gradient data and the difficulty of constructing geomagnetic maps; the particles used in step 5 The filtering state estimation method does not require accurate analytical expressions and system measurement equations of the geomagnetic field, but only needs discrete observation data and matching reference maps, effectively solving the problem of the lack of practical geomagnetic modulus gradient models and measurement equations in the integrated navigation filtering method dilemma. The invention can estimate the real track of the carrier or the position of the waypoint, correct the track of the carrier according to the estimation result, and estimate and compensate the drift of the inertial navigation gyro at the same time. It provides an ideal way for underwater vehicles to realize precise autonomous navigation.

Description of drawings

Figure 1 Schematic diagram of observation plane and extension plane;

The matched filtering flow chart of the inertial/geomagnetic modulus gradient integrated navigation system of Fig. 2;

Fig. 3 Framework of inertial/geomagnetic modulus gradient integrated navigation system.

Detailed ways

Embodiments of the present invention are described in detail below in conjunction with accompanying drawings:

An inertial navigation error correction method based on geomagnetic modulus gradient and particle filter of the present invention uses geomagnetic anomaly data to construct an underwater geomagnetic modulus gradient reference map in advance and stores it in an integrated navigation computer. The geomagnetic modulus gradient measurement device obtains the measured value of the geomagnetic modulus gradient at the location of the carrier in real time, and the inertial navigation system calculates the location of the carrier, and finds the predicted value of the geomagnetic modulus gradient on the geomagnetic modulus gradient reference map according to the calculated position. The difference between the measured value and the predicted value of the geomagnetic modulus gradient is used as the observation information. The particle filter technology is used to estimate the inertial navigation error of the observation information, and the error compensation of the inertial navigation system is performed according to the estimation result. The specific steps are as follows:

Step 1. The inertial navigation system calculates the position of the carrier according to the accelerometer information on the submarine represents the obtained carrier latitude, Represents the longitude of the carrier obtained from the solution.

Step 2. Predict the position error of the carrier according to the state equation of the geomagnetic modulus gradient/inertial integrated navigation system The inertial navigation position is corrected by the carrier position error to obtain the predicted value of the real position Find the predicted true position in the Geomagnetic Modulus Gradient Benchmark The corresponding geomagnetic modulus gradient at The calculated value and the true geomagnetic modulus gradient at the position The relationship is:

e _m is the error of the geomagnetic modulus gradient reference map.

The geomagnetic modulus gradient reference map is constructed as follows: the existing aviation, sea and other measured geomagnetic anomaly data are extended to the underwater reference plane through a one-step wave number domain iteration method, and the geomagnetic The frequency domain expression of the anomaly, the cosine inverse transform is performed on the frequency domain expression of the geomagnetic anomaly to obtain the space domain expression of the geomagnetic modulus gradient, and the underwater geomagnetic modulus gradient reference map is obtained from the space domain expression of the geomagnetic modulus gradient, Store the obtained geomagnetic modulus gradient reference map in the integrated navigation computer

Step 3. When the submarine sails underwater, the real position of the submarine is obtained in real time by the geomagnetic modulus gradient measurement device on it. The measured value of the geomagnetic modulus gradient at True Geomagnetic Modulus Gradient with geomagnetic modulus gradient measurements The relationship is:

Where e _s is the measurement noise of the geomagnetic modulus gradient measurement device.

Step 4. Obtain the difference between the predicted modulus gradient value and the observed modulus gradient value from steps 2 and 3, namely

Step 5. Estimate the state of the system based on the particle filter estimation algorithm based on particle dynamics mimic physics optimization: use the predicted value of the geomagnetic modulus gradient obtained in step 4 and geomagnetic modulus gradient observations The difference between the particle weights is updated to obtain an estimate of the system state. The specific steps of estimating the state of the system by particle filter estimation algorithm based on particle dynamics mimic physics optimization are as follows:

①Initialization.

② Forecast. from Sampling a new set of particles and calculating particle weights.

③ Optimize particle distribution. The particle distribution is optimized by using the particle dynamics mimic physics optimization process to obtain a new particle set, and the iterative optimization ends.

④ Calculate the weight of the new particle and normalize it.

⑤ Resampling. If the number of effective particles is less than the set threshold, resample and return

⑥ state estimation,

The filter continuously estimates the inertial navigation position error through updating and recursion, and corrects the system position output so that the position error gradually tends to zero. At the same time, the gyro drift is estimated, and the current time drift is obtained by filtering.

Step 6. Perform error compensation on the inertial navigation system according to the estimation result in step 5.

The invention provides an inertial navigation error correction method applied underwater based on geomagnetic modulus gradient and particle filter, the method can establish a geomagnetic modulus gradient reference map without a large amount of underwater geomagnetic modulus gradient data, particle filter The state estimation method does not need to establish a geomagnetic field model and measurement equation, and the application of particle filter in integrated navigation system is not limited by Gaussian assumption and weak nonlinearity, so it has certain advantages in the realization of integrated navigation filter. The inertial navigation error correction method based on the geomagnetic modulus gradient and the particle filter of the present invention effectively solves the difficulty of constructing the underwater geomagnetic map, the instability of the long-distance downward continuation of the aeromagnetic survey data, and the particle degradation and sample degradation of the particle filter. It is suitable for high-precision error compensation of the inertial navigation system of underwater vehicles.

Step 1. The inertial navigation system calculates the position of the carrier according to the accelerometer information on the submarine represents the obtained carrier latitude, Represents the longitude of the carrier obtained from the solution.

Step 2. Predict the position error of the carrier according to the state equation of the geomagnetic modulus gradient/inertial integrated navigation system The state equation of the geomagnetic modulus gradient/inertial integrated navigation system is:

In the formula, A is the state matrix, B is the system noise matrix, and W is the system noise.

The northeast (E, N, U) geographic coordinate system is selected as the navigation coordinate system (n system), and the system state equation is composed of velocity error, attitude error and position error equations. The state variable is selected as

In the formula, δλ, is longitude and latitude error; δV _E , δV _N are east and north velocity errors; φ _E , φ _N , φ _U are attitude errors; ε _x , ε _y , ε _z are gyro constant drifts; ε _rx , ε _ry , ε _rz is the random drift of the gyro.

The inertial navigation position is corrected by the carrier position error to obtain the predicted value of the real position Find the predicted true position in the Geomagnetic Modulus Gradient Benchmark The corresponding geomagnetic modulus gradient at The calculated value and the real geomagnetic modulus gradient The relationship is:

e _m is the error of the geomagnetic modulus gradient reference map.

The construction method of the geomagnetic modulus gradient reference map is as follows: the underwater geomagnetic modulus gradient map is constructed using the existing sea area aeromagnetic data and the sea surface ship geomagnetic anomaly data, and the aeromagnetic data needs to be extended downward to the underwater reference surface , the extension process is as follows:

A one-step wavenumber domain iteration method is used to extend the aeromagnetic data and the surface ship geomagnetic anomaly data downward. The iterative process uses the upward extension of the potential field. Figure 1 shows the schematic diagram of the observation plane and the extension plane. There is a passive space between the plane Γ _A (z=h) and Γ _B (z=0), ΔT ₀ (x, y) is the geomagnetic anomaly data on Γ _B , which is the known observation, ΔT _h (x, y) is the geomagnetic anomaly on Γ _A , and is the quantity to be sought. The extension process is as follows:

(1) Vertically project the Fourier transform S ₀ (k _x , _ky ) of ΔT ₀ (x, y) onto the Γ _A plane as the initial value of the geomagnetic anomaly spectrum on the Γ _A plane

(2) When there is no field source between Γ _A and Γ _B , the potential field satisfies the Laplace equation c, and the wavenumber response function is extended upward Depend on Calculate the potential field spectrum on Γ _B

(3) Use S ₀ (k _x , k _y ) and difference correction have to λ is the step size, generally 0<λ<1.

(4) Repeat steps 2 and 3, when ε _T is a very small number, or the maximum number of iterations is reached, and the iteration ends.

(5) Spectrum of geomagnetic anomaly Perform inverse transformation to obtain the geomagnetic anomaly ΔT _h (x, y) on the downward continuation plane,

Because the cosine transform has higher energy compression performance, the minimum mean square error principle is the closest to the Karhunen-Loeve transform performance in the first-order Markov process, which can reduce the Gibbs boundary effect. Therefore, the obtained geomagnetic anomaly ΔT _h (x, y) is cosine transformed to obtain its frequency domain expression

ΔT _C (u,v)=C[ΔT _h (x,y)] (4)

Here C(·) denotes a cosine transform. The relationship between the geomagnetic modulus gradients ΔT _x , ΔT _y and ΔT _z in three spatial directions and the geomagnetic anomaly ΔT _h :

Among them, C ^-1 (·) represents the inverse cosine transform. The underwater geomagnetic modulus gradient reference map is drawn according to the expressions of the geomagnetic modulus gradients ΔT _x , ΔT _y and ΔT _z in the three spatial directions obtained.

Step 3. When the carrier sails underwater, the real position of the submarine is obtained in real time by the geomagnetic modulus gradient measurement device on it. The measured value of the geomagnetic modulus gradient at True Geomagnetic Modulus Gradient with geomagnetic modulus gradient measurements The relationship is:

Where e _s is the measurement noise of the geomagnetic modulus gradient measurement device.

Step 4. Obtain the difference between the predicted modulus gradient value and the observed modulus gradient value from steps 2 and 3, namely

Step 5. Estimate the state of the system based on the particle filter estimation algorithm based on particle dynamics mimic physics optimization: use the predicted value of the geomagnetic modulus gradient obtained in step 4 and geomagnetic modulus gradient observations The difference between the particle weights is updated to obtain an estimate of the system state. Figure 2 is a flow chart of the matched filtering of the inertial/geomagnetic modulus gradient integrated navigation system.

The specific steps of estimating the state of the system by particle filter estimation algorithm based on particle dynamics mimic physics optimization are as follows:

(1) Initialization.

(2) Prediction. from Sampling a new set of particles and calculating particle weights.

(3) Optimize particle distribution. The particle distribution is optimized by using the particle dynamics mimic physics optimization process to obtain a new particle set, and the iterative optimization ends.

(4) Calculate the weight of the new particle and normalize it.

(5) Resampling. If the number of effective particles is less than the set threshold, resample and return

(6) state estimation,

The filter continuously estimates the inertial navigation position error through updating and recursion, and corrects the system position output so that the position error gradually tends to zero. At the same time, the gyro drift is estimated, and the current time drift is obtained by filtering.

Step 6. Perform error compensation on the inertial navigation system according to the estimation result in step 5. Fig. 3 is a frame diagram of the inertial/geomagnetic modulus gradient integrated navigation system.

Beneficial effects of the present invention are described as follows:

Using the underwater geomagnetic modulus gradient reference map constructed by geomagnetic anomaly data and particle filter technology for navigation and positioning, this method has good concealment and measurement accuracy, and can perform high-precision compensation for inertial navigation errors autonomously and continuously around the clock; The invented inertial navigation error correction method based on the geomagnetic modulus gradient and particle filter effectively solves many problems such as the difficulty in constructing the underwater geomagnetic map, the inability to establish the geomagnetic gradient model and measurement equation, and the usability of the matched filter algorithm for large initial position errors. , which is suitable for high-precision error compensation of underwater submersible inertial navigation system.

Claims (2)

1. A kind of 1. ins error bearing calibration based on earth magnetism modulus gradient and particle filter, it is characterised in that：

   The position of step 1, inertial navigation system according to where accelerometer information on submarine resolves carrier Represent and resolve what is obtained Carrier latitude,Represent the carrier longitude for resolving and obtaining；

   Step 2,

   The site error of carrier is predicted according to the state equation of earth magnetism modulus gradient/inertia combined navigation systemEarth magnetism mould Amount gradient/inertia combined navigation system state equation be：

   <mrow> <mover> <mi>X</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mi>A</mi> <mi>X</mi> <mo>+</mo> <mi>B</mi> <mi>W</mi> </mrow>

   In formula, A is state matrix, and B is system noise acoustic matrix, and W is system noise；

   It is n systems that northeast day (E, N, U) geographic coordinate system, which is chosen, as navigational coordinate system, and system state equation is by velocity error, appearance State error and site error equation composition；State variable is elected as

   In formula, δ λ,For warp, latitude error；δV_E、δV_NFor east, north orientation speed error；φ_E、φ_N、φ_UFor attitude error；ε_x、 ε_y、ε_zFor gyroscope constant value drift；ε_rx、ε_ry、ε_rzFor Modelling of Random Drift of Gyroscopes；

   Inertial navigation position is modified by carrier positions error, obtains the predicted value of actual positionIn earth magnetism mould The actual position of prediction is found in amount gradient reference mapLocate corresponding earth magnetism modulus gradient The resolving value and true earth magnetism modulus gradientRelation is：

   e_mFor earth magnetism modulus gradient reference map error；

   The earth magnetism modulus gradient reference map construction method is as follows：Utilize existing marine site Aeromagnetic data and Sea Surface Ship geodetic magnetic anomaly Regular data builds underwater earth magnetism modulus gradient figure, it is necessary to which Aeromagnetic data downward continuation to underwater benchmark face, continuation process is as follows：

   Downward continuation, iterative process are carried out to Aeromagnetic data and Sea Surface Ship geodetic magnetic anomaly regular data using a step wave-number domain iterative method Potential field upward continuation, plane Γ are used_AAnd Γ_BBetween be passive space, z=h, z=0, Δ T₀(x, y) is Γ_BOn earth magnetism Abnormal data, is known observed quantity, Δ T_h(x, y) is Γ_AOn magnetic anomaly, be amount to be asked；Continuation process is as follows：

   (2.1) by Δ T₀The Fourier transformation S of (x, y)₀(k_x,k_y) upright projection is to Γ_AOn face, as Γ_AMagnetic anomaly on face Wave spectrum initial value

   (2.2) Γ is worked as_A、Γ_BBetween when there is no field source, potential field meets Laplace's equation c, with upward continuation WAVENUMBER RESPONSE functionByCalculate Γ_BOn potential field wave spectrum

   (2.3) S is used₀(k_x,k_y) withDifference correction λ is step-length, takes 0 ＜ λ ＜ 1；

   (2.4) the 2.2nd step and the 2.3rd step are repeated, whenε_TIt is the number of very little, or reaches iteration most Big number, iteration terminate；

   (2.5) magnetic anomalies over the groundMake inverse transformation, obtain the magnetic anomaly Δ T in downward continuation plane_h(x, y),

   To obtained magnetic anomaly Δ T_h(x, y) carries out cosine transform and obtains its frequency-domain expression

   ΔT_C(u, v)=C [Δ T_h(x,y)]

   Here C () represents cosine transform；Earth magnetism modulus gradient Δ T on three direction in spaces_x、ΔT_yWith Δ T_zWith ground magnetic anomaly Normal Δ T_hBetween relation：

   <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;T</mi> <mi>x</mi> </msub> <mo>=</mo> <msup> <mi>C</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mn>2</mn> <msub> <mi>&amp;pi;iu&amp;Delta;T</mi> <mi>C</mi> </msub> <mrow> <mo>(</mo> <mrow> <mi>u</mi> <mo>,</mo> <mi>v</mi> </mrow> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;T</mi> <mi>y</mi> </msub> <mo>=</mo> <msup> <mi>C</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mn>2</mn> <msub> <mi>&amp;pi;iv&amp;Delta;T</mi> <mi>C</mi> </msub> <mrow> <mo>(</mo> <mrow> <mi>u</mi> <mo>,</mo> <mi>v</mi> </mrow> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;T</mi> <mi>z</mi> </msub> <mo>=</mo> <msup> <mi>C</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mn>2</mn> <mi>&amp;pi;</mi> <msqrt> <mrow> <msup> <mi>u</mi> <mn>2</mn> </msup> <mo>+</mo> <msup> <mi>v</mi> <mn>2</mn> </msup> </mrow> </msqrt> <msub> <mi>&amp;Delta;T</mi> <mi>C</mi> </msub> <mrow> <mo>(</mo> <mrow> <mi>u</mi> <mo>,</mo> <mi>v</mi> </mrow> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>

   Wherein, C^-1() represents cosine inverse transformation；According to earth magnetism modulus gradient Δ T on the three of acquisition direction in spaces_x、ΔT_yWith ΔT_zExpression formula draws underwater earth magnetism modulus gradient reference map；

   Submarine is obtained in actual position by earth magnetism modulus gradient measuring device thereon in real time when step 3, submarine underwater navigationThe earth magnetism modulus gradient measured value at placeTrue earth magnetism modulus gradientWith Earth magnetism modulus gradient measured valueRelation is：

   Wherein e_sIt is that earth magnetism modulus gradient measuring device measures noise；

   Step 4, obtain prediction modulus gradient value by step 2,3 and observe the difference between modulus gradient value, i.e.,

   Step 5, the particle filter algorithm for estimating based on the optimization of particle dynamics mimicry physics estimate system mode：Utilize The earth magnetism modulus gradient predicted value obtained in step 4With earth magnetism modulus gradient observation Between difference, more new particle weights, obtain the estimation of system mode；Particle filter based on the optimization of particle dynamics mimicry physics Ripple algorithm for estimating estimates system mode：

   5.1 initialization；

   5.2 prediction；FromMiddle sampling new particle collection, calculates particle weights；

   5.3 optimization particle distributions；Particle distribution is optimized using particle dynamics mimicry physics optimization process, obtains new particle Collection, 5.4 iteration optimizations terminate；

   New particle weights are calculated, and are normalized；

   5.5 resampling；If number of effective particles is less than given threshold, resampling is carried out, is returned

   5.6 state estimations,

   Wave filter constantly estimates inertial navigation site error, the output of correction system position, makes site error gradual by renewal and recursion Go to zero；Gyroscopic drift is estimated at the same time, and filtering obtains current time drift；

   Step 6, according to the estimated result of step 5 carry out error compensation to inertial navigation system.
2. 2. a kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter according to claim 1, its It is characterized in that：The earth magnetism modulus gradient reference map is so built：By existing aviation, sea actual measurement magnetic anomaly number According to the frequency that the magnetic anomaly obtained through continuation by a step wave-number domain iterative method continuation to underwater benchmark face, is asked using cosine transform Domain expression formula, the frequency-domain expression progress cosine inverse transformation to magnetic anomaly obtain the spatial domain representation of earth magnetism modulus gradient, Underwater earth magnetism modulus gradient reference map is obtained by the spatial domain representation of earth magnetism modulus gradient, the earth magnetism modulus gradient base that will be obtained Quasi- figure is stored in integrated navigation computer.