CN104215259B - A kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter - Google Patents
A kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter Download PDFInfo
- Publication number
- CN104215259B CN104215259B CN201410416153.4A CN201410416153A CN104215259B CN 104215259 B CN104215259 B CN 104215259B CN 201410416153 A CN201410416153 A CN 201410416153A CN 104215259 B CN104215259 B CN 104215259B
- Authority
- CN
- China
- Prior art keywords
- mrow
- earth magnetism
- modulus gradient
- magnetism modulus
- error
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
- Geophysics And Detection Of Objects (AREA)
Abstract
The invention belongs to underwater geomagnetic auxiliary navigation positioning field, and in particular to a kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter.The present invention includes:Position of the inertial navigation system according to where accelerometer information on submarine resolves carrier；The site error of carrier is predicted according to the state equation of earth magnetism modulus gradient/inertia combined navigation system；Earth magnetism modulus gradient measured value of the submarine in actual position is obtained by earth magnetism modulus gradient measuring device thereon in real time during submarine underwater navigation；Obtain prediction modulus gradient value and observe the difference between modulus gradient value；Particle filter algorithm for estimating based on the optimization of particle dynamics mimicry physics estimates system mode：Error compensation is carried out to inertial navigation system.The present invention corrects carrier flight path according to estimated result, while inertial navigation gyroscopic drift is estimated and compensated.Realize that accurate independent navigation provides a kind of desirable route for underwater carrier.
Description
Technical field
The invention belongs to underwater geomagnetic auxiliary navigation positioning field, and in particular to be based on earth magnetism modulus gradient and grain to one kind
The ins error bearing calibration of son filtering.
Background technology
To ensure underwater carrier normal operation, carrier must possess high-precision navigator fix ability underwater for a long time, this is right
Underwater navigation technology proposes very high requirement.As the core equipment of underwater navigation system, inertial navigation system position error
It is with accumulated time, it is necessary to carry out biharmonic correction.Earth's magnetic field is the intrinsic physical field of the earth, and underwater earth-magnetism navigation positioning has
The features such as passive, radiationless, round-the-clock, full region, be to realize real-time, continuous, the accurate underwater independent navigation of underwater vehicle
One of desirable route, the theoretical research with technology of underwater earth-magnetism navigation is of great significance and real value.
In recent years, domestic and international research institution and scholars have carried out to the widely studied of inertial navigation system error calibration method.
Relatively successfully underwater ins error bearing calibration is broadly divided into two big types, i.e. flight path geometric match algorithm and Kalman at present
Filter method of estimation.Flight path geometric match algorithm such as relevant matches, ICCP etc. require small initial position error, it is impossible to suitable
Answer the requirement of big initial error.Kalman Filter Estimation ins error method needs accurate measurement equation, and measurement equation is non-
In the case of linearly relatively strong, large error can be caused when measuring point carries out linear approximation.Other filtering methods, which exist, to be also required for
Measurement equation.
The present invention proposes a kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter, utilizes ground magnetic anomaly
Regular data builds underwater earth magnetism modulus gradient reference map and is simultaneously stored in integrated navigation computer, by thereon during submarine underwater navigation
Earth magnetism modulus gradient measuring device obtain in real time the earth magnetism modulus gradient measured value through marine site, using being previously stored in computer
In earth magnetism modulus gradient reference map obtain predicted value, earth magnetism modulus gradient measured value and predicted value and between difference as observing
Information, carries out ins error estimation to observation information by particle filter technology, inertial navigation system is missed according to estimated result
Difference compensation.Measurement equation is not required in the present invention, and the earth magnetism gradient former and measurement equation for solving practicality can not be established, matched filtering
Algorithm realizes the high-accuracy compensation to inertial navigation system error in problems such as the availabilities of big initial position error.
The content of the invention
It is an object of the invention to provide a kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter.
The object of the present invention is achieved like this：
The position of step 1, inertial navigation system according to where accelerometer information on submarine resolves carrier Represent and resolve
Obtained carrier latitude,Represent the carrier longitude for resolving and obtaining；
Step 2, the site error according to the state equation of earth magnetism modulus gradient/inertia combined navigation system prediction carrierInertial navigation position is modified by carrier positions error, obtains the predicted value of actual position
The actual position of prediction is found in earth magnetism modulus gradient reference mapLocate corresponding earth magnetism modulus gradientResolving value and true earth magnetism modulus gradient at positionRelation is：
emFor earth magnetism modulus gradient reference map error；
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 esIt 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 observationBetween difference, more new particle weights, obtain the estimation of system mode；Based on particle dynamics mimetic
The particle filter algorithm for estimating of reason optimization 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 grain
Subset,
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 by renewal and recursion
Gradually 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.
Earth magnetism modulus gradient reference map is so built：The actual measurement magnetic anomaly data such as existing aviation, sea are led to
A step wave-number domain iterative method continuation is crossed to underwater benchmark face, the frequency domain table of the magnetic anomaly obtained through continuation is sought using cosine transform
Up to formula, the frequency-domain expression progress cosine inverse transformation to magnetic anomaly obtains the spatial domain representation of earth magnetism modulus gradient, by ground
The spatial domain representation of magnetic modulus gradient obtains underwater earth magnetism modulus gradient reference map, the earth magnetism modulus gradient reference map that will be obtained
It is stored in integrated navigation computer.
The beneficial effects of the present invention are：The ins error correction side based on earth magnetism modulus gradient and particle filter proposed
Method, the method that modulus gradient and cosine transform are changed using magnetic anomaly build earth magnetism modulus gradient reference map, can be in very great Cheng
Existing magnetic anomaly data are developed and utilized on degree, carrying out forward modeling to magnetic anomaly data using cosine transform can reduce
Gibbs boundary effects, solve the problems, such as that currently lacking underwater earth magnetism modulus gradient data and geomagnetic chart is difficult to structure；Step 5 is adopted
Particle filter method for estimating state, without the accurate analytical expression and system measurements equation in earth's magnetic field, only needs discrete
Observed quantity data and matching reference map, effectively solve in integrated navigation filtering method without practical earth magnetism modulus gradient model and amount
Survey the predicament of equation.The present invention can estimate the true flight path of carrier or way point position, corrected and carried according to estimated result
Body flight path, while inertial navigation gyroscopic drift is estimated and compensated.Realize that accurate independent navigation provides a kind of reason for underwater carrier
Think approach.
Brief description of the drawings
Fig. 1 plane of visions and continuation floor map；
The matched filtering flow chart of Fig. 2 inertia/earth magnetism modulus gradient integrated navigation system；
Fig. 3 inertia/earth magnetism modulus gradient integrated navigation system frame.
Embodiment
Embodiments of the present invention are described in detail below in conjunction with the accompanying drawings：
A kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter of the present invention, utilizes magnetic anomaly
Data build underwater earth magnetism modulus gradient reference map and are stored in integrated navigation computer in advance, by it during carrier underwater navigation
Upper earth magnetism modulus gradient measuring device obtains the earth magnetism modulus gradient measured value of carrier position in real time, is resolved by inertial navigation system
Carrier position, earth magnetism modulus gradient predicted value is found according to position is resolved on earth magnetism modulus gradient reference map.Earth magnetism mould
Measure gradient measured value and predicted value and between difference be used as observation information, by particle filter technology to observation information progress inertial navigation
Estimation error, error compensation is carried out according to estimated result to inertial navigation system.It is comprised the following steps that：
The position of step 1, inertial navigation system according to where accelerometer information on submarine resolves carrier Represent and resolve
Obtained carrier latitude,Represent the carrier longitude for resolving and obtaining.
Step 2, the site error according to the state equation of earth magnetism modulus gradient/inertia combined navigation system prediction carrierInertial navigation position is modified by carrier positions error, obtains the predicted value of actual position
The actual position of prediction is found in earth magnetism modulus gradient reference mapLocate corresponding earth magnetism modulus gradientThe resolving value and true earth magnetism modulus gradient at the positionRelation is：
emFor earth magnetism modulus gradient reference map error.
The earth magnetism modulus gradient reference map is so built：Magnetic anomaly is surveyed on existing aviation, sea etc.
Data ask the magnetic anomaly that is obtained through continuation using cosine transform by a step wave-number domain iterative method continuation to underwater benchmark face
Frequency-domain expression, the frequency-domain expression progress cosine inverse transformation to magnetic anomaly obtain the spatial domain expression of earth magnetism modulus gradient
Formula, underwater earth magnetism modulus gradient reference map is obtained by the spatial domain representation of earth magnetism modulus gradient, by obtained earth magnetism modulus ladder
Degree reference map is stored in integrated navigation computer
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 esIt 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：Profit
With 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
What ripple algorithm for estimating estimated system mode comprises the following steps that：
1. initialize.
2. predict.FromMiddle sampling new particle collection, calculates particle weights.
3. optimize particle distribution.Particle distribution is optimized using particle dynamics mimicry physics optimization process, obtains new grain
Subset, iteration optimization terminate.
4. calculating new particle weights, and normalize.
5. resampling.If number of effective particles is less than given threshold, resampling is carried out, is returned
6. state estimation,
Wave filter constantly estimates inertial navigation site error, the output of correction system position, makes site error by renewal and recursion
Gradually 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.
Corrected the present invention provides a kind of applied to underwater based on the ins error of earth magnetism modulus gradient and particle filter
Method, this method largely underwater earth magnetism modulus gradient data need not can establish earth magnetism modulus gradient reference map, particle filter
Method for estimating state is without establishing Geomagnetic Field Model and measurement equation, and application of the particle filter in integrated navigation system
Assume the limitation with small nonlinearity from Gauss, have certain advantage in terms of integrated navigation filters realization.The present invention based on
The ins error bearing calibration of earth magnetism modulus gradient and particle filter, efficiently solve underwater geomagnetic chart be difficult to build, magnetic air
The problems such as survey big unstability apart from downward continuation of data, the sample degeneracy and poor sample of particle filter, suitable for underwater
The high-precision error compensation of latent device inertial navigation system.
The position of step 1, inertial navigation system according to where accelerometer information on submarine resolves carrier Represent and resolve
Obtained carrier latitude,Represent the carrier longitude for resolving and obtaining.
Step 2, the site error according to the state equation of earth magnetism modulus gradient/inertia combined navigation system prediction carrierThe state equation of earth magnetism modulus gradient/inertia combined navigation system is：
In formula, A is state matrix, and B is system noise acoustic matrix, and W is system noise.
Northeast day (E, N, U) geographic coordinate system is chosen as navigational coordinate system (n systems), system state equation is missed by speed
Difference, attitude error and site error equation composition.State variable is elected as
In formula, δ λ,For warp, latitude error；δVE、δVNFor east, north orientation speed error；φE、φN、φUMissed for posture
Difference；ε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
The actual position of prediction is found in modulus gradient reference mapLocate corresponding earth magnetism modulus gradient
The resolving value and true earth magnetism modulus gradientRelation is：
emFor 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 by Aeromagnetic data downward continuation to underwater benchmark face, continuation process
It is as follows：
Downward continuation, iteration are carried out to Aeromagnetic data and Sea Surface Ship geodetic magnetic anomaly regular data using a step wave-number domain iterative method
Process has used potential field upward continuation, and Fig. 1 gives plane of vision and continuation floor map.Plane ΓA(z=h) and ΓB(z
=0) it is passive space between, Δ T0(x, y) is ΓBOn magnetic anomaly data, be known observed quantity, Δ Th(x, y) is ΓA
On magnetic anomaly, be amount to be asked.Continuation process is as follows：
(1) by Δ T0The Fourier transformation S of (x, y)0(kx,ky) upright projection is to ΓAOn face, as ΓAThe magnetic anomaly of face Shangdi
Ordinary wave composes initial value
(2) Γ is worked asA、Γ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
(3) S is used0(kx,ky) withDifference correction λ is step-length, generally takes 0 ＜ λ ＜ 1.
(4) the 2nd step and the 3rd step are repeated, whenεTIt is the number of very little, or reaches iteration most
Big number, iteration terminate.
(5) magnetic anomalies over the groundMake inverse transformation, obtain the magnetic anomaly Δ T in downward continuation planeh(x, y),
Since cosine transform has the energy compression performance of higher, missed in single order markoff process according to lowest mean square
Poor principle is closest to Karhunen-Loeve properties, can reduce Gibbs boundary effects.So ground magnetic anomaly to obtaining
Normal Δ Th(x, y) carries out cosine transform and obtains its frequency-domain expression
ΔTC(u, v)=C [Δ Th(x,y)] (4)
Here C () represents cosine transform.Earth magnetism modulus gradient Δ T on three direction in spacesx、ΔTyWith Δ TzWith ground
Magnetic anomaly Δ ThBetween relation：
Wherein, C-1() represents cosine inverse transformation.According to earth magnetism modulus gradient Δ T on the three of acquisition direction in spacesx、Δ
TyWith Δ TzExpression 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, carrier underwater navigationThe earth magnetism modulus gradient measured value at placeTrue earth magnetism modulus gradientWith
Earth magnetism modulus gradient measured valueRelation is：
Wherein esIt 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 observationBetween difference, more new particle weights, obtain the estimation of system mode.Fig. 2 is inertia/earth magnetism modulus ladder
Spend the matched filtering flow chart of integrated navigation system.
Particle filter algorithm for estimating based on the optimization of particle dynamics mimicry physics estimates system mode specific
Step is as follows：
(1) initialize.
(2) predict.FromMiddle sampling new particle collection, calculates particle weights.
(3) particle distribution is optimized.Particle distribution is optimized using particle dynamics mimicry physics optimization process, obtains new grain
Subset, iteration optimization terminate.
(4) new particle weights are calculated, and are normalized.
(5) resampling.If number of effective particles is less than given threshold, resampling is carried out, is returned
(6) state estimation,
Wave filter constantly estimates inertial navigation site error, the output of correction system position, makes site error by renewal and recursion
Gradually 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, Fig. 3 inertia/earth magnetism modulus gradient to inertial navigation system
Integrated navigation system frame diagram.
Beneficial effect of the present invention is described as follows：
It is fixed that underwater the earth magnetism modulus gradient reference map and particle filter technology built using magnetic anomaly data navigate
Position, this method have good concealment and measurement accuracy, round-the-clock, autonomous, continuously ins error can be carried out high-precision
Compensation；The ins error bearing calibration based on earth magnetism modulus gradient and particle filter of the present invention, efficiently solves underwater earth magnetism
Figure is difficult to build, and earth magnetism gradient former and measurement equation can not be established, and matched filtering algorithm is in the available of big initial position error
The problems such as property, suitable for the high-precision error compensation of underwater hiding-machine inertial navigation system.
Claims (2)
- 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>&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 asIn formula, δ λ,For warp, latitude error；δVE、δVNFor 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：emFor 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 usedAAnd ΓBBetween be passive space, z=h, z=0, Δ T0(x, y) is ΓBOn earth magnetism Abnormal data, is known observed quantity, Δ Th(x, y) is ΓAOn magnetic anomaly, be amount to be asked；Continuation process is as follows：(2.1) by Δ T0The Fourier transformation S of (x, y)0(kx,ky) upright projection is to ΓAOn face, as ΓAMagnetic anomaly on face Wave spectrum initial value(2.2) Γ is worked asA、Γ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 used0(kx,ky) 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 planeh(x, y),To obtained magnetic anomaly Δ Th(x, y) carries out cosine transform and obtains its frequency-domain expressionΔTC(u, v)=C [Δ Th(x,y)]Here C () represents cosine transform；Earth magnetism modulus gradient Δ T on three direction in spacesx、ΔTyWith Δ TzWith ground magnetic anomaly Normal Δ ThBetween relation：<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&Delta;T</mi> <mi>x</mi> </msub> <mo>=</mo> <msup> <mi>C</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>&lsqb;</mo> <mrow> <mn>2</mn> <msub> <mi>&pi;iu&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>&rsqb;</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&Delta;T</mi> <mi>y</mi> </msub> <mo>=</mo> <msup> <mi>C</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>&lsqb;</mo> <mrow> <mn>2</mn> <msub> <mi>&pi;iv&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>&rsqb;</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&Delta;T</mi> <mi>z</mi> </msub> <mo>=</mo> <msup> <mi>C</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>&lsqb;</mo> <mrow> <mn>2</mn> <mi>&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>&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>&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 spacesx、ΔTyWith ΔTzExpression 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 esIt 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 returned5.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. 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.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201410416153.4A CN104215259B (en) | 2014-08-22 | 2014-08-22 | A kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201410416153.4A CN104215259B (en) | 2014-08-22 | 2014-08-22 | A kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN104215259A CN104215259A (en) | 2014-12-17 |
| CN104215259B true CN104215259B (en) | 2018-04-24 |
Family
ID=52096994
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201410416153.4A Active CN104215259B (en) | 2014-08-22 | 2014-08-22 | A kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN104215259B (en) |
Families Citing this family (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104807462B (en) * | 2015-04-30 | 2018-09-07 | 中测高科（北京）测绘工程技术有限责任公司 | Indoor earth-magnetism navigation benchmark drawing generating method and system |
| CN105425304B (en) * | 2015-11-03 | 2017-09-15 | 哈尔滨工业大学 | A kind of compensation method of Aircraft magnetic disturbance |
| CN105301666B (en) * | 2015-11-05 | 2017-09-26 | 哈尔滨工业大学 | A kind of self-adapting regulation method for the magnetic disturbance penalty coefficient that navigates |
| CN105424036B (en) * | 2015-11-09 | 2018-02-13 | 东南大学 | A kind of inexpensive underwater hiding-machine terrain aided inertia combined navigation localization method |
| CN106125026B (en) * | 2016-06-12 | 2019-02-26 | 哈尔滨工程大学 | A kind of three axis magnetometer total error parameter identification and bearing calibration independent of earth's magnetic field amount |
| CN106382931A (en) * | 2016-08-19 | 2017-02-08 | 北京羲和科技有限公司 | An indoor positioning method and a device therefor |
| CN106919785B (en) * | 2017-01-23 | 2019-07-16 | 哈尔滨工程大学 | It is a kind of that online compensation method in magnetic field is interfered based on the carrier of ground magnetic vector and particle filter |
| CN106917621B (en) * | 2017-01-25 | 2020-02-07 | 重庆大学 | Small-aperture single-gyroscope horizontal well rotation directional inclination measurement device and method |
| CN107291659B (en) * | 2017-05-16 | 2020-09-25 | 哈尔滨工程大学 | Recursive cosine transform method for extending plane modulus gradient field upwards in one step in plane geomagnetic abnormal field |
| CN107462247A (en) * | 2017-07-18 | 2017-12-12 | 深圳天珑无线科技有限公司 | A kind of indoor orientation method, device and computer-readable recording medium |
| CN107632964B (en) * | 2017-09-06 | 2021-06-11 | 哈尔滨工程大学 | Downward continuation recursive cosine transform method for plane geomagnetic abnormal field |
| CN108362281B (en) * | 2018-02-24 | 2020-11-24 | 中国人民解放军61540部队 | Long-baseline underwater submarine matching navigation method and system |
| CN109633490B (en) * | 2019-01-23 | 2021-04-02 | 中国科学院上海微***与信息技术研究所 | Calibration method of full-tensor magnetic gradient measurement assembly |
| CN109739263B (en) * | 2019-01-25 | 2020-06-30 | 清华大学 | Submarine detecting navigation method based on magnetic signal continuation algorithm for submarine detection |
| DE112020001559T5 (en) * | 2019-03-28 | 2022-01-13 | Sony Group Corporation | INFORMATION PROCESSING EQUIPMENT, PROGRAM AND INFORMATION PROCESSING METHOD |
| CN112566009B (en) * | 2019-09-26 | 2022-12-27 | 成都易书桥科技有限公司 | Participation type indoor positioning system based on geomagnetism |
| CN111076717B (en) * | 2019-12-31 | 2022-11-25 | 南京工程学院 | Geomagnetic-assisted inertial navigation simulation system and method based on global geomagnetic abnormal field |
| CN112684396B (en) * | 2020-11-20 | 2024-03-01 | 国网江苏省电力有限公司营销服务中心 | Data preprocessing method and system for electric energy meter operation error monitoring model |
| CN113503891B (en) * | 2021-04-22 | 2022-11-29 | 中国人民解放军海军工程大学 | SINSDVL alignment correction method, system, medium and equipment |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103344242B (en) * | 2013-07-02 | 2015-11-25 | 哈尔滨工业大学 | Based on the geomagnetic matching navigation method of absolute force and gradient |
| CN103630139B (en) * | 2013-12-17 | 2015-12-02 | 哈尔滨工程大学 | A kind of full attitude determination method of underwater carrier measured based on earth magnetism gradient tensor |
-
2014
- 2014-08-22 CN CN201410416153.4A patent/CN104215259B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN104215259A (en) | 2014-12-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104215259B (en) | A kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter | |
| CN102829785B (en) | Air vehicle full-parameter navigation method based on sequence image and reference image matching | |
| Wu et al. | Velocity/position integration formula part I: Application to in-flight coarse alignment | |
| CN105698822B (en) | Initial Alignment Method between autonomous type inertial navigation based on reversed Attitude Tracking is advanced | |
| CN103076017B (en) | Method for designing Mars entry phase autonomous navigation scheme based on observability degree analysis | |
| CN108759838A (en) | Mobile robot multiple sensor information amalgamation method based on order Kalman filter | |
| CN102353378B (en) | Adaptive federal filtering method of vector-form information distribution coefficients | |
| CN106289246A (en) | A kind of rods arm measure method based on position and orientation measurement system | |
| CN109931955B (en) | Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering | |
| CN103630137A (en) | Correction method used for attitude and course angles of navigation system | |
| CN104049269B (en) | A kind of target navigation mapping method based on laser ranging and MEMS/GPS integrated navigation system | |
| CN104697520B (en) | Integrated gyro free strap down inertial navigation system and gps system Combinated navigation method | |
| CN104132662A (en) | Closed-loop Kalman filter inertial positioning method based on zero velocity update | |
| CN104374405A (en) | MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering | |
| CN104062687A (en) | Air ground integrated geomagnetic field combined observation method and system | |
| CN102116634A (en) | Autonomous dimensionality reduction navigation method for deep sky object (DSO) landing detector | |
| Cai et al. | Improving airborne strapdown vector gravimetry using stabilized horizontal components | |
| CN103017787A (en) | Initial alignment method suitable for rocking base | |
| CN103398725A (en) | Star-sensor-based initial alignment method of strapdown inertial navigation system | |
| CN105988129A (en) | Scalar-estimation-algorithm-based INS/GNSS combined navigation method | |
| CN112577518A (en) | Inertial measurement unit calibration method and device | |
| Kwon et al. | Gravity requirements for compensation of ultra-precise inertial navigation | |
| CN103256932A (en) | Replacement and extrapolation combined navigation method | |
| Liu et al. | Geomagnetism aided inertial navigation system | |
| CN114909608A (en) | Trenchless pipeline positioning method based on MIMU/mile wheel/photoelectric speed measurement sensor combination |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |