CN106441309A - Covariance intersection and fusion based Mars entry section distribution type autonomous navigation method - Google Patents

Abstract

The invention provides a covariance intersection and fusion based Mars entry section distribution type autonomous navigation method which comprises the following steps of establishing a three-degree-of-freedom kinetic model of a Mars atmospheric entry section navigation system; establishing a measurement model of a Mars atmospheric entry section; establishing a distribution type fault-tolerant autonomous navigation system; dispersing and linearizing a three-degree-of-freedom kinetic model and a measurement model of the distribution type fault-tolerant autonomous navigation system; establishing a subfilter based on an EKF (Extended Kalman Filter) navigation filtering algorithm; and establishing an information fusion device based on a covariance intersection and fusion algorithm. According to the covariance intersection and fusion based Mars entry section distribution type autonomous navigation method, autonomous navigation is carried out by adopting a mode of distribution type information fusion with stronger fault tolerance, so that improvement of utilization efficiency of wireless communication is facilitated, the calculated amount of autonomous navigation filtering is reduced, and the fault tolerance of the Mars atmospheric entry section autonomous navigation system, the navigation filtering stability and the autonomous navigation accurate of the Mars atmospheric entry section to a prober are improved.

Description

Mars approach section distributed freedom air navigation aid based on covariance mixing together

Technical field

The present invention relates to the technical field of martian atmosphere approach section multi-sensor information fusion, and in particular to a kind of based on association The Mars approach section distributed freedom air navigation aid of variance mixing together, can be used to improve martian atmosphere approach section independent navigation Precision and fault-tolerant ability.

Background technology

In martian atmosphere approach section, Mars probes are faced with the rugged environments such as peak value overload, high temperature hyperpyrexia, detector It is installed in protective cover, causes substantial amounts of navigation sensor work, relies 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 following mars exploration.Can be relied on very based on martian atmosphere approach section The research communicated by high frequency radio, scholars propose the integrated navigation scheme of IMU and radio distance-measuring, are that detector is entering The independent navigation for entering section provides extra information, improves the independent navigation precision of detector.But, combinations thereof navigation side Method is the integrated navigation system of centralized filtering, be faced with heavy computational burden, fault-tolerant ability difference the problems such as, and it cannot be guaranteed that filter The real-time of ripple device.Furthermore, due to the very high frequency(VHF) radio communication meeting between the in-orbit beacon of detector and Mars or surface beacon There is the situation that the plasma sheath around by detector weakens even dropout, the fault-tolerant ability of centralized filtering can be made to enter one Step reduces.Therefore, for the integrated navigation of martian atmosphere approach section, from suitable Navigation method, navigation sensor is entered Row information merges, and improves the motility for calculating, and reduces amount of calculation, and it is problem demanding prompt solution to strengthen fault-tolerant ability.

With respect to centralized filtering, distributed (or referred to as decentralized) filtering eliminates each sub- shape using information sharing principle State estimate dependency, with amount of calculation little, flexible design, fault freedom are good the features such as.Covariance mixing together algorithm is base In local valuation and the convex combination of conservative error variance, the diverging of state estimation is efficiently avoid, with preferable concordance, Can guarantee that the information fusion that filter state is estimated has higher precision.The present invention adopts the structure of Distributed filtering, at each EKF (being abbreviated as EKF) filtering algorithm is adopted in subfilter, then using covariance mixing together algorithm pair Each subfilter carries out information fusion and feedback, constructs a kind of distributed fault-tolerance filtering method, for improving martian atmosphere The independent navigation precision of approach section and fault-tolerant ability.

Content of the invention

The present invention solves the non-linear independent navigation filtering algorithm of existing martian atmosphere approach section integrated navigation and cannot disappear Except the technical problem that coloured noise impact navigation accuracy and fault-tolerant ability are poor, the present invention proposes a kind of covariance intersection that is based on and melts The Mars approach section distributed freedom air navigation aid of conjunction, adopts EKF filtering algorithm in each subfilter, Using covariance mixing together algorithm, information fusion and feedback are carried out to each subfilter, it is contemplated that in Navigation System Model Coloured noise problem and the problem of multi-sensor fault tolerance information fusion, are effectively improved martian atmosphere approach section to detector Independent navigation precision, while also improve the fault-tolerant ability of independent navigation.

In order to solve 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, it is characterised in that step is as follows：

Step one：Set up the three-degrees-of-freedom dynamics model of martian atmosphere approach section navigation system：If martian atmosphere is entered The atmosphere of section is static with respect to Mars, and state vector is taken asr,v,γ,θ,λ,ψ Represent height, speed, flight path angle, longitude, latitude and the course angle of detector, b respectively_aFor in Inertial Measurement Unit IMU The constant value drift of accelerometer；Set up the three-degrees-of-freedom dynamics model of martian atmosphere approach section：

Wherein, g (r)=μ/r²For Mars acceleration of gravity, μ is the gravity acceleration constant of Mars, and w (t) is zero-mean White Gaussian noise, t express time；

Step 2：Set up the measurement model of martian atmosphere approach section：Using distributed navigation filtering algorithm by IMU and detection The distance of device and a beacon has three sub- navigation system as a subsystem, then corresponding martian atmosphere approach section Three measurement equations are：

Wherein, h₁(·)、h₂(·)、h₃() represents the non-linear measurement function of three sub- navigation system, v respectively₁(t)、 v₂(t) and v₃T () is respectively the measurement noise of three sub- navigation system, R₁Represent between Mars probes and surface beacon 1 away from From R₂Represent the distance between Mars probes and surface beacon 2, R₃Represent the distance between Mars probes and in-orbit beacon, a^vLower three axial accelerations of speed system for IMU output；η_aFor the measurement noise vector of IMU metrical information, ξ_R1、ξ_R2And ξ_R3Point Not Wei bidirectional ranging measurement noise vector；η_a、ξ_R1、ξ_R2And ξ_R3White Gaussian noise for zero-mean；

Wherein, between Mars probes and the in-orbit beacon of Mars or martian surface beacon under Mars barycenter inertial system away from From：

Wherein, r_lRepresent the coordinate of Mars probes, r_i(i=1,2,3) represent martian surface beacon 1, martian surface respectively The coordinate of the in-orbit beacon of beacon 2, Mars；

Step 3：Set up distributed fault-tolerance autonomous navigation system：Son filter set up by three measurement equation binding kineticses models Ripple device carries out state estimation respectively, then using covariance mixing together algorithm, the state of detector is entered in information fusion device Row information merges output state estimated value, and feeds back in navigation system and carry out time renewal；

Step 4：The three-degrees-of-freedom dynamics model of discrete and linearisation distributed fault-tolerance autonomous navigation system and measurement mould Type：Discretization is carried out respectively to three-degrees-of-freedom dynamics model and three measurement equations：

x_k+1=F (x_k)+w_k,

z_ik=h_i(x_k)+v_ik, i=1,2,3；

Wherein, x_k(k=1,2,3 ...) represent k moment show state value, F (x_k) for f (x (t), t) discrete after non-linear Function of state, h_i(x_k) it is h_i(x (t), t) discrete after non-linear measurement function, w_kAnd v_ikOrthogonal, w_kVariance matrix For Q_k, v_ikVariance matrix be R_ik；

Kinetic model after linearisation is discrete：Nonlinear state function F (x after will be discrete_k) surround estimated valuePress Taylor series expansion obtains corresponding linear kineticss equation to single order item：

x_k+1=Φ_kx_k+u_k+w_k

And：

Wherein, Φ_kFor state-transition matrix,For estimated valuePosteriority state value；

Measurement model after linearisation is discrete：By three nonlinear discrete function h_i(x_k) surround predicted estimate valueBy Thailand Series expansion is strangled to single order item, obtain linear measurement equation accordingly：

z_ik=H_ikx_k+y_ik+v_ik, (i=1,2,3)；

And

Wherein,For estimated valuePrior state value, y_ikFor measurement control item；

Step 5：Set up 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：Set up the information fusion device based on covariance mixing together algorithm：Based on three state estimationWith And corresponding varivance matrixThe state that information fusion obtains+1 step of kth is carried out using covariance mixing together algorithm EstimateWith estimation error variance matrix P_CI,k+1, then will obtain againAnd P_CI,k+1Pass to three subfilters Its state and varivance matrix are updated.

The drag acceleration D of the detector and lift acceleration L are expressed as：

Wherein, C_DFor resistance coefficient, C_LFor 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, and corresponding exponential atmosphere density model is：

ρ in formula₀It is the nominal density of Mars approach section air, r₀For the radial reference position of Mars, h_sFor martian atmosphere Calibration height.

Lower three axial accelerations a of the speed system of IMU output^vIt is expressed as；

a^v=[- D-D L/D sin φ D L/D cos φ]^T.

In the step 5 the step of EKF filtering algorithm it is：

The initial value for choosing state estimation and varivance matrix is respectivelyAnd P₀；

The state estimation for being walked by kthAnd varivance matrixUnderstand, the predicted state of+1 step of kth is estimated For

The forecast error variance matrix of+1 step of kthFor：

Secondly, filtering gain matrix K of+1 step of kth_kFor

Finally, the state estimation of+1 step of kthFor

The estimation error variance matrix of+1 step of kthFor

Wherein, I represents unit matrix.

In the step 6 the step of covariance mixing together algorithm it is：

Wherein, three weight coefficient ω₁,ω₂,ω₃>0, meet ω₁+ω₂+ω₃=1, and determined by following formula：

Wherein,||·||_FRepresent Frobenius norm.

Described by Taylor series expansion to the method for single order item it is：For infinite function f (x) that can be micro-, Tai Leji Number is deployed into single order item and is：

F (x)=f (a)+f ' (a) (x-a),

Wherein, first derivative of the f ' (a) representative function f (x) at point x=a.

Compared with traditional centralized Navigation method, the present invention is using the stronger distributed information fusion of fault-tolerant ability Mode carry out independent navigation, be conducive to improve radio communication utilization ratio, reduce independent navigation filtering amount of calculation, The fault-tolerant ability of martian atmosphere approach section autonomous navigation system is improve, the stability of Navigation is enhanced, is effectively improved Independent navigation precision of the martian atmosphere approach section to detector.

Description of the drawings

In order to be illustrated more clearly that the embodiment of the present invention or technical scheme of the prior art, below will be to embodiment or existing Accompanying drawing to be used needed for technology description is had to be briefly described, it should be apparent that, drawings in the following description are only this Some embodiments of invention, for those of ordinary skill in the art, on the premise of not paying creative work, acceptable Other accompanying drawings are obtained according to these accompanying drawings.

Fig. 1 is the integrated navigation scheme of martian atmosphere approach section of the present invention.

Fig. 2 is the flow chart of the present invention.

The structure chart that Fig. 3 is filtered for distributed fault-tolerance of the present invention.

Fig. 4 filters the error comparison diagram with centralized filtering for distributed fault-tolerance of the present invention.

The Error Graph of distributed Fault Tolerant Filtering when Fig. 5 is broken down for one beacon of the present invention.

Specific embodiment

Below in conjunction with the accompanying drawing in the embodiment of the present invention, the technical scheme in the embodiment of the present invention is carried out clear, complete Site preparation is described, it is clear that described embodiment is only a part of embodiment of the present invention, rather than whole embodiments.It is based on Embodiment in the present invention, it is every other that those of ordinary skill in the art are obtained under the premise of creative work is not paid Embodiment, belongs to the scope of protection of the 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 one, set up 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 asSet up fire The three-degrees-of-freedom dynamics model of star air approach section：

Wherein, t express time, r, v, γ, θ, λ, ψ represent respectively the height of detector, speed, flight path angle, longitude, Latitude and course angle, b_aFor the constant value drift of accelerometer in Inertial Measurement Unit (being abbreviated as IMU), g (r)=μ/r²For Mars Acceleration of gravity, μ is the gravity acceleration constant of Mars, and w (t) is the white Gaussian noise of zero-mean, and D and L is expressed as visiting Drag acceleration and the lift acceleration of device is surveyed, and：

In formula, C_DFor resistance coefficient, C_LFor 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, and corresponding exponential atmosphere density model is：

In formula, ρ₀It is the nominal density of Mars approach section air, r₀For the radial reference position of Mars, h_sFor martian atmosphere Calibration height.

Wherein, the parameter value in step one is respectively：Acceleration of gravity μ=42828.29 × 10 of Mars⁹m³/s², Mars Radial reference position r₀=3437.2km, the calibration height h of martian atmosphere_s=7.5km.

Wherein, lift-drag ratio L/D=C in the checking of the present invention_L/C_D, ballistic coefficient B=C_DS/m and nominal atmospheric density ρ₀'s Reference value and estimated value are as shown in table 1.

Parameter value and estimated value in the checking of table 1

Step 2, set up the measurement model of martian atmosphere approach section.

Acceleration and detector and the in-orbit beacon of Mars or Mars table that the present invention is provided with the IMU of Mars probes The distance between face beacon information is used as the measurement information of Mars probes approach section independent navigation.In order to reduce amount of calculation, increase The fault freedom of strong navigation system, using distributed navigation filtering algorithm using the distance of IMU and detector and a beacon as One subsystem, has three sub- navigation system, then three measurement equations of corresponding martian atmosphere approach section are

In formula, h₁(·)、h₂(·)、h₃() represents the non-linear measurement function of three sub- navigation system, v respectively₁(t)、 v₂(t) and v₃T () is respectively the measurement noise of three sub- navigation system, R₁、R₂And R₃Represent that Mars probes are believed with surface respectively The distance between mark 1, surface beacon 2 and in-orbit beacon, η_aFor the measurement noise vector of IMU metrical information, it is the height of zero-mean This white noise, ξ_R1, ξ_R2And ξ_R3The measurement noise of respectively bidirectional ranging is vectorial, is the white Gaussian noise of zero-mean；a^vFor Lower three axial accelerations of speed system of IMU output, and：

a^v=[- D-D L/D sin φ D L/D cos φ]^T(8).

Wherein, pass through transmitting and receiving service between Mars probes and the in-orbit beacon of Mars or martian surface beacon, can To obtain the two distance under Mars barycenter inertial system (transmitting and receiving service range finding schematic diagram is referring to Fig. 1)：

In formula, r_lRepresent the coordinate of Mars probes, r_i(i=1,2,3) represent martian surface beacon 1, martian surface respectively The coordinate of the in-orbit beacon of beacon 2, Mars.

Wherein, in present invention checking, two in-orbit beacons of Mars and a martian surface fixed beacon are selected, corresponding letter Target initial position and speed are as shown in table 2 respectively.

The in-orbit beacon of table 2 and initial position and the speed of fixed beacon

Three axial acceleration constant value drift b of IMU are measured in present invention checking_aIt is taken as [0.3cos (t/100) 0.3cos(t/200) 0.3sin(t/100)]^Tm/s2.

Step 3, set up 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 carry out radio communication with Mars probes, provide three range information z respectively_i(i=1,2,3).In order to Reduce the amount of calculation of Navigation, the fault-tolerance ability of autonomous navigation system is improved, to three groups of measurement information binding kineticses moulds Subfilter set up by type, and state estimation obtains state estimation respectivelyAnd varivance matrixSo In information fusion device, information fusion is carried out to the state of detector using covariance mixing together algorithm again afterwards and obtain state estimation ValueWith varivance matrix P_CI, output state estimated valueVarivance matrix P_CIAnd corresponding coefficient ω_i(i=1, 2,3) feeding back in navigation system carries out time renewal.Corresponding distributed fault-tolerance autonomous navigation system structure chart is referring to Fig. 3.

Step 4, the three-degrees-of-freedom dynamics model of discrete and linearisation distributed fault-tolerance autonomous navigation system and measurement mould Type.

Respectively discretization is carried out to above-mentioned kinetic model (1) and measurement model (5)～(7), is obtained：

x_k+1=F (x_k)+w_k(10)

z_ik=h_i(x_k)+v_ik, i=1,2,3 (11),

In formula, x_k(k=1,2,3 ...) represent k moment show state value, F (x_k) for f (x (t), t) discrete after non-linear Function of state, h_i(x_k) (i=1,2,3) be h_i(x (t), t) discrete after non-linear measurement function, w_kAnd v_ik(i=1,2,3) mutual Uncorrelated, w_kVariance matrix be Q_k, v_ikVariance matrix be R_ik, v_ik(i=1,2,3) dependency between is unknown.

Then, by four equation linearisations of the kinetic model after discretization and measurement model formula (10) and formula (11). 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 kineticss equation can be obtained：

x_k+1=Φ_kx_k+u_k+w_k(12)

And：

Wherein, Φ_kFor state-transition matrix,For estimated valuePosteriority state value.

Then, by three nonlinear discrete function h in formula (11)_i(x_k) (i=1,2,3) surround predicted estimate valuePress Taylor series expansion can obtain linear measurement equation accordingly to single order item：

z_ik=H_ikx_k+y_ik+v_ik, (i=1,2,3) (15)

And：

Wherein,For estimated valuePrior state value, y_ikIt is considered as measurement control item.Above-mentioned linearisation kinetics side In journey and linearisation measurement equation, u_kAnd y_ikNonrandom outer effect item can be regarded as.

Wherein, " by the F (x in formula (10) in step 4_k) surround estimated valueBy Taylor series expansion to single order item " and " by three nonlinear discrete function h in formula (11)_i(x_k) (i=1,2,3) surround predicted estimate valueBy Taylor series expansion To single order item " in Taylor series expansion to the method for single order item be：For infinite function f (x) that can be micro-, Taylor series Being deployed into single order item is：

F (x)=f (a)+f ' (a) (x-a) (18)

In formula, first derivative of the f ' (a) representative function f (x) at point x=a.

The subfilter of step 5, foundation based on EKF Navigation algorithm.

For nonlinear navigation system model formula (10) after above-mentioned discretization and formula (11), warp is adopted in subfilter The EKF filtering algorithm of allusion quotation realizes the state estimation of the detector of antithetical phrase navigation system.The EKF filtering algorithm for being adopted realizes step For：

First, choose state estimation and the initial value of varivance matrix is respectivelyAnd P₀.

Then, the state estimation for being walked by kthAnd varivance matrixUnderstand ,+1 step predicted state of kth is estimatedFor

The one-step prediction varivance matrix of+1 step of kthFor

Secondly, filtering gain matrix K of+1 step of kth_kFor

Finally, the state estimation of+1 step of kthFor

The estimation error variance matrix of+1 step of kthFor

Wherein, I represents unit matrix.Mars probes can be existed 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, are that the covariance mixing together of next step provides state estimationAnd error Variance matrix

The information fusion device of step 6, foundation based on covariance mixing together algorithm.

The value according to a preliminary estimate of Mars probes is obtained for subfilter in step 5, based on three state estimationWith And corresponding varivance matrixThe state that information fusion obtains+1 step of kth is carried out using covariance mixing together algorithm EstimateWith estimation error variance matrix P_CI,k+1, then will obtain againAnd P_CI,k+1Pass to three subfilters Its state and varivance matrix are updated.The covariance mixing together algorithm for being adopted is：

In formula, three coefficient ω₁,ω₂,ω₃>0, and meet ω₁+ω₂+ω₃=1, and determined by following formula：

Wherein,||·||_FRepresent Frobenius norm, calculating formula is

The present invention sets up IMU and detector autonomous with the distributed fault-tolerance of beacon distance respectively by above six steps The measurement model of navigation, then carries out preliminary estimation, Ran Houli using EKF Navigation algorithm to the state of Mars probes With covariance mixing together algorithm, the state of Mars probes is merged, so as to reduce the amount of calculation of Navigation, improve The fault-tolerant ability of martian atmosphere approach section autonomous navigation system, and enhanced navigation filter wave stability, reach efficiently high-precision in real time Degree estimates the purpose of Mars probes navigational state.

Wherein, need to initialize the parameter of Mars probes in present invention checking, 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

The distributed fault-tolerance filtering of the present invention is contrasted shown in Fig. 4 with the error of centralized EKF filtering, a letter of the present invention When mark breaks down, distributed Fault Tolerant Filtering error is as shown in Figure 5.From fig. 4, it can be seen that when three beacons are all without fault, The error of distributed fault-tolerance Navigation (being abbreviated as DCIEKF) of the present invention method and centralized EKF Navigation method almost phase When.But, when one of beacon is broken down in 250s-300s, centralized EKF Navigation method has dissipated (figure In do not represent), and the distributed fault-tolerance Navigation method of the present invention due to lack a navigation information, cause in 250s- The larger fluctuation of 300s filtering error appearance, but filter effect or acceptable on the whole.It can thus be seen that this Bright distributed fault-tolerance Navigation method has higher precision and fault-tolerant ability.

The rugged environments such as the detector face Lingao temperature hyperpyrexia due to martian atmosphere approach section, detector is wrapped in protection In cover, cause only only have IMU to work always.Very high frequency(VHF) radio can be relied on to be communicated based on martian atmosphere approach section IMU Research, being Mars probes provides extra information in the independent navigation of approach section, is conducive to improving the autonomous of detector 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 be deposited Plasma sheath around by detector weakens the situation of even dropout, so be possible to using centralized navigation can not be complete Corresponding navigation information is obtained entirely, and wastes the Computer Storage space of preciousness.Therefore, the present invention adopts fault-tolerant ability The mode of stronger distributed information fusion carries out independent navigation, is conducive to improving the utilization ratio of radio communication, reaches and carry The independent navigation precision of high Mars probes.Meanwhile, the present invention is using covariance mixing together algorithm based on local valuation and guarantor Keep the convex combination of error variance, it is to avoid the diverging of state estimation, with preferable concordance, it is ensured that martian atmosphere approach section Independent navigation there is higher precision and fault-tolerant ability.

Presently preferred embodiments of the present invention is the foregoing is only, not in order to limit the present invention, all essences in the present invention Within god and principle, any modification, equivalent substitution and improvement that is made etc., should be included within the scope of the present invention.

Claims (6)

1. a kind of Mars approach section distributed freedom air navigation aid based on covariance mixing together, it is characterised in that step is such as Under：

Step one：Set up the three-degrees-of-freedom dynamics model of martian atmosphere approach section navigation system：If martian atmosphere approach section Atmosphere is static with respect to Mars, and state vector is taken asR, v, γ, θ, λ, ψ are respectively Represent height, speed, flight path angle, longitude, latitude and the course angle of detector, b_aFor accelerating in Inertial Measurement Unit IMU The constant value drift of degree meter；Set up the three-degrees-of-freedom dynamics model of martian atmosphere approach section：

$\stackrel{\·}{x}\left(t\right)=f\left(x\right(t),t)+w\left(t\right)=\left(\begin{array}{c}v\sin \mathrm{\γ}\\ -D-g\left(r\right)sin\mathrm{\γ}\\ (\frac{v}{r}-\frac{g\left(r\right)}{v})\cos \mathrm{\γ}+\frac{L}{v}cos\mathrm{\φ}\\ \frac{v\cos \mathrm{\γ}sin\mathrm{\ψ}}{r\cos \mathrm{\λ}}\\ \frac{v\cos \mathrm{\γ}\cos \mathrm{\ψ}}{r}\\ \frac{v}{r}\sin \mathrm{\ψ}\cos \mathrm{\γ}tan\mathrm{\λ}+\frac{L}{v\cos \mathrm{\γ}}sin\mathrm{\φ}\\ 0\\ 0\\ 0\end{array}\right)+w\left(t\right),$

Wherein, g (r)=μ/r²For Mars acceleration of gravity, μ is the gravity acceleration constant of Mars, and w (t) is the Gauss of zero-mean White noise, t express time；

Step 2：Set up the measurement model of martian atmosphere approach section：Using distributed navigation filtering algorithm by IMU and detector with The distance of one beacon has three sub- navigation system as a subsystem, then three of corresponding martian atmosphere approach section Measurement equation is：

$z_1=h_1\left(x\right(t),t)+v_1\left(t\right)=\left[\begin{array}{c}{a}^v+b_a\\ R_1\end{array}\right]+\left[\begin{array}{c}{\mathrm{\η}}_a\\ {\mathrm{\ξ}}_{R1}\end{array}\right],$

$z_2=h_2\left(x\right(t),t)+v_2\left(t\right)=\left[\begin{array}{c}{a}^v+b_a\\ R_2\end{array}\right]+\left[\begin{array}{c}{\mathrm{\η}}_a\\ {\mathrm{\ξ}}_{R2}\end{array}\right],$

$z_3=h_3\left(x\right(t),t)+v_3\left(t\right)=\left[\begin{array}{c}{a}^v+b_a\\ R_3\end{array}\right]+\left[\begin{array}{c}{\mathrm{\η}}_a\\ {\mathrm{\ξ}}_{R3}\end{array}\right];$

Wherein, h₁(·)、h₂(·)、h₃() represents the non-linear measurement function of three sub- navigation system, v respectively₁(t)、v₂(t) And v₃T () is respectively the measurement noise of three sub- navigation system, R₁Represent the distance between Mars probes and surface beacon 1, R₂ Represent the distance between Mars probes and surface beacon 2, R₃Represent the distance between Mars probes and in-orbit beacon, a^vFor Lower three axial accelerations of speed system of IMU output；η_aFor the measurement noise vector of IMU metrical information, ξ_R1、ξ_R2And ξ_R3Respectively Measurement noise vector for bidirectional ranging；η_a、ξ_R1、ξ_R2And ξ_R3White Gaussian noise for zero-mean；

Wherein, the distance between Mars probes and the in-orbit beacon of Mars or martian surface beacon under Mars barycenter inertial system：

$R_i=\sqrt{{(r_l-r_i)}^T(r_l-r_i)};$

Wherein, r_lRepresent the coordinate of Mars probes, r_i(i=1,2,3) represent martian surface beacon 1, martian surface beacon respectively 2nd, the coordinate of the in-orbit beacon of Mars；

Step 3：Set up distributed fault-tolerance autonomous navigation system：Subfilter set up by three measurement equation binding kineticses models Carry out state estimation respectively, then in information fusion device, letter is carried out to the state of detector using covariance mixing together algorithm Breath fusion output state estimated value, and feed back in navigation system and carry out time renewal；

Step 4：The three-degrees-of-freedom dynamics model of discrete and linearisation distributed fault-tolerance autonomous navigation system and measurement model： Discretization is carried out respectively to three-degrees-of-freedom dynamics model and three measurement equations：

x_k+1=F (x_k)+w_k,

z_ik=h_i(x_k)+v_ik, i=1,2,3；

Wherein, x_k(k=1,2,3 ...) represent k moment show state value, F (x_k) for f (x (t), t) discrete after nonlinear state Function, h_i(x_k) it is h_i(x (t), t) discrete after non-linear measurement function, w_kAnd v_ikOrthogonal, w_kVariance matrix be Q_k, v_ikVariance matrix be R_ik；

Kinetic model after linearisation is discrete：Nonlinear state function F (x after will be discrete_k) surround estimated valueBy Taylor's level Number is deployed into single order item, obtains corresponding linear kineticss equation：

x_k+1=Φ_kx_k+u_k+w_k

And：

${\mathrm{\Φ}}_k=\frac{\∂F\left(x_k\right)}{\∂x_k}{|}_{x_k={\hat{x}}_k^{+}}$

$u_k=F\left(x_k\right)-{\mathrm{\Φ}}_k{\hat{x}}_k^{+};$

Wherein, Φ_kFor state-transition matrix,For estimated valuePosteriority state value；

Measurement model after linearisation is discrete：By three nonlinear discrete function h_i(x_k) surround predicted estimate valueBy Taylor's level Number is deployed into single order item, obtains linear measurement equation accordingly：

z_ik=H_ikx_k+y_ik+v_ik, (i=1,2,3)；

And

$H_{ik}=\frac{\∂h_i\left(x_k\right)}{\∂x_k}{|}_{x_k={\hat{x}}_k^{-}}$

$y_{ik}=h_i\left({\hat{x}}_k^{-}\right)-H_{ik}{\hat{x}}_k^{-},(i=1,2,3);$

Wherein,For estimated valuePrior state value, y_ikFor measurement control item；

Step 5：Set up the subfilter based on EKF Navigation algorithm：Antithetical phrase navigation system is realized using EKF filtering algorithm Detector state estimation, obtain the height r of the detector of subfilter, speed v, flight path angle γ, longitude θ, latitude λ, course angle ψ, state estimationAnd varivance matrix

Step 6：Set up the information fusion device based on covariance mixing together algorithm：Based on three state estimationAnd it is corresponding Varivance matrixThe state estimation that information fusion obtains+1 step of kth is carried out using covariance mixing together algorithmWith estimation error variance matrix P_CI,k+1, then will obtain againAnd P_CI,k+1Three subfilters are passed to which State and varivance matrix are updated.

2. the martian atmosphere approach section distributed fault-tolerance independent navigation based on covariance mixing together according to claim 1 Method, it is characterised in that the drag acceleration D of the detector and lift acceleration L are expressed as：

$D=\frac{C_{D}S}{m}\stackrel{\‾}{q},$

$L=\frac{C_{L}S}{m}\stackrel{\‾}{q}$

Wherein, C_DFor resistance coefficient, C_LFor 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, and corresponding exponential atmosphere density model is：

$\mathrm{\ρ}={\mathrm{\ρ}}_0{e}^{\frac{r_0-r}{h_s}}$

ρ in formula₀It is the nominal density of Mars approach section air, r₀For the radial reference position of Mars, h_sCalibration for martian atmosphere Highly.

3. the martian atmosphere approach section distributed fault-tolerance independent navigation based on covariance mixing together according to claim 1 Method, it is characterised in that lower three axial accelerations a of the speed system of IMU output^vIt is expressed as；

a^v=[- D-D L/D sin φ D L/D cos φ]^T.

4. the martian atmosphere approach section distributed fault-tolerance independent navigation based on covariance mixing together according to claim 1 Method, it is characterised in that in the step 5 the step of EKF filtering algorithm be：

The initial value for choosing state estimation and varivance matrix is respectivelyAnd P₀；

The state estimation for being walked by kthAnd varivance matrixUnderstand, the predicted state of+1 step of kth is estimatedFor

${\hat{x}}_{k+1}^{-}=f\left({\hat{x}}_k^{+}\right);$

The forecast error variance matrix of+1 step of kthFor：

$P_{k+1}^{-}={\mathrm{\Φ}}_k{P}_k^{+}{\mathrm{\Φ}}_k^T+Q_k;$

Secondly, filtering gain matrix K of+1 step of kth_kFor

$K_k=P_{k+1}^{-}{H}_k^T{(H_k{P}_{k+1}^{-}{H}_k^T+R_k)}^{-1};$

Finally, the state estimation of+1 step of kthFor

${\hat{x}}_{k+1}^{+}={\hat{x}}_{k+1}^{-}+K_k\[z_k-h\left({\hat{x}}_{k+1}^{-}\right)\]$

The estimation error variance matrix of+1 step of kthFor

$P_{k+1}^{+}=(I-K_k{H}_k)P_{k+1}^{-},$

Wherein, I represents unit matrix.

5. the Mars approach section distributed freedom air navigation aid based on covariance mixing together according to claim 1, its It is characterised by, in the step 6 the step of covariance mixing together algorithm is：

$P_{CI,k+1}={\[{\mathrm{\ω}}_1{\left(P_{1,k+1}^{+}\right)}^{-1}+{\mathrm{\ω}}_2{\left(P_{2,k+1}^{+}\right)}^{-1}+{\mathrm{\ω}}_3{\left(P_{3,k+1}^{+}\right)}^{-1}\]}^{-1}$

${\hat{x}}_{CI,k+1}=P_{CI,k+1}{\[{\mathrm{\ω}}_1{\left(P_{1,k+1}^{+}\right)}^{-1}{\hat{x}}_{1,k+1}^{+}+{\mathrm{\ω}}_2{\left(P_{2,k+1}^{+}\right)}^{-1}{\hat{x}}_{2,k+1}^{+}+{\mathrm{\ω}}_3{\left(P_{3,k+1}^{+}\right)}^{-1}{\hat{x}}_{3,k+1}^{+}\]}^{-1},$

Wherein, three weight coefficient ω₁,ω₂,ω₃>0, meet ω₁+ω₂+ω₃=1, and determined by following formula：

${\mathrm{\ω}}_i=\frac{\left|\right|P_{k+1}^{\mathrm{\ω}}|{|}_F-||P_{k+1}^{\mathrm{\ω}}-P_{i,k+1}^{+}|{|}_F+\left|\right|P_{i,k+1}^{+}|{|}_F}{3\left|\right|P_{k+1}^{\mathrm{\ω}}|{|}_F+\underset{j=1}{\overset{3}{\Σ}}\left(\right|\left|P_{i,k+1}^{+}\right|{|}_F-\left|\right|P_{k+1}^{\mathrm{\ω}}-P_{i,k+1}^{+}\left|{|}_F\right)},$

Wherein,||·||_FRepresent Frobenius norm.

6. the Mars approach section distributed freedom air navigation aid based on covariance mixing together according to claim 1, its It is characterised by, described by Taylor series expansion to the method for single order item is：For infinite function f (x) that can be micro-, Tai Leji Number is deployed into single order item and is：

F (x)=f (a)+f ' (a) (x-a),

Wherein, first derivative of the f ' (a) representative function f (x) at point x=a.