CN103411614B - The iteration SKF method of Mars power dropping section multi-source information integrated navigation - Google Patents

Abstract

An iteration SKF method for Mars power dropping section multi-source information integrated navigation, step is as follows: the kinetics equation one, utilizing Mars power dropping section; Two, the measurement equation of Mars power dropping section is set up; Three, discretize kinetics equation and measurement equation and after linearization kinetics equation and measurement equation; Four, iteration SKF filtering algorithm is utilized to export navigation information.By above four steps, tectonodynamics equation and measurement equation, then utilize iteration SKF filtering algorithm to eliminate the impact of metrical information medial error, guarantee the stability of filtering algorithm, estimates the object of detector navigational state when reaching efficient real.It has modified the impact of measurement equation large deviations on filtering effectively, and utilize alternative manner, have modified the filtering error that the truncation error brought due to Taylor series is brought, improve navigation accuracy, enhance the stability of filtering, thus navigational state can be estimated real-time and efficiently to detector.

Description

The iteration SKF method of Mars power dropping section multi-source information integrated navigation

Technical field

The present invention relates to the autonomous navigation method of a kind of Mars power dropping section integrated navigation, be specifically related to the iteration SKF method of a kind of Mars power dropping section multi-source information integrated navigation, belong to space flight autonomous navigation technology field.

Background technology

Mars probes will carry out martian surface soft landing must experience approach section, descending branch and landing phase (being called for short EDL) three phases.Although whole EDL process only has 6-10 minute, this is one of the most dangerous, most important process of whole mars exploration task.

When power dropping section starts from Mars probes distance ground 6-12 km.During beginning, open supersonic speed parachute, but because now Mach number is higher, cannot jettisoning hot baffle immediately, therefore this section still cannot use other sensors, and Inertial Measurement Unit (being called for short IMU) can only be relied on to navigate; When speed is reduced to 0.7 Mach, hot baffle is by jettisoning, and the surveying instruments such as the altitude gauge that Mars probes carry, radar Doppler and laser radar are started working, and the metrical information that multiple surveying instrument now can be utilized to provide carries out independent navigation.The volume of the surveying instruments such as radar, weight and power consumption are all larger, application is caused to be restricted, particularly on small-sized Mars probes, Given this kind of microsensor of U.S.'s jet thrust development in laboratory (being called for short MCAV), it adopts laser to carry out ranging and range rate, the height in 3 directions of direct output detector and speed, gathered altitude gauge and velograph two kinds of sensors, have lightweight, volume is little, precision is high, low power consumption and other advantages.The metrical information that the present invention is based on IMU and MCAV provides carries out information fusion, sets up the autonomous navigation method of a kind of Mars power dropping section integrated navigation.

There is height that the gyroscope of IMU and the unknown constant value drift of accelerometer and MCAV provide in the integrated navigation system based on IMU and MCAV and speed exists the problems such as deviation, and existing filtering method cannot well address this problem.The gyro of IMU and the constant value drift of accelerometer are incorporated in estimated state and utilize EKF (being called for short EKF) to estimate by Peng Yuming, but there is constant value site error at X and Y-direction, and it is bad to the drift estimate effect of gyro and accelerometer, and cause dimension excessive without mark filtering method (being called for short UKF) owing to having expanded state, cannot requirement of real time (see Peng Yuming, " novel Mars EDL Navigation, Guidance and Control technical research ", Master's thesis, Nanjing Aero-Space University, 2011.).

The present invention considers the variance of these deviations in the iteration SKF filtering of the Mars power dropping section multi-source information integrated navigation proposed and is introduced in filtering algorithm, but does not estimate them.Meanwhile, measurement is upgraded and uses alternative manner, contribute to eliminating the truncation error brought when carrying out Taylor series expansion to measurement equation, the stability of delta filter.This kind of independent navigation filtering algorithm not only considers these deviations and blocks the impact on navigational state, and decreases calculated amount and computation complexity, is conducive to the state of Mars power dropping section to detector and estimates real-time and efficiently.

Summary of the invention

The object of this invention is to provide the iteration SKF method of a kind of Mars power dropping section multi-source information integrated navigation.In Mars power dropping section, kinetics equation is nonlinear, and existing non-linear independent navigation filtering algorithm cannot these deviations and the truncation error brought when carrying out Taylor series expansion to measurement equation in elimination pharmacokinetic equation and measurement equation, can have a strong impact on the convergence of estimated state, or employing state is augmented the phenomenon being unfavorable for estimating in real time that method causes calculated amount sharply to increase.Iteration SKF filtering algorithm proposed by the invention considers the variance of these deviations and is introduced in filtering algorithm, but does not estimate them, utilizes alternative manner to eliminate the truncation error brought when carrying out Taylor series expansion to measurement equation simultaneously.This kind of independent navigation filtering algorithm not only considers these deviations and truncation error to the impact of navigational state, and decreases calculated amount and computation complexity, is conducive to the state of Mars power dropping section to detector and estimates real-time and efficiently.

The invention provides the iteration SKF method of a kind of Mars power dropping section multi-source information integrated navigation, it comprises following four steps:

Step one, utilize the kinetics equation of Mars power dropping section

The situation more complicated of Mars power dropping section, sets up accurate kinetic model very difficult, is considering, on the basis that IMU exports, to utilize the kinetics equation of its tectonic kinetics descending branch:

$\stackrel{\·}{r}=v$

$\stackrel{\·}{v}=C_b^i(\tilde{a}-b_a)+C_g^{i}g-C_b^i{\mathrm{\η}}_a---\left(1\right)$

$\stackrel{\·}{\mathrm{\Ω}}=K(\widetilde{\mathrm{\ω}}-b_{\mathrm{\ω}})-K{\mathrm{\η}}_{\mathrm{\ω}}$

In formula, r=[xyz] ^trepresent that landing point is connected the position vector under being, v=[v _xv _yv _z] ^trepresent that landing point is connected the velocity vector under being, Ω=[σ θ ψ] ^trepresent that landing point is connected the attitude angle under being, controlled quentity controlled variable σ is the roll angle of detector, and θ is the longitude of detector, and ψ is the course angle of detector. represent that Mars centered inertial is tied to the transition matrix of detector body coordinate system, represent that Mars geographic coordinate is tied to the transition matrix of Mars centered inertial system. represent the linear acceleration of three axis under the body coordinate system exported by accelerometer in IMU, b _arepresent the constant value drift of accelerometer, η _arepresent the noise of accelerometer; represent the momentary rotational angle speed of three axis under the body coordinate system exported by gyro in IMU, b _ωrepresent the constant value drift of gyro, η _ωrepresent the noise of gyro.G represents the Mars acceleration of gravity of geographic coordinate system.K represents attitude kinematics matrix

$K=\frac{1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}\sin \mathrm{\σ}& \sin \mathrm{\θ}\cos \mathrm{\σ}\\ 0& \cos \mathrm{\θ}\cos \mathrm{\σ}& -\cos \mathrm{\θ}\sin \mathrm{\σ}\\ 0& \sin \mathrm{\σ}& \cos \mathrm{\σ}\end{array}\right]---\left(2\right)$

Getting state vector is X=[r ^tv ^tΩ ^t] ^t, then the kinetics equation (1) of power dropping section can be reduced to

$\stackrel{\·}{X}=f\left(X\right)+w---\left(3\right)$

In formula, f (X) is mission nonlinear continuous state transfer function, system noise

$w={\left[\begin{array}{ccc}{0}_{3\×1}& -C_b^i{\mathrm{\η}}_a^T& -{\mathrm{K\η}}_{\mathrm{\ω}}^T\end{array}\right]}^T$ For the white Gaussian noise of zero-mean.

The measurement equation of step 2, Mars power dropping section

What the microsensor MCAV be fixed in body coordinate system exported is the height of detector and the speed of three axles, then based on its metrical information, set up the measurement equation of Mars power dropping section:

Z=h(X)+b+v _m(4)

In formula,

h(X)=[r _zv ^b] ^T,(5)

R _zfor the Z axis coordinate (i.e. the areographic height of detector distance) of detector, v ^bfor the speed under body coordinate system, its expression formula is

$v^b=C_i^{b}v---\left(6\right)$

In formula, represent that detector body coordinate is tied to the transition matrix of Mars centered inertial system.B is constant value bias vector.Measurement noises v _mfor the white Gaussian noise of zero-mean, and uncorrelated with system noise w.

The above-mentioned kinetics equation of step 3, discretize (3) and measurement equation (4),

X _k+1=F(X _k)+w _k（7）

in formula, k=1,2,3 ..., F (X _k) for f (X) discrete after nonlinear state transfer function, for h (X) discrete after non-linear measurement function, w _kand v _kuncorrelated mutually, and its variance matrix is respectively Q _kand R _k.

By the nonlinear discrete function F (X in formula (7) _k) around estimated value be launched into Taylor series, and omit the above item of second order, the kinetics equation after obtaining linearization

X _k+1=Φ _k+1/kX _k+U _k+w _k（9）

In formula,

${\mathrm{\Φ}}_{k+1/k}=\frac{\∂F\left(X_k\right)}{{\∂X}_k}{|}_{X_k={\hat{X}}_k}---\left(10\right)$

$U_k=F\left({\hat{X}}_k\right)-\frac{\∂F\left(X_k\right)}{{\∂X}_k}{|}_{X_k={\hat{X}}_k}\·{\hat{X}}_k---\left(11\right)$

Then, then by the nonlinear discrete function in formula (8) around estimated value with be launched into Taylor series, and omit the above item of second order, the measurement equation after obtaining linearization

Z _k=H _kX _k+Y _k+v _k（12）

In formula,

By said process, just obtain the kinetics equation after linearization and measurement equation, U in formula _kand Y _kfor nonrandom outer effect item.

Step 4, iteration SKF filtering algorithm and navigation information export

Because the constant value deviation b in formula (4) fails accurately to know, therefore Schmidt-Kalman filtering algorithm (SchmidtKalmanfilter, be called for short SKF) on the basis not estimating these deviations, its variance is considered to be dissolved in filtering algorithm, that is to say the cross covariance by considering deviation and state, increasing the precision of filtering.Simultaneously, due to faced by be nonlinear filtering, truncation error can be produced when Taylor series expansion is carried out to measurement equation, therefore the method for iteration is adopted to reduce the impact of error on filtering non-linear measurement equation being carried out to linearization generation, thus reach minimizing Divergent Phenomenon, improve filtering accuracy, guarantee the numerical stability of filtering.Iteration SKF filtering algorithm performing step of the present invention is as follows:

1. be augmented state vector X _k, add by constant value bias vector b, then kinetics equation (9) and measurement equation (12) become

$\left[\begin{array}{c}{X}_{k+1}\\ b_{k+1}\end{array}\right]=\left[\begin{array}{cc}{\mathrm{\Φ}}_{k+1|k}& 0\\ 0& I\end{array}\right]\left[\begin{array}{c}{X}_k\\ b_k\end{array}\right]+\begin{array}{c}\left[\begin{array}{c}{w}_k\\ 0\end{array}\right]\end{array}---\left(15\right)$

$Z_k=[H_k,I]\left[\begin{array}{c}{X}_k\\ b_k\end{array}\right]+v_k---\left(16\right)$

In formula, constant value bias vector satisfies condition: b _k+1=b _k, and its variance matrix B ₀meet

B ₀=Cov{b ₀}=Cov{b _k}(17)

With the Cross-covariance C of deviation and state _kmeet

$C_k=E\left\{{\tilde{X}}_k{b}_k^T\right\}=E\left\{(X_k-{\hat{X}}_k)b_k^T\right\},---\left(18\right)$

And initial value is C ₀=0.In formula, for the state estimation of Kalman filtering kth step.

The error covariance matrix that corresponding and kinetics equation (15) and measurement equation (16) kth walk for

In formula, for C _ktransposed matrix, P _kfor state X _kerror covariance matrix

$P_k=E\left\{{\tilde{X}}_k{\tilde{X}}_k^T\right\}=E\left\{(X_k-{\hat{X}}_k){(X_k-{\hat{X}}_k)}^T\right\}---\left(20\right)$

When starting filtering calculating, need init state vector sum error covariance matrix, and set init state vector as X ₀and error covariance matrix is P ₀.

2. the state estimation that time renewal process is walked by kth can obtain, the state one-step prediction of kth+1 step for ${\hat{X}}_{k+1|k}={\mathrm{\Φ}}_{k+1|k}{\hat{X}}_k,---\left(21\right)$

And the one-step prediction varivance matrix of kth+1 step for

Then can obtain the one-step prediction varivance matrix P of state and deviation _k+1|kand C _k+1|k

$P_{k+1|k}={\mathrm{\Φ}}_{k+1|k}{P}_k{\mathrm{\Φ}}_{k+1|k}^T+Q_k---\left(23\right)$

C _k+1|k=Φ _k+1|kC _k(24)

3. measure renewal process

The present invention uses alternative manner to carry out measurement renewal: work as i=1,2,3 ... time, carry out cycle calculations as follows.

1) the filter gain matrix of the i-th step is calculated for

Wherein, owing to not needing estimated bias, therefore the gain matrix of injunction bias term is zero.

Then state gain matrix for

$K_{k+1}^i=[P_{k+1|k}{\left(H_{k+1}^i\right)}^T+C_{k+1|k}]{\left({\mathrm{\Ω}}_{k+1}^i\right)}^{-1}---\left(26\right)$

${\mathrm{\Ω}}_{k+1}^i=H_{k+1}^i{P}_{k+1|k}{\left(H_{k+1}^i\right)}^T+C_{k+1|k}^T{\left(H_{k+1}^i\right)}^T+H_{k+1}^i{C}_{k+1|k}+B_0+R_{k+1}---\left(27\right)$

2) the i-th step measurement information residual error is calculated

3) state estimation of the i-th step is calculated

${\hat{X}}_{k+1}^i={\hat{X}}_{k+1/k}+K_{k+1}^i\{{\tilde{Z}}_{k+1}^i-H_{k+1}^i{\hat{X}}_{k+1/k}\}---\left(29\right)$

In formula,

Alternative manner is used to carry out in measurement renewal process, when the estimated value of gained state vector (wherein threshold value is set to ε to the condition that 2 norms meeting vector meet _limit):

${\left|\right|{\hat{X}}_{k+1}^i-{\hat{X}}_{k+1}^{i-1}\left|\right|}_2<{\mathrm{\&epsiv;}}_{\mathrm{limit}}---\left(31\right)$

Time and end loop calculate.

4. utilize and measure the state estimation in upgrading and corresponding parameter, calculate the estimation error variance matrix of kth+1 step for

Then state estimation error variance matrix P _k+1for

$P_{k+1}=P_{k+1|k}-K_{k+1}^i{\mathrm{\&Omega;}}_{k+1}^i{\left(K_{k+1}^i\right)}^T---\left(33\right)$

With the Cross-covariance C of deviation and state _k+1for

$C_{k+1}=C_{k+1|k}-K_{k+1}^i(H_{k+1}^i{C}_{k+1|k}+B_0)---\left(34\right)$

Carry out the real-time status estimated value that can obtain Mars power dropping section detector by above 4 step circulations, comprise the position vector of detector, velocity vector and attitude angle.

The present invention is made up of step one, step 2, step 3 and step 4 four steps altogether, by tectonodynamics equation and the measurement equation setting up multi-source information integrated navigation, then iteration SKF filtering algorithm is utilized to eliminate the impact of metrical information medial error, and guarantee the stability of filtering algorithm, estimate the object of detector navigational state when reaching efficient real.

Wherein, step 2 Chinese style (6) detector body coordinate is wherein tied to the transition matrix of Mars centered inertial system for inverse matrix, concrete to adopt when calculating

Wherein, in step 3 described " in formula, k=1,2,3 ... ", when generally calculating, k value is k=1,2,3 ..., N, wherein N was determined by filtering time and sampling period.Such as when filtering time is 60 seconds, when the sampling period is 1 hertz, N=60/1=60.

Wherein, " discretize kinetics equation (3) and measurement equation (4) " described in step 3, the method adopted is Taylor series expansion method.Taylor series be mathematically one infinite can be micro-the power series expansion of function f (x):

$f\left(x\right)=\underset{n=0}{\overset{\&infin;}{\mathrm{\&Sigma;}}}\frac{f^{\left(n\right)}\left(a\right)}{n!}{(x-a)}^n---\left(35\right)$

In formula, n! Represent the factorial of n, and f ⁽ⁿ⁾a () representative function f (x) is at the n order derivative at an x=a place.In practical application, Taylor series need to block, and only get finite term, therefore can produce corresponding truncation error.

Wherein, the norm that step 4 Chinese style (31) is used use following form calculus: vector x=(x ₁, x ₂..., x _n) 2 norms be that in x, each element square sum opens radical sign again, namely

${\left|\right|x\left|\right|}_2=\sqrt{x_1^2+x_2^2+...+x_n^2}---\left(36\right)$

Advantage of the present invention is: iteration SKF filtering algorithm of the present invention is compared with traditional EKF, increase only a little calculated amount, just by the information fusion of measurement equation large deviations to in the estimation procedure of state vector, effectively have modified the impact of measurement equation large deviations on filtering, and utilize alternative manner, have modified the filtering error that the truncation error brought due to Taylor series is brought preferably, improve navigation accuracy, enhance the stability of filtering, thus navigational state can be estimated real-time and efficiently to detector.

Accompanying drawing explanation

Fig. 1 is process flow diagram of the present invention.

Fig. 2 is the filtering method in the present invention---the schematic diagram of iteration SKF filtering algorithm.

Embodiment

Below in conjunction with accompanying drawing, the present invention is elaborated.

The iteration SKF method of a kind of Mars power dropping of the present invention section multi-source information integrated navigation, its calculation flow chart as shown in Figure 1 with the schematic diagram of iteration SKF filtering algorithm as shown in Figure 2, it comprises following four steps:

The kinetics equation of step one, Mars power dropping section

The situation more complicated of Mars power dropping section, sets up accurate kinetic model very difficult, is considering, on the basis that IMU exports, to utilize the kinetics equation of its tectonic kinetics descending branch:

$\stackrel{\&CenterDot;}{r}=v$

$\stackrel{\&CenterDot;}{v}=C_b^i(\tilde{a}-b_a)+C_g^{i}g-C_b^i{\mathrm{\&eta;}}_a---\left(1\right)$

$\stackrel{\&CenterDot;}{\mathrm{\&Omega;}}=K(\widetilde{\mathrm{\&omega;}}-b_{\mathrm{\&omega;}})-K{\mathrm{\&eta;}}_{\mathrm{\&omega;}}$

In formula, r=[xyz] ^trepresent that landing point is connected the position vector under being, v=[v _xv _yv _z] ^trepresent that landing point is connected the velocity vector under being, Ω=[σ θ ψ] ^trepresent that landing point is connected the attitude angle under being, controlled quentity controlled variable σ is the roll angle of detector, and θ is the longitude of detector, and ψ is the course angle of detector. represent that Mars centered inertial is tied to the transition matrix of detector body coordinate system, represent that Mars geographic coordinate is tied to the transition matrix of Mars centered inertial system. represent the linear acceleration of three axis under the body coordinate system exported by accelerometer in IMU, b _arepresent the constant value drift of accelerometer, η _arepresent the noise of accelerometer; represent the momentary rotational angle speed of three axis under the body coordinate system exported by gyro in IMU, b _ωrepresent the constant value drift of gyro, η _ωrepresent the noise of gyro.G represents the Mars acceleration of gravity of geographic coordinate system.K represents attitude kinematics matrix

$K=\frac{1}{\cos \mathrm{\&theta;}}\left[\begin{array}{ccc}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\sin \mathrm{\&sigma;}& \sin \mathrm{\&theta;}\cos \mathrm{\&sigma;}\\ 0& \cos \mathrm{\&theta;}\cos \mathrm{\&sigma;}& -\cos \mathrm{\&theta;}\sin \mathrm{\&sigma;}\\ 0& \sin \mathrm{\&sigma;}& \cos \mathrm{\&sigma;}\end{array}\right]---\left(2\right)$

Getting state vector is X=[r ^tv ^tΩ ^t] ^t, then the kinetics equation (1) of power dropping section can be reduced to

$\stackrel{\&CenterDot;}{X}=f\left(X\right)+w---\left(3\right)$

In formula, f (X) is mission nonlinear continuous state transfer function, system noise

$w={\left[\begin{array}{ccc}{0}_{3\&times;1}& -C_b^i{\mathrm{\&eta;}}_a^T& -{\mathrm{K\&eta;}}_{\mathrm{\&omega;}}^T\end{array}\right]}^T$ For the white Gaussian noise of zero-mean.

Wherein, in concrete enforcement, the initial value of gyro and accelerometer is generally set to zero, and error is generally set to 5 × 10 respectively ^-5rad/s and 3 × 10 ^-3m/s ².

The measurement equation of step 2, Mars power dropping section

What the microsensor MCAV be fixed in body coordinate system exported is the height of detector and the speed of three axles, then based on its metrical information, set up the measurement equation of Mars power dropping section:

Z=h(X)+b+v _m(4)

In formula,

H (X)=[r _zv ^b] ^t, (5) r _zfor the Z axis coordinate (i.e. the areographic height of detector distance) of detector, v ^bfor the speed under body coordinate system, its expression formula is

$v^b=C_i^{b}v---\left(6\right)$ In formula, represent that detector body coordinate is tied to the transition matrix of Mars centered inertial system.B is constant value bias vector.Measurement noises v _mfor the white Gaussian noise of zero-mean, and uncorrelated with system noise w.

The above-mentioned kinetics equation of step 3, discretize (3) and measurement equation (4),

X _k+1=F(X _k)+w _k（7）

In formula, k=1,2,3 ..., F (X _k) for f (X) discrete after nonlinear state transfer function, for h (X) discrete after non-linear measurement function, w _kand v _kuncorrelated mutually, and its variance matrix is respectively Q _kand R _k.

By the nonlinear discrete function F (X in formula (7) _k) around estimated value be launched into Taylor series, and omit the above item of second order, the kinetics equation after obtaining linearization

$X_{k+1}={\mathrm{\&Phi;}}_{k+1|k}{X}_k+U_k+w_k---\left(9\right)$

In formula,

${\mathrm{\&Phi;}}_{k+1|k}=\frac{\&PartialD;F\left(X_k\right)}{{\&PartialD;X}_k}{|}_{X_k={\hat{X}}_k}---\left(10\right)$

$U_k=F\left({\hat{X}}_k\right)-\frac{\&PartialD;F\left(X_k\right)}{{\&PartialD;X}_k}{|}_{X_k={\hat{X}}_k}\&CenterDot;{\hat{X}}_k---\left(11\right)$

Then, then by the nonlinear discrete function in formula (8) around estimated value with be launched into Taylor series, and omit the above item of second order, the measurement equation after obtaining linearization

Z _k=H _kX _k+Y _k+v _k（12）

In formula,

By said process, just obtain the kinetics equation after linearization and measurement equation, U in formula _kand Y _kfor nonrandom outer effect item.

Step 4, iteration SKF filtering algorithm and navigation information export

Because the constant value deviation b in formula (4) fails accurately to know, therefore Schmidt-Kalman filtering algorithm (SchmidtKalmanfilter, be called for short SKF) on the basis not estimating these deviations, its variance is considered to be dissolved in filtering algorithm, that is to say the cross covariance by considering deviation and state, increasing the precision of filtering.Simultaneously, due to faced by be nonlinear filtering, truncation error can be produced when Taylor series expansion is carried out to measurement equation, therefore the method for iteration is adopted to reduce the impact of error on filtering non-linear measurement equation being carried out to linearization generation, thus reach minimizing Divergent Phenomenon, improve filtering accuracy, guarantee the numerical stability of filtering.Iteration SKF filtering algorithm performing step of the present invention is as follows:

1. be augmented state vector X _k, add by constant value bias vector b, then kinetics equation (9) and measurement equation (12) become

$\left[\begin{array}{c}{X}_{k+1}\\ b_{k+1}\end{array}\right]=\left[\begin{array}{cc}{\mathrm{\&Phi;}}_{k+1|k}& 0\\ 0& I\end{array}\right]\left[\begin{array}{c}{X}_k\\ b_k\end{array}\right]+\begin{array}{}\end{array}\left[\begin{array}{c}{w}_k\\ 0\end{array}\right]---\left(\text{15}\right)$

$Z_k=[H_k,I]\left[\begin{array}{c}{X}_k\\ b_k\end{array}\right]+v_k---\left(16\right)$

In formula, constant value bias vector satisfies condition: b _k+1=b _k, and its variance matrix B ₀meet

B ₀=Cov{b ₀}=Cov{b _k}(17)

With the Cross-covariance C of deviation and state _kmeet

$C_k=E\left\{{\tilde{X}}_k{b}_k^T\right\}=E\left\{(X_k-{\hat{X}}_k)b_k^T\right\},---\left(18\right)$

And initial value is C ₀=0.In formula, for the state estimation of Kalman filtering kth step.

The error covariance matrix that corresponding and kinetics equation (15) and measurement equation (16) kth walk for

In formula, for C _ktransposed matrix, P _kfor state X _kerror covariance matrix

$P_k=E\left\{{\tilde{X}}_k{\tilde{X}}_k^T\right\}=E\left\{(X_k-{\hat{X}}_k){(X_k-{\hat{X}}_k)}^T\right\}---\left(20\right)$

When starting filtering calculating, need init state vector sum error covariance matrix, and set init state vector as X ₀and error covariance matrix is P ₀.

2. time renewal process

The state estimation walked by kth can obtain, the state one-step prediction of kth+1 step for

${\hat{X}}_{k+1|k}={\mathrm{\&Phi;}}_{k+1|k}{\hat{X}}_k,---\left(21\right)$

And the one-step prediction varivance matrix of kth+1 step for

Then can obtain the one-step prediction varivance matrix P of state and deviation _k+1|kand C _k+1|k

$P_{k+1|k}={\mathrm{\&Phi;}}_{k+1|k}{P}_k{\mathrm{\&Phi;}}_{k+1|k}^T+Q_k---\left(23\right)$

$C_{k+1|k}={\mathrm{\&Phi;}}_{k+1|k}{C}_k---\left(24\right)$

3. measure renewal process

The present invention uses alternative manner to carry out measurement renewal: work as i=1,2,3 ... time, carry out cycle calculations as follows.

2) the filter gain matrix of the i-th step is calculated for

Wherein, owing to not needing estimated bias, therefore the gain matrix of injunction bias term is zero.

Then state gain matrix for

$K_{k+1}^i=[P_{k+1|k}{\left(H_{k+1}^i\right)}^T+C_{k+1|k}]{\left({\mathrm{\&Omega;}}_{k+1}^i\right)}^{-1}---\left(26\right)$

${\mathrm{\&Omega;}}_{k+1}^i=H_{k+1}^i{P}_{k+1|k}{\left(H_{k+1}^i\right)}^T+C_{k+1|k}^T{\left(H_{k+1}^i\right)}^T+H_{k+1}^i{C}_{k+1|k}+B_0+R_{k+1}---\left(\text{27}\right)$

2) the i-th step measurement information residual error is calculated

3) state estimation of the i-th step is calculated

${\hat{X}}_{k+1}^i={\hat{X}}_{k+1|k}+K_{k+1}^i\{{\tilde{Z}}_{k+1}^i-H_{k+1}^i{\hat{X}}_{k+1|k}\}---\left(29\right)$

In formula,

Alternative manner is used to carry out in measurement renewal process, when the estimated value of gained state vector (wherein threshold value is set to ε to the condition that 2 norms meeting vector meet _limit):

${\left|\right|{\hat{X}}_{k+1}^i-{\hat{X}}_{k+1}^{i-1}\left|\right|}_2<{\mathrm{\&epsiv;}}_{\mathrm{limit}}---\left(31\right)$

Time and end loop calculate.

4. utilize and measure the state estimation in upgrading and corresponding parameter, calculate the estimation error variance matrix of kth+1 step for

Then state estimation error variance matrix P _k+1for

$P_{k+1}=P_{k+1|k}-K_{k+1}^i{\mathrm{\&Omega;}}_{k+1}^i{\left(K_{k+1}^i\right)}^T---\left(33\right)$

With the Cross-covariance C of deviation and state _k+1for

$C_{k+1}=C_{k+1|k}-K_{k+1}^i(H_{k+1}^i{C}_{k+1|k}+B_0)---\left(34\right)$

Carry out the real-time status estimated value that can obtain Mars power dropping section detector by above 4 step circulations, comprise the position vector of detector, velocity vector and attitude angle.The schematic diagram of iteration SKF filtering algorithm of the present invention is see Fig. 2.

The present invention is made up of step one, step 2, step 3 and step 4 four steps altogether, by tectonodynamics equation and the measurement equation setting up multi-source information integrated navigation, then iteration SKF filtering algorithm is utilized to eliminate the impact of metrical information medial error, and guarantee the stability of filtering algorithm, estimate the object of detector navigational state when reaching efficient real.

Wherein, the Mars gravity acceleration g of geographic coordinate system listed in step one, because detector distance martian surface during descending branch is very near, therefore hypothesis g is constant, and is set to g=[00-3.69] ^t.

Wherein, the detector body coordinate in step 2 Chinese style (6) is tied to the transition matrix of Mars centered inertial system for inverse matrix, concrete to adopt when calculating

Wherein, described in step 3 " in formula, k=1,2,3 ... ", when generally calculating, k value is k=1,2,3 ..., N, wherein N was determined by filtering time and sampling period.Such as when filtering time is 60 seconds, when the sampling period is 1 hertz, N=60/1=60.

Wherein, " discretize kinetics equation (3) and measurement equation (4) " described in step 3, the method adopted is Taylor series expansion method.Taylor series be mathematically one infinite can be micro-the power series expansion of function f (x):

$f\left(x\right)=\underset{n=0}{\overset{\&infin;}{\mathrm{\&Sigma;}}}\frac{f^{\left(n\right)}\left(a\right)}{n!}{(x-a)}^n---\left(35\right)$

In formula, n! Represent the factorial of n, and f ⁽ⁿ⁾a () representative function f (x) is at the n order derivative at an x=a place.In practical application, Taylor series need to block, and only get finite term, therefore can produce corresponding truncation error.

Wherein, norm used in step 4 Chinese style (31) " use following form calculus: vector x=(x ₁, x ₂..., x _n) 2 norms be that in x, each element square sum opens radical sign again, namely

${\left|\right|x\left|\right|}_2=\sqrt{x_1^2+x_2^2+\&CenterDot;\&CenterDot;\&CenterDot;+x_n^2}---\left(36\right)$

Advantage of the present invention is: iteration SKF filtering algorithm of the present invention is compared with traditional EKF, increase only a little calculated amount, just by the information fusion of measurement equation large deviations to in the estimation procedure of state vector, effectively have modified the impact of measurement equation large deviations on filtering, and utilize alternative manner, have modified the filtering error that the truncation error brought due to Taylor series is brought preferably, improve navigation accuracy, enhance the stability of filtering, thus navigational state can be estimated real-time and efficiently to detector.

Claims (5)

1. an iteration SKF method for Mars power dropping section multi-source information integrated navigation, is characterized in that: the method step is as follows:

Step one, utilize the kinetics equation of Mars power dropping section

Considering, on the basis that IMU exports, to utilize the kinetics equation of its tectonic kinetics descending branch:

$\stackrel{\&CenterDot;}{r}=v$

$\stackrel{\&CenterDot;}{v}=C_b^i(\tilde{a}-b_a)+C_g^{i}g-C_b^i{\mathrm{\&eta;}}_a---\left(1\right)$

$\stackrel{\&CenterDot;}{\mathrm{\&Omega;}}=K(\widetilde{\mathrm{\&omega;}}-b_{\mathrm{\&omega;}})-{\mathrm{K\&eta;}}_{\mathrm{\&omega;}}$

In formula, r=[xyz] ^trepresent that landing point is connected the position vector under being, v=[v _xv _yv _z] ^trepresent that landing point is connected the velocity vector under being, Ω=[σ θ ψ] ^trepresent that landing point is connected the attitude angle under being, controlled quentity controlled variable σ is the roll angle of detector, and θ is the longitude of detector, and ψ is the course angle of detector; represent that Mars centered inertial is tied to the transition matrix of detector body coordinate system, represent that Mars geographic coordinate is tied to the transition matrix of Mars centered inertial system; represent the linear acceleration of three axis under the body coordinate system exported by accelerometer in IMU, b _arepresent the constant value drift of accelerometer, η _arepresent the noise of accelerometer; represent the momentary rotational angle speed of three axis under the body coordinate system exported by gyro in IMU, b _ωrepresent the constant value drift of gyro, η _ωrepresent the noise of gyro; G represents the Mars acceleration of gravity of geographic coordinate system; K represents attitude kinematics matrix

$K=\frac{1}{\cos \mathrm{\&theta;}}\left[\begin{array}{ccc}cos\mathrm{\&theta;}& sin\mathrm{\&theta;}sin\mathrm{\&sigma;}& sin\mathrm{\&theta;}cos\mathrm{\&sigma;}\\ 0& cos\mathrm{\&theta;}cos\mathrm{\&sigma;}& -\cos \mathrm{\&theta;}sin\mathrm{\&sigma;}\\ 0& sin\mathrm{\&sigma;}& cos\mathrm{\&sigma;}\end{array}\right]---\left(2\right)$

Getting state vector is X=[r ^tv ^tΩ ^t] ^t, then the kinetics equation (1) of power dropping section is reduced to

$\stackrel{\&CenterDot;}{X}=f\left(X\right)+w---\left(3\right)$

In formula, f (X) is mission nonlinear continuous state transfer function, system noise $w={\left[\begin{array}{ccc}{0}_{3\&times;1}& -C_b^i{\mathrm{\&eta;}}_a^T& -{\mathrm{K\&eta;}}_{\mathrm{\&omega;}}^T\end{array}\right]}^T$ For the white Gaussian noise of zero-mean;

The measurement equation of step 2, Mars power dropping section

What the microsensor MCAV be fixed in body coordinate system exported is the height of detector and the speed of three axles, then based on its metrical information, set up the measurement equation of Mars power dropping section:

Z＝h(X)+b+v _m(4)

In formula, Z is measuring value;

h(X)＝[r _zv ^b] ^T，(5)

R _zfor the Z axis coordinate of detector, i.e. the areographic height of detector distance, v ^bfor the speed under body coordinate system, its expression formula is

$v^b=C_i^{b}v---\left(6\right)$

In formula, represent that detector body coordinate is tied to the transition matrix of Mars centered inertial system, b is constant value bias vector, measurement noises v _mfor the white Gaussian noise of zero-mean, and uncorrelated with system noise w;

The above-mentioned kinetics equation of step 3, discretize (3) and measurement equation (4),

X _k+1＝F(X _k)+w _k(7)

In formula, k=1,2,3 ..., F (X _k) for f (X) discrete after nonlinear state transfer function, for h (X) discrete after non-linear measurement function, w _kand v _kuncorrelated mutually, and its variance matrix is respectively Q _kand R _k;

By the nonlinear discrete function F (X in formula (7) _k) around estimated value be launched into Taylor series, and omit the above item of second order, the kinetics equation after obtaining linearization

X _k+1＝Φ _k+1/kX _k+U _k+w _k(9)

In formula,

${\mathrm{\&Phi;}}_{k+1/k}=\frac{\&part;F\left(X_k\right)}{\&part;X_k}{|}_{x_k={\hat{x}}_k}---\left(10\right)$

$U_k=F\left({\hat{X}}_k\right)-\frac{\&part;F\left(X_k\right)}{\&part;X_k}{|}_{x_k={\hat{x}}_k}\&CenterDot;{\hat{X}}_k---\left(11\right)$

Then, then by the nonlinear discrete function in formula (8) around estimated value with be launched into Taylor series, and omit the above item of second order, the measurement equation after obtaining linearization

Z _k＝H _kX _k+Y _k+v _k(12)

In formula,

By said process, just obtain the kinetics equation after linearization and measurement equation, U in formula _kand Y _kfor nonrandom outer effect item;

Step 4, iteration SKF filtering algorithm and navigation information export

Described iteration SKF filtering algorithm performing step is as follows:

1. be augmented state vector X _k, add by constant value bias vector b, then kinetics equation (9) and measurement equation (12) become

$\left[\begin{array}{c}{X}_{k+1}\\ b_{k+1}\end{array}\right]=\left[\begin{array}{cc}{\mathrm{\&Phi;}}_{k+1|k}& 0\\ 0& I\end{array}\right]\left[\begin{array}{c}{X}_k\\ b_k\end{array}\right]+\left[\begin{array}{c}{w}_k\\ 0\end{array}\right]---\left(15\right)$

$Z_k=\&lsqb;H_k,I\&rsqb;\left[\begin{array}{c}{X}_k\\ b_k\end{array}\right]+v_k---\left(16\right)$

In formula, I is unit matrix, and constant value bias vector satisfies condition: b _k+1=b _k, and its variance matrix B ₀meet

B ₀＝Cov{b ₀}＝Cov{b _k}(17)

With the Cross-covariance C of deviation and state _kmeet

$C_k=E\left\{{\tilde{X}}_k{b}_k^T\right\}=E\left\{(X_k-{\hat{X}}_k)b_k^T\right\},---\left(18\right)$

And initial value is C ₀=0; In formula, for the state estimation of Kalman filtering kth step;

The error covariance matrix that corresponding and kinetics equation (15) and measurement equation (16) kth walk for

In formula, for C _ktransposed matrix, P _kfor state X _kerror covariance matrix

$P_k=E\left\{{\tilde{X}}_k{\tilde{X}}_k^T\right\}=E\left\{(X_k-{\hat{X}}_k){(X_k-{\hat{X}}_k)}^T\right\}---\left(20\right)$

When starting filtering calculating, need init state vector sum error covariance matrix, and set init state vector as X ₀and error covariance matrix is P ₀;

2. time renewal process

The state estimation walked by kth can obtain, the state one-step prediction of kth+1 step for

${\hat{X}}_{k+1/k}={\mathrm{\&Phi;}}_{k+1|k}{\hat{X}}_k,---\left(21\right)$

And the one-step prediction varivance matrix of kth+1 step for

Then obtain the one-step prediction varivance matrix P of state and deviation _k+1kand C _k+1k

$P_{k+1|k}={\mathrm{\&Phi;}}_{k+1|k}{P}_k{\mathrm{\&Phi;}}_{k+1|k}^T+Q_k---\left(23\right)$

C _k+1|k＝Φ _k+1|kC _k(24)

3. measure renewal process

Use alternative manner to carry out measurement to upgrade: work as i=1,2,3 ... time, carry out cycle calculations as follows:

1) the filter gain matrix of the i-th step is calculated for

Wherein, owing to not needing estimated bias, therefore the gain matrix of injunction bias term is zero;

Then state gain matrix for

$K_{k+1}^i=\&lsqb;P_{k+1|k}{\left(H_{k+1}^i\right)}^T+C_{k+1|k}\&rsqb;{\left({\mathrm{\&Omega;}}_{k+1}^i\right)}^{-1}---\left(26\right)$

${\mathrm{\&Omega;}}_{k+1}^i=H_{k+1}^i{P}_{k+1|k}{\left(H_{k+1}^i\right)}^T+C_{k+1|k}^T{\left(H_{k+1}^i\right)}^T+H_{k+1}^i{C}_{k+1|k}+B_0+R_{k+1}---\left(27\right)$

2) the i-th step measurement information residual error is calculated

3) state estimation of the i-th step is calculated

${\hat{X}}_{k+1}^i={\hat{X}}_{k+1/k}+K_{k+1}^i\{{\tilde{Z}}_{k+1}^i-H_{k+1}^i{\hat{X}}_{k+1/k}\}---\left(29\right)$

In formula,

Alternative manner is used to carry out in measurement renewal process, when the estimated value of gained state vector the threshold value of condition-wherein that 2 norms meeting vector meet is set to ε _limit:

$\left|\right|{\hat{X}}_{k+1}^i-{\hat{X}}_{k+1}^{i-1}|{|}_2<{\mathrm{\&epsiv;}}_{\lim it}---\left(31\right)$

Time and end loop calculate;

4. utilize and measure the state estimation in upgrading and corresponding parameter, calculate the estimation error variance matrix of kth+1 step for

Then state estimation error variance matrix P _k+1for

$P_{k+1}=P_{k+1|k}-K_{k+1}^i{\mathrm{\&Omega;}}_{k+1}^i{\left(K_{k+1}^i\right)}^T---\left(33\right)$

With the Cross-covariance C of deviation and state _k+1for

$C_{k+1}=C_{k+1|k}-K_{k+1}^i(H_{k+1}^i{C}_{k+1|k}+B_0)---\left(34\right)$

Namely obtained the real-time status estimated value of Mars power dropping section detector by above 4 step circulations, comprise the position vector of detector, velocity vector and attitude angle.

2. the iteration SKF method of a kind of Mars power dropping section multi-source information according to claim 1 integrated navigation, is characterized in that: step 2 Chinese style (6) detector body coordinate is wherein tied to the transition matrix of Mars centered inertial system for inverse matrix, concrete to adopt when calculating

3. the iteration SKF method of a kind of Mars power dropping section multi-source information according to claim 1 integrated navigation, is characterized in that: in step 3 described " in formula, k=1,2,3; ... ", during calculating, k value is k=1,2,3 ..., N, wherein N was determined by filtering time and sampling period.

4. the iteration SKF method of a kind of Mars power dropping section multi-source information according to claim 1 integrated navigation, it is characterized in that: " discretize kinetics equation (3) and measurement equation (4) " described in step 3, the method adopted is Taylor series expansion method, Taylor series be mathematically one infinite can be micro-the power series expansion of function f (x):

$f\left(x\right)=\underset{n=0}{\overset{\mathrm{\&infin;}}{\&Sigma;}}\frac{f^{\left(n\right)}\left(a\right)}{n!}{(x-a)}^n---\left(35\right)$

In formula, n! Represent the factorial of n, and f ⁽ⁿ⁾a () representative function f (x) is at the n order derivative at an x=a place.

5. the iteration SKF method of a kind of Mars power dropping section multi-source information according to claim 1 integrated navigation, is characterized in that: the norm that step 4 Chinese style (31) is used use following form calculus: vector x=(x ₁, x ₂..., x _n) 2 norms be that in x, each element square sum opens radical sign again, namely

$\left|\right|x|{|}_2=\sqrt{x_1^2+x_2^2+\mathrm{...}+x_n^2}---\left(36\right).$