CN106441309B - Mars approach section distributed freedom air navigation aid based on covariance mixing together - Google Patents
Mars approach section distributed freedom air navigation aid based on covariance mixing together Download PDFInfo
- Publication number
- CN106441309B CN106441309B CN201611000780.5A CN201611000780A CN106441309B CN 106441309 B CN106441309 B CN 106441309B CN 201611000780 A CN201611000780 A CN 201611000780A CN 106441309 B CN106441309 B CN 106441309B
- Authority
- CN
- China
- Prior art keywords
- mars
- approach section
- state
- navigation
- covariance
- 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
- 238000013459 approach Methods 0.000 title claims abstract description 55
- 238000005259 measurement Methods 0.000 claims abstract description 47
- 238000001914 filtration Methods 0.000 claims abstract description 30
- 230000004927 fusion Effects 0.000 claims abstract description 14
- 239000011159 matrix material Substances 0.000 claims description 42
- 239000000523 sample Substances 0.000 claims description 25
- 230000001133 acceleration Effects 0.000 claims description 22
- 238000000034 method Methods 0.000 claims description 13
- 230000005484 gravity Effects 0.000 claims description 7
- 230000002457 bidirectional effect Effects 0.000 claims description 3
- 230000003068 static effect Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 abstract description 7
- 238000004891 communication Methods 0.000 abstract description 6
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 208000021760 high fever Diseases 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 230000003313 weakening effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/24—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for cosmonautical navigation
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Astronomy & Astrophysics (AREA)
- Navigation (AREA)
Abstract
The Mars approach section distributed freedom air navigation aid based on covariance mixing together that the invention proposes a kind of, the steps include: the three-degrees-of-freedom dynamics model for establishing martian atmosphere approach section navigation system；Establish the measurement model of martian atmosphere approach section；Establish distributed fault-tolerance autonomous navigation system；Discrete and linearisation distributed fault-tolerance autonomous navigation system three-degrees-of-freedom dynamics model and measurement model；Establish the subfilter based on EKF Navigation algorithm；Establish the information fusion device based on covariance mixing together algorithm.The present invention carries out independent navigation by the way of the stronger distributed information fusion of fault-tolerant ability, be conducive to improve the utilization efficiency of radio communication, reduce the calculation amount of independent navigation filtering, improve the fault-tolerant ability of martian atmosphere approach section autonomous navigation system, the stability of Navigation is enhanced, effectively improves martian atmosphere approach section to the independent navigation precision of detector.
Description
Technical field
The present invention relates to martian atmosphere approach section technical fields combined of multi-sensor information, and in particular to one kind is based on association
The Mars approach section distributed freedom air navigation aid of variance mixing together, can be used for improving martian atmosphere approach section independent navigation
Precision and fault-tolerant ability.
Background technique
In martian atmosphere approach section, Mars probes are faced with the rugged environments such as peak value overload, high temperature high fever, detector
It is installed in shield, causes a large amount of navigation sensor that can not work, rely solely on Inertial Measurement Unit (being abbreviated as IMU)
Independent navigation is carried out, the precision of navigation is difficult to meet the requirement of the following mars exploration.It can be by very based on martian atmosphere approach section
The research that high frequency radio is communicated, scholars propose the integrated navigation scheme of IMU and radio distance-measuring, be detector into
The independent navigation for entering section provides additional information, improves the independent navigation precision of detector.But said combination navigation side
Method is the integrated navigation system of centralization filtering, is faced with heavy computational burden, the problems such as fault-tolerant ability is poor, and cannot be guaranteed filter
The real-time of wave device.Furthermore due to the very high frequency(VHF) radio communication meeting between detector and the in-orbit beacon of Mars or surface beacon
The case where in the presence of even dropout is weakened by the plasma sheath around detector, can make the fault-tolerant ability of centralization filtering into one
Step reduces.Therefore, for the integrated navigation of martian atmosphere approach section, select suitable Navigation method to navigation sensor into
Row information fusion, improves the flexibility of calculating, reduces calculation amount, and enhancing fault-tolerant ability is a problem to be solved.
It is filtered relative to centralization, each sub- shape is eliminated in distributed (or being decentralized) filtering using information sharing principle
The correlation of state estimation, has the characteristics that small calculation amount, flexible design, error resilience performance are good.Covariance mixing together algorithm is base
In the convex combination of local valuation and conservative error variance, the diverging of state estimation is efficiently avoided, there is preferable consistency,
It can guarantee that the information of filter state estimation merges precision with higher.The present invention uses the structure of Distributed filtering, each
Extended Kalman filter (being abbreviated as EKF) filtering algorithm is used in subfilter, then utilizes covariance mixing together algorithm pair
Each subfilter carries out information fusion and feedback, a kind of distributed fault-tolerance filtering method is constructed, for improving martian atmosphere
The independent navigation precision and fault-tolerant ability of approach section.
Summary of the invention
The non-linear independent navigation filtering algorithm that the present invention solves existing martian atmosphere approach section integrated navigation can not disappear
Except coloured noise influences navigation accuracy and the poor technical problem of fault-tolerant ability, the present invention proposes a kind of to melt based on covariance intersection
The Mars approach section distributed freedom air navigation aid of conjunction uses Extended Kalman filter filtering algorithm in each subfilter,
Information fusion and feedback are carried out to each subfilter using covariance mixing together algorithm, it is contemplated that in Navigation System Model
The problem of coloured noise problem and multi-sensor fault tolerance information merge, effectively improves martian atmosphere approach section to detector
Independent navigation precision, while also improving the fault-tolerant ability of independent navigation.
In order to solve the above-mentioned technical problem, the technical scheme is that a kind of Mars based on covariance mixing together
Approach section distributed freedom air navigation aid, which is characterized in that steps are as follows:
Step 1: it establishes the three-degrees-of-freedom dynamics model of martian atmosphere approach section navigation system: setting martian atmosphere entrance
The atmosphere of section is static with respect to Mars, and state vector is taken asr,v,γ,θ,λ,ψ
Respectively indicate height, speed, flight path angle, longitude, latitude and the course angle of detector, baFor in Inertial Measurement Unit IMU
The constant value drift of accelerometer；Establish the three-degrees-of-freedom dynamics model of martian atmosphere approach section:
Wherein, g (r)=μ/r2For Mars acceleration of gravity, μ is the gravity acceleration constant of Mars, and w (t) is zero-mean
White Gaussian noise, t indicate the time；
Step 2: it establishes the measurement model of martian atmosphere approach section: using distributed navigation filtering algorithm by IMU and detection
Device, as a subsystem, shares three sub- navigation system at a distance from a beacon, then corresponding martian atmosphere approach section
Three measurement equations are as follows:
Wherein, h1(·)、h2(·)、h3() respectively indicates the non-linear measurement function of three sub- navigation system, v1(t)、
v2(t) and v3(t) be respectively three sub- navigation system measurement noise, R1Indicate between Mars probes and surface beacon 1 away from
From R2Indicate the distance between Mars probes and surface beacon 2, R3Indicate the distance between Mars probes and in-orbit beacon,
avFor lower three axial accelerations of speed system of IMU output；ηaFor the measurement noise vector of IMU metrical information, ξR1、ξR2And ξR3Point
Not Wei bidirectional ranging measurement noise vector；ηa、ξR1、ξR2And ξR3For the white Gaussian noise of zero-mean；
Wherein, between Mars probes and the in-orbit beacon of Mars or martian surface beacon under Mars mass center inertial system away from
From:
Wherein, rlIndicate the coordinate of Mars probes, ri(i=1,2,3) martian surface beacon 1, martian surface are respectively indicated
The coordinate of the in-orbit beacon of beacon 2, Mars；
Step 3: distributed fault-tolerance autonomous navigation system: three measurement equation binding kinetics model foundation filters is established
Wave device carries out state estimation respectively, then in information fusion device using covariance mixing together algorithm to the state of detector into
Row information merges output state estimated value, and feeds back and carry out time update into navigation system；
Step 4: discrete and linearisation distributed fault-tolerance autonomous navigation system three-degrees-of-freedom dynamics model and measurement mould
Type: discretization is carried out respectively to three-degrees-of-freedom dynamics model and three measurement equations:
xk+1=F (xk)+wk,
zik=hi(xk)+vik, i=1,2,3；
Wherein, xk(k=1,2,3 ...) expression k moment shows state value, F (xk) it is non-linear after f (x (t), t) is discrete
Function of state, hi(xk) it is hiNon-linear measurement function after (x (t), t) is discrete, wkAnd vikIt is irrelevant, wkVariance matrix
For Qk, vikVariance matrix be Rik；
Linearize it is discrete after kinetic model: the nonlinear state function F (x after will be discretek) surround estimated valueIt presses
Taylor series expansion obtains corresponding linear dynamics equation to single order item:
xk+1=Φkxk+uk+wk
And:
Wherein, ΦkFor state-transition matrix,For estimated valuePosteriority state value；
Linearize it is discrete after measurement model: by three nonlinear discrete function hi(xk) surround predictive estimation valueBy Thailand
Series expansion is strangled to single order item, obtains linear measurement equation accordingly:
zik=Hikxk+yik+vik, (i=1,2,3)；
And
Wherein,For estimated valuePrior state value, yikFor measurement control item；
Step 5: it establishes the subfilter based on EKF Navigation algorithm: antithetical phrase navigation is realized using EKF filtering algorithm
The state estimation of the detector of system, obtain the height r of the detector of subfilter, speed v, flight path angle γ, longitude θ,
Latitude λ, course angle ψ, state estimationAnd varivance matrix
Step 6: it establishes the information fusion device based on covariance mixing together algorithm: being based on three state estimationsWith
And corresponding varivance matrixInformation is carried out using covariance mixing together algorithm to merge to obtain the state of+1 step of kth
EstimationWith estimation error variance matrix PCI,k+1, then will obtain againAnd PCI,k+1Pass to three subfilters
Its state and varivance matrix are updated.
The drag acceleration D and lift acceleration L of the detector are respectively indicated are as follows:
Wherein, CDFor resistance coefficient, CLFor lift coefficient, S is the area of reference of detector, and m is the quality of detector,For dynamic pressure, ρ is the atmospheric density of Mars approach section, corresponding exponential atmosphere density model are as follows:
ρ in formula0It is the nominal density of Mars approach section atmosphere, r0For the radial reference position of Mars, hsFor martian atmosphere
Calibration height.
The lower three axial acceleration a of speed system of the IMU outputvIt is expressed as；
av=[- D-DL/Dsin φ DL/Dcos φ]T。
In the step 5 the step of EKF filtering algorithm are as follows:
The initial value for choosing state estimation and varivance matrix is respectivelyAnd P0；
The state estimation walked by kthAnd varivance matrixIt is found that the predicted state of+1 step of kth is estimated
For
The prediction varivance matrix of+1 step of kthAre as follows:
Secondly, the filtering gain matrix K of+1 step of kthkFor
Finally, the state estimation of+1 step of kthFor
The estimation error variance matrix of+1 step of kthFor
Wherein, I indicates unit matrix.
In the step 6 the step of covariance mixing together algorithm are as follows:
Wherein, three weighting coefficient ω1,ω2,ω3> 0, meet ω1+ω2+ω3=1, and determined by following formula:
Wherein,||·||FIndicate Frobenius norm.
The method by Taylor series expansion to single order item is: for an infinite function f (x) that can be micro-, Tai Leji
Number is deployed into single order item are as follows:
F (x)=f (a)+f ' (a) (x-a),
Wherein, first derivative of f ' (a) the representative function f (x) at point x=a.
Compared with traditional centralized Navigation method, the present invention uses the stronger distributed information fusion of fault-tolerant ability
Mode carry out independent navigation, be conducive to improve radio communication utilization efficiency, reduce independent navigation filtering calculation amount,
The fault-tolerant ability for improving martian atmosphere approach section autonomous navigation system enhances the stability of Navigation, effectively improves
Independent navigation precision of the martian atmosphere approach section to detector.
Detailed description of the invention
In order to more clearly explain the embodiment of the invention or the technical proposal in the existing technology, to embodiment or will show below
There is attached drawing needed in technical description to be briefly described, it should be apparent that, the accompanying drawings in the following description is only this
Some embodiments of invention for those of ordinary skill in the art without creative efforts, can be with
It obtains other drawings based on these drawings.
Fig. 1 is the integrated navigation scheme of martian atmosphere approach section of the present invention.
Fig. 2 is flow chart of the invention.
Fig. 3 is the structure chart of distributed fault-tolerance of the present invention filtering.
Fig. 4 is the error comparison diagram of distributed fault-tolerance of the present invention filtering with centralization filtering.
Fig. 5 is the Error Graph of distribution Fault Tolerant Filtering when one beacon of the present invention breaks down.
Specific embodiment
Following will be combined with the drawings in the embodiments of the present invention, and technical solution in the embodiment of the present invention carries out clear, complete
Site preparation description, it is clear that described embodiments are only a part of the embodiments of the present invention, instead of all the embodiments.It is based on
Embodiment in the present invention, those of ordinary skill in the art are obtained every other under that premise of not paying creative labor
Embodiment shall fall within the protection scope of the present invention.
As shown in Figures 2 and 3, a kind of Mars approach section distributed freedom air navigation aid based on covariance mixing together,
It includes following six step:
Step 1: establishing the kinetic model of martian atmosphere approach section navigation system.
In martian atmosphere approach section, detector is seen as motorless particle.Ignore the rotation of Mars, while it is big to set Mars
The atmosphere of gas approach section is static with respect to Mars.State vector is taken asEstablish fire
The three-degrees-of-freedom dynamics model of star atmosphere approach section:
Wherein, t indicates the time, r, v, γ, θ, λ, ψ respectively indicate the height of detector, speed, flight path angle, longitude,
Latitude and course angle, baFor the constant value drift of accelerometer in Inertial Measurement Unit (being abbreviated as IMU), g (r)=μ/r2For Mars
Acceleration of gravity, μ are the gravity acceleration constant of Mars, and w (t) is the white Gaussian noise of zero-mean, and D and L are expressed as visiting
The drag acceleration and lift acceleration of device are surveyed, and:
In formula, CDFor resistance coefficient, CLFor lift coefficient, S is the area of reference of detector, and m is the quality of detector,For dynamic pressure, ρ is the atmospheric density of Mars approach section, corresponding exponential atmosphere density model are as follows:
In formula, ρ0It is the nominal density of Mars approach section atmosphere, r0For the radial reference position of Mars, hsFor martian atmosphere
Calibration height.
Wherein, the parameter value in step 1 is respectively as follows: acceleration of gravity μ=42828.29 × 10 of Mars9m3/s2, Mars
Radial reference position r0=3437.2km, the calibration height h of martian atmospheres=7.5km.
Wherein, lift resistance ratio L/D=C in verifying of the inventionL/CD, ballistic coefficient B=CDS/m and nominal atmospheric density ρ0's
Reference value and estimated value are as shown in table 1.
Parameter value and estimated value in the verifying of table 1
Step 2: establishing the measurement model of martian atmosphere approach section.
The present invention is with the IMU of the Mars probes acceleration provided and detector and the in-orbit beacon of Mars or Mars table
Measurement information of the distance between the face beacon information as Mars probes approach section independent navigation.In order to reduce calculation amount, increase
The error resilience performance of strong navigation system, using the conduct at a distance from a beacon by IMU and detector of distributed navigation filtering algorithm
One subsystem shares three sub- navigation system, then three measurement equations of corresponding martian atmosphere approach section are
In formula, h1(·)、h2(·)、h3() respectively indicates the non-linear measurement function of three sub- navigation system, v1(t)、
v2(t) and v3(t) be respectively three sub- navigation system measurement noise, R1、R2And R3It respectively indicates Mars probes and surface is believed
The distance between mark 1, surface beacon 2 and in-orbit beacon, ηaIt is the height of zero-mean for the measurement noise vector of IMU metrical information
This white noise, ξR1, ξR2And ξR3The respectively measurement noise vector of bidirectional ranging is the white Gaussian noise of zero-mean；avFor
Lower three axial accelerations of speed system of IMU output, and:
av=[- D-DL/Dsin φ DL/Dcos φ]T (8)。
It wherein, can by transmitting and receiving service between Mars probes and the in-orbit beacon of Mars or martian surface beacon
To obtain distance (transmitting and receiving service ranging schematic diagram referring to Fig. 1) of the two under Mars mass center inertial system:
In formula, rlIndicate the coordinate of Mars probes, ri(i=1,2,3) martian surface beacon 1, martian surface are respectively indicated
The coordinate of the in-orbit beacon of beacon 2, Mars.
Wherein, the in-orbit beacon of two Mars and a martian surface fixed beacon are selected in present invention verifying, it is corresponding to believe
Target initial position and speed difference are as shown in table 2.
The initial position and speed of the in-orbit beacon of table 2 and fixed beacon
Three axial acceleration constant value drift b of IMU are measured in present invention verifyingaIt is taken as [0.3cos (t/100)
0.3cos(t/200) 0.3sin(t/100)]Tm/s2。
Step 3: establishing distributed fault-tolerance autonomous navigation system.
There are the in-orbit beacon 1 of martian surface beacon, Mars and Mars in-orbit in the autonomous navigation scheme of martian atmosphere approach section
2 three beacons of beacon and Mars probes carry out radio communication, provide three range information z respectivelyi(i=1,2,3).In order to
The calculation amount for reducing Navigation, improves the fault-tolerance ability of autonomous navigation system, to three groups of measurement information binding kinetics moulds
Type establishes subfilter, and state estimation obtains state estimation respectivelyAnd varivance matrixSo
Information is carried out using state of the covariance mixing together algorithm to detector in information fusion device again afterwards and merges to obtain state estimation
ValueWith varivance matrix PCI, output state estimated valueVarivance matrix PCIAnd corresponding coefficient ωi(i=1,
2,3) feedback carries out time update into navigation system.Corresponding distributed fault-tolerance autonomous navigation system structure chart is referring to Fig. 3.
Step 4: discrete and linearisation distributed fault-tolerance autonomous navigation system three-degrees-of-freedom dynamics model and measurement mould
Type.
Discretization is carried out to above-mentioned kinetic model (1) and measurement model (5)~(7) respectively, is obtained:
xk+1=F (xk)+wk (10)
zik=hi(xk)+vik, i=1,2,3 (11),
In formula, xk(k=1,2,3 ...) expression k moment shows state value, F (xk) it is non-linear after f (x (t), t) is discrete
Function of state, hi(xk) (i=1,2,3) be hiNon-linear measurement function after (x (t), t) is discrete, wkAnd vik(i=1,2,3) mutually
It is uncorrelated, wkVariance matrix be Qk, vikVariance matrix be Rik, vik(i=1,2,3) correlation between is unknown.
Then, kinetic model and measurement model formula (10) after discretization and four equations of formula (11) are linearized.
Namely by the nonlinear state function F (x after discrete in formula (10)k) surround estimated valueBy Taylor series expansion to single order
, corresponding linear dynamics equation can be obtained:
xk+1=Φkxk+uk+wk (12)
And:
Wherein, ΦkFor state-transition matrix,For estimated valuePosteriority state value.
Then, by three nonlinear discrete function h in formula (11)i(xk) (i=1,2,3) surround predictive estimation valueIt presses
Taylor series expansion can obtain linear measurement equation accordingly to single order item:
zik=Hikxk+yik+vik, (i=1,2,3) (15)
And:
Wherein,For estimated valuePrior state value, yikIt is considered as measurement control item.Above-mentioned linearisation dynamics side
In journey and linearisation measurement equation, ukAnd yikNonrandom outer effect item can be regarded as.
Wherein, " by the F (x in formula (10) in step 4k) surround estimated valueBy Taylor series expansion to single order item " and
" by three nonlinear discrete function h in formula (11)i(xk) (i=1,2,3) surround predictive estimation valueBy Taylor series expansion
To single order item " in the method for Taylor series expansion to single order item be: for an infinite function f (x) that can be micro-, Taylor series
It is deployed into single order item are as follows:
F (x)=f (a)+f ' (a) (x-a) (18)
In formula, first derivative of f ' (a) the representative function f (x) at point x=a.
Step 5: establishing the subfilter based on EKF Navigation algorithm.
For the nonlinear navigation system model formula (10) and formula (11) after above-mentioned discretization, using warp in subfilter
The EKF filtering algorithm of allusion quotation realizes the state estimation to the detector of sub- navigation system.Used EKF filtering algorithm realizes step
Are as follows:
Firstly, the initial value for choosing state estimation and varivance matrix is respectivelyAnd P0。
Then, the state estimation walked by kthAnd varivance matrixIt is found that+1 step predicted state of kth is estimatedFor
The one-step prediction varivance matrix of+1 step of kthFor
Secondly, the filtering gain matrix K of+1 step of kthkFor
Finally, the state estimation of+1 step of kthFor
The estimation error variance matrix of+1 step of kthFor
Wherein, I indicates unit matrix.It can be existed to Mars probes by the circulation of above formula (18)~formula (22) totally 5 steps
Filter state in subfilter carries out real-time estimation, obtains the value according to a preliminary estimate of subfilter namely height, the speed of detector
Degree, flight path angle, longitude, latitude and course angle provide state estimation for the covariance mixing together of next stepAnd error
Variance matrix
Step 6: establishing the information fusion device based on covariance mixing together algorithm.
The value according to a preliminary estimate of Mars probes is obtained for subfilter in step 5, is based on three state estimationsWith
And corresponding varivance matrixInformation is carried out using covariance mixing together algorithm to merge to obtain the state of+1 step of kth
EstimationWith estimation error variance matrix PCI,k+1, then will obtain againAnd PCI,k+1Pass to three subfilters
Its state and varivance matrix are updated.Used covariance mixing together algorithm are as follows:
In formula, three coefficient ω1,ω2,ω3> 0, and meet ω1+ω2+ω3=1, and determined by following formula:
Wherein,||·||FIndicate that Frobenius norm, calculating formula are
For the present invention by above six steps, the distributed fault-tolerance for establishing IMU and detector and beacon distance respectively is autonomous
Then the measurement model of navigation carries out preliminary estimation using state of the EKF Navigation algorithm to Mars probes, then sharp
It is merged with state of the covariance mixing together algorithm to Mars probes, to reduce the calculation amount of Navigation, is improved
The fault-tolerant ability of martian atmosphere approach section autonomous navigation system, and enhanced navigation filters wave stability, reaches efficiently high-precision in real time
The purpose of degree estimation Mars probes navigational state.
Wherein, it needs to initialize the parameter of Mars probes in present invention verifying, the reference of corresponding detector
Value and estimated value are as shown in table 3.
The reference value and estimated value of 3 detector of table
Shown in the error comparison diagram 4 of distributed fault-tolerance filtering of the invention with centralization EKF filtering, a letter of the invention
Distribution Fault Tolerant Filtering error is as shown in Figure 5 when mark breaks down.From fig. 4, it can be seen that when three beacons all do not have in the event of failure,
The error of distributed fault-tolerance Navigation (being abbreviated as DCIEKF) method of the present invention and centralization EKF Navigation method almost phase
When.But when one of beacon is when 250s-300s breaks down, centralized EKF Navigation method has dissipated (figure
In do not represent), and distributed fault-tolerance Navigation method of the invention due to lack a navigation information, cause in 250s-
The biggish fluctuation of 300s filtering error appearance, but filter effect or acceptable on the whole.It can thus be seen that this hair
Bright distributed fault-tolerance Navigation method precision with higher and fault-tolerant ability.
Rugged environments, the detectors such as the detector face Lingao temperature high fever due to martian atmosphere approach section are wrapped in protection
In cover, lead to only only have IMU to work always.It can be communicated by very high frequency(VHF) radio based on martian atmosphere approach section IMU
Research, additional information is provided in the independent navigation of approach section for Mars probes, be conducive to improve detector it is autonomous
Navigation accuracy.But the very high frequency(VHF) radio communication between detector and the in-orbit beacon of Mars or martian surface beacon can also deposit
The case where weakening even dropout by the plasma sheath around detector, so being possible to using centralization navigation cannot be complete
Corresponding navigation information is obtained entirely, and wastes valuable computer memory space.Therefore, the present invention uses fault-tolerant ability
The mode of stronger distributed information fusion carries out independent navigation, is conducive to the utilization efficiency for improving radio communication, reaches and mention
The independent navigation precision of high Mars probes.Meanwhile the present invention is based on local valuation and guarantor using covariance mixing together algorithm
The convex combination for keeping error variance avoids the diverging of state estimation, has preferable consistency, ensure that martian atmosphere approach section
Independent navigation precision with higher and fault-tolerant ability.
The foregoing is merely illustrative of the preferred embodiments of the present invention, is not intended to limit the invention, all in essence of the invention
Within mind and principle, any modification, equivalent replacement, improvement and so on be should all be included in the protection scope of the present invention.
Claims (5)
1. a kind of Mars approach section distributed freedom air navigation aid based on covariance mixing together, which is characterized in that its step
It is as follows:
Step 1: it establishes the three-degrees-of-freedom dynamics model of martian atmosphere approach section navigation system: setting martian atmosphere approach section
Atmosphere is static with respect to Mars, and state vector is taken asR, v, γ, θ, λ, ψ difference
Indicate height, speed, flight path angle, longitude, latitude and the course angle of detector, baTo accelerate in Inertial Measurement Unit IMU
Spend the constant value drift of meter；Establish the three-degrees-of-freedom dynamics model of martian atmosphere approach section:
Wherein, g (r)=μ/r2For Mars acceleration of gravity, μ is the gravity acceleration constant of Mars, and D indicates the resistance of detector
Acceleration, L indicate that the lift acceleration of detector, w (t) are the white Gaussian noise of zero-mean, and t indicates the time；
Step 2: establish the measurement model of martian atmosphere approach section: using distributed navigation filtering algorithm by IMU and detector with
The distance of one beacon shares three sub- navigation system as a subsystem, then three of corresponding martian atmosphere approach section
Measurement equation are as follows:
Wherein, h1(·)、h2(·)、h3() respectively indicates non-linear measurement function, v1(t)、v2(t) and v3It (t) is respectively to measure
Survey noise, R1Indicate the distance between Mars probes and surface beacon 1, R2It indicates between Mars probes and surface beacon 2
Distance, R3Indicate the distance between Mars probes and in-orbit beacon, avFor lower three axial accelerations of speed system of IMU output；
ηaFor the measurement noise vector of IMU metrical information, ξR1、ξR2And ξR3The respectively measurement noise vector of bidirectional ranging；ηa、ξR1、ξR2
And ξR3For the white Gaussian noise of zero-mean；
Mars probes between the in-orbit beacon of Mars or martian surface beacon under Mars mass center inertial system at a distance from:
Wherein, rlIndicate the coordinate of Mars probes, riIt is in-orbit to respectively indicate martian surface beacon 1, martian surface beacon 2, Mars
The coordinate of beacon, i=1,2,3；
Step 3: distributed fault-tolerance autonomous navigation system: three measurement equation binding kinetics model foundation subfilters is established
State estimation is carried out respectively, and letter is then carried out to the state of detector using covariance mixing together algorithm in information fusion device
Breath fusion output state estimated value, and feed back and carry out time update into navigation system；
Step 4: discrete and linearisation distributed fault-tolerance autonomous navigation system three-degrees-of-freedom dynamics model and measurement model:
Discretization is carried out respectively to three-degrees-of-freedom dynamics model and three measurement equations:
xk+1=F (xk)+wk,
zik=hi(xk)+vik, i=1,2,3；
Wherein, xkIndicate the k moment shows state value, k=1,2,3 ..., F (xk) be f (x (t), t) it is discrete after nonlinear state
Function, hi(xk) it is hiNon-linear measurement function after (x (t), t) is discrete, wkAnd vikIt is irrelevant, wkVariance matrix be Qk,
vikVariance matrix be Rik；
Linearize it is discrete after kinetic model: the nonlinear state function F (x after will be discretek) surround posteriority state valueIt presses
Taylor series expansion obtains corresponding linear dynamics equation to single order item:
xk+1=Φkxk+uk+wk
And:
Wherein, ΦkFor state-transition matrix,For estimated valuePosteriority state value；
Linearize it is discrete after measurement model: by three nonlinear discrete function hi(xk) surround prior state valueBy Taylor's grade
Number is deployed into single order item, obtains linear measurement equation accordingly:
zik=Hikxk+yik+vik, i=1,2,3；
And
Wherein,For estimated valuePrior state value, yikIt is considered as measurement control item；
Step 5: it establishes the subfilter based on EKF Navigation algorithm: being realized using EKF filtering algorithm to sub- navigation system
Detector state estimation, obtain the height, speed, flight path angle, longitude, latitude, course of the detector of subfilter
Angle ,+1 step of kth prior state valueAnd varivance matrix
Step 6: it establishes the information fusion device based on covariance mixing together algorithm: being based on three state estimationsAnd phase
The varivance matrix answeredInformation is carried out using covariance mixing together algorithm to merge to obtain the state estimation of+1 step of kthWith estimation error variance matrixThen it will obtain againAnd PCI,k+1Three subfilters are passed to it
State and varivance matrix are updated；
In the step 6 the step of covariance mixing together algorithm are as follows:
Wherein, three weighting coefficient ω1,ω2,ω3> 0, meets ω1+ω2+ω3=1, and determined by following formula:
Wherein,||·||FIndicate Frobenius norm.
2. the Mars approach section distributed freedom air navigation aid according to claim 1 based on covariance mixing together,
It is characterized in that, the drag acceleration D and lift acceleration L of the detector are respectively indicated are as follows:
Wherein, CDFor resistance coefficient, CLFor lift coefficient, S is the area of reference of detector, and m is the quality of detector,
For dynamic pressure, ρ is the atmospheric density of Mars approach section, corresponding exponential atmosphere density model are as follows:
ρ in formula0It is the nominal density of Mars approach section atmosphere, r0For the radial reference position of Mars, hsFor the calibration of martian atmosphere
Highly.
3. the Mars approach section distributed freedom air navigation aid according to claim 1 based on covariance mixing together,
It is characterized in that, the lower three axial acceleration a of speed system of the IMU outputvIt is expressed as；
av=[- D-DL/Dsin φ DL/Dcos φ]T。
4. the Mars approach section distributed freedom air navigation aid according to claim 1 based on covariance mixing together,
It is characterized in that, in the step 5 the step of EKF filtering algorithm are as follows:
The initial value for choosing state estimation and varivance matrix is respectivelyAnd P0；
The estimated value walked by kthAnd varivance matrixIt is found that the prior state of+1 step of kth is estimatedFor
The prediction varivance matrix of+1 step of kthAre as follows:
Secondly, the filtering gain matrix K of+1 step of kthkFor
Finally, the state estimation of+1 step of kthFor
The estimation error variance matrix of+1 step of kthFor
Wherein, I indicates unit matrix.
5. the Mars approach section distributed freedom air navigation aid according to claim 1 based on covariance mixing together,
It is characterized in that, the method by Taylor series expansion to single order item is: for an infinite function f (x) that can be micro-, Tai Leji
Number is deployed into single order item are as follows:
F (x)=f (a)+f ' (a) (x-a),
Wherein, first derivative of f ' (a) the representative function f (x) at point x=a.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611000780.5A CN106441309B (en) | 2016-11-14 | 2016-11-14 | Mars approach section distributed freedom air navigation aid based on covariance mixing together |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611000780.5A CN106441309B (en) | 2016-11-14 | 2016-11-14 | Mars approach section distributed freedom air navigation aid based on covariance mixing together |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106441309A CN106441309A (en) | 2017-02-22 |
| CN106441309B true CN106441309B (en) | 2019-03-12 |
Family
ID=58207838
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201611000780.5A Active CN106441309B (en) | 2016-11-14 | 2016-11-14 | Mars approach section distributed freedom air navigation aid based on covariance mixing together |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106441309B (en) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107820260A (en) * | 2017-10-06 | 2018-03-20 | 廖忠民 | Libration point underlying network |
| CN109211233B (en) * | 2018-09-25 | 2020-10-23 | 常熟理工学院 | Elevator motion detection and abnormal position parking judgment method based on acceleration sensor |
| CN111089580B (en) * | 2018-10-23 | 2023-02-10 | 北京自动化控制设备研究所 | Unmanned war chariot simultaneous positioning and map construction method based on covariance intersection |
| CN113375634B (en) * | 2021-04-30 | 2022-10-14 | 北京临近空间飞行器***工程研究所 | Altitude measurement method based on atmospheric model and aircraft normal overload combination |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7580907B1 (en) * | 2004-01-14 | 2009-08-25 | Evolved Machines, Inc. | Invariant object recognition |
| CN103344245B (en) * | 2013-07-09 | 2015-11-18 | 北京航空航天大学 | The UD-SKF method of Mars approach section IMU and the navigation of very high frequency(VHF) radio combination |
| CN104266650B (en) * | 2014-10-25 | 2016-11-02 | 哈尔滨工业大学 | A kind of Mars landing device air approach section air navigation aid based on sampled point inheritance strategy |
| CN105300387B (en) * | 2015-11-03 | 2018-04-10 | 北京航空航天大学 | A kind of martian atmosphere approach section nonlinear and non-Gaussian order filtering method |
| CN105844058B (en) * | 2016-04-19 | 2019-02-05 | 沈阳化工大学 | A kind of distributed Extended Kalman filter method for estimating state of sewage disposal process |
-
2016
- 2016-11-14 CN CN201611000780.5A patent/CN106441309B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN106441309A (en) | 2017-02-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106441309B (en) | Mars approach section distributed freedom air navigation aid based on covariance mixing together | |
| Bevly | Global positioning system (GPS): A low-cost velocity sensor for correcting inertial sensor errors on ground vehicles | |
| CN106405670B (en) | A kind of gravity anomaly data processing method suitable for strapdown marine gravitometer | |
| CN110274588A (en) | Double-layer nested factor graph multi-source fusion air navigation aid based on unmanned plane cluster information | |
| CN109556632A (en) | INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering | |
| CN103744052B (en) | A kind of double star being applied to aerial target location surveys time-of-arrival direction finding method and device | |
| CN105066994A (en) | Data fusion method for flush air data system and inertial navigation system | |
| CN101246012B (en) | Combinated navigation method based on robust dissipation filtering | |
| CN108387227A (en) | The multinode information fusion method and system of airborne distribution POS | |
| CN106681336A (en) | Unmanned-aerial-vehicle height measurement control system based on fault-tolerance processing and control method | |
| CN109443342A (en) | NEW ADAPTIVE Kalman's UAV Attitude calculation method | |
| CN105300387B (en) | A kind of martian atmosphere approach section nonlinear and non-Gaussian order filtering method | |
| CN107246872B (en) | Single-particle filtering navigation device and method based on MEMS sensor and VLC positioning fusion | |
| CN104729510B (en) | A kind of extraterrestrial target determines method with respect to accompanying flying track | |
| Yang et al. | A stable SINS/UWB integrated positioning method of shearer based on the multi-model intelligent switching algorithm | |
| CN103344245B (en) | The UD-SKF method of Mars approach section IMU and the navigation of very high frequency(VHF) radio combination | |
| CN107576977A (en) | The UAV Navigation System and method adaptively merged based on multi-source information | |
| CN101702037B (en) | Method for obtaining surface layer meteorological element profile cross-sectional data based on remote control airship | |
| Trigubovich et al. | Complex technology of navigation and geodetic support of airborne electromagnetic surveys | |
| Madany et al. | Modelling and simulation of robust navigation for unmanned air systems (UASs) based on integration of multiple sensors fusion architecture | |
| CN113359167A (en) | Method for fusing and positioning GPS and laser radar through inertial measurement parameters | |
| Kaygısız et al. | Enhancing positioning accuracy of GPS/INS system during GPS outages utilizing artificial neural network | |
| CN106931966B (en) | A kind of Combinated navigation method based on the fitting of Taylor's high-order remainder | |
| Wilson et al. | Passive navigation using local magnetic field variations | |
| Wu et al. | New method of underwater passive navigation based on gravity gradient |
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 |