CN103411614A - Iteration SKF (Schmidt Kalman Filter) method of multi-source information integrated navigation of Mars power descent stage - Google Patents

Abstract

The invention discloses an iteration SKF method of multi-source information integrated navigation of the Mars power descent stage, which comprises the following steps: 1, utilizing the kinetics equation of the Mars power descent stage; 2, building the measurement equation of the Mars power descent stage; 3, discretizing the kinetics equation and the measurement equation, and then linearizing the kinetics equation and the measurement equation so as to obtain the novel kinetics equation and measurement equation; 4, outputting navigation information through utilizing the SKF filtering algorithm. Through the four steps, the kinetics equation and the measurement equation are built, and then the influence of errors in the measurement information is eliminated through utilizing the iteration SKF filtering algorithm to ensure the stability of the filtering algorithm, so that the purpose that navigation state of a detector is estimated efficiently and in real time. The method efficiently corrects the influence on the filtering caused by errors in the measurement equation, corrects the filtering errors caused by the truncation error caused by the Taylor series by utilizing the iteration method, improves the navigation precision, and enhances the stability of the filtering process, so that the navigation state of the detector can be estimated efficiently and in real time.

Description

The iteration SKF method of Mars power descending branch multi-source information integrated navigation

Technical field

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

Background technology

Mars probes will carry out the 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 that whole mars exploration task is the most dangerous, one of most important process.

When the power descending branch starts from Mars probes apart from ground 6-12 km.During beginning, open the supersonic speed parachute, but because now Mach number is higher, jettisoning hot baffle, so this section immediately still can't be used other sensors, can only rely on Inertial Measurement Unit (being called for short IMU) to navigate; When speed was reduced to 0.7 Mach, hot baffle was by jettisoning, and the surveying instruments such as the altitude gauge carried on Mars probes, radar Doppler and laser radar are started working, and now can utilize the metrical information that multiple surveying instrument provides to carry out independent navigation.The volume of the surveying instruments such as radar, weight and power consumption are all larger, cause application to be restricted, particularly on small-sized Mars probes, Given this U.S.'s jet thrust development in laboratory a kind of microsensor (be called for short MCAV), it adopts laser to carry out ranging and range rate, directly height and the speed of 3 directions of output detector, gathered two kinds of sensors of altitude gauge and velograph, 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 is carried out information fusion, sets up the autonomous navigation method of a kind of Mars power descending branch integrated navigation.

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

The present invention, in the iteration SKF filtering of the Mars power descending branch multi-source information integrated navigation proposed has been considered the variance of these deviations and has been introduced into filtering algorithm, does not still estimate them.Simultaneously, use alternative manner to measuring to upgrade, help to eliminate the truncation error of bringing when measurement equation is carried out to Taylor series expansion, the stability of increment filtering.This kind independent navigation filtering algorithm has not only been considered these deviations and block the impact on navigational state, and has reduced calculated amount and computation complexity, is conducive to Mars power descending branch the state of detector is estimated real-time and efficiently.

Summary of the invention

The iteration SKF method that the purpose of this invention is to provide the integrated navigation of a kind of Mars power descending branch multi-source information.In Mars power descending branch, kinetics equation is nonlinear, and the truncation error that existing non-linear independent navigation filtering algorithm brings can't eliminate these deviations in kinetics equation and measurement equation and measurement equation is carried out to Taylor series expansion the time, can have a strong impact on the convergence of estimated state, or the employing state is augmented the phenomenon that is unfavorable for real-time estimation that method causes calculated amount sharply to increase.Iteration SKF filtering algorithm proposed by the invention has been considered the variance of these deviations and has been introduced in filtering algorithm, but do not estimated them, the truncation error of bringing while utilizing simultaneously the alternative manner elimination to carry out Taylor series expansion to measurement equation.This kind independent navigation filtering algorithm has not only been considered the impact on navigational state of these deviations and truncation error, and has reduced calculated amount and computation complexity, is conducive to Mars power descending branch the state of detector is estimated real-time and efficiently.

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

Step 1, utilize the kinetics equation of Mars power descending branch

The situation more complicated of Mars power descending branch, set up accurate kinetic model very difficult, on the basis of considering IMU output, utilizes the kinetics equation of its structure power 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=[x y z] ^TMean be connected position vector under being of landing point, v=[v _xv _yv _z] ^TMean be connected velocity vector under being of landing point, Ω=[σ θ ψ] ^TMean be connected attitude angle under being of landing point, controlled quentity controlled variable σ is the roll angle of detector, and θ is the longitude of detector, and ψ is the course angle of detector.

Mean that the Mars centered inertial is tied to the transition matrix of detector body coordinate system,

Mean that the Mars geographic coordinate is tied to the transition matrix of Mars centered inertial system. Mean in IMU by three axial linear accelerations under the body coordinate system of accelerometer output, b _aThe constant value drift that means accelerometer, η _aThe noise that means accelerometer;

Mean in IMU by three axial instantaneous angular velocity of rotations under the body coordinate system of gyro output, b _ωThe constant value drift that means gyro, η _ωThe noise that means gyro.G means the Mars acceleration of gravity of geographic coordinate system.K means attitude motion 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, the kinetics equation of power descending branch (1) can be reduced to

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

In formula, f (X) is the non-linear continuous state transfer function of system, system noise

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

The measurement equation of step 2, Mars power descending branch

What be fixed on microsensor MCAV output on body coordinate system is the speed of height and three axles of detector, take the measurement equation of its metrical information as Foundation Mars power descending branch:

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

In formula,

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

R _zFor the Z axis coordinate (being 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,

Mean that the detector body coordinate is tied to the transition matrix of Mars centered inertial system.B is normal value bias vector.Measure noise v _mFor the white Gaussian noise of zero-mean, and uncorrelated with system noise w.

Step 3, the above-mentioned kinetics equation of discretize (3) and measurement equation (4),

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

In formula, k=1,2,3 ..., F (X _k) be the nonlinear state transfer function of f (X) after discrete,

Non-linear measurement function for h (X) after discrete, 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 above of second order, obtain linearization kinetics equation afterwards

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 above of second order, obtain linearization measurement equation afterwards

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

In formula,

By said process, just obtained kinetics equation and the measurement equation after the linearization, U in formula _kAnd Y _kFor nonrandom outer effect.

Step 4, iteration SKF filtering algorithm and navigation information output

Because the normal value deviation b in formula (4) fails accurately to know, therefore Schmidt-Kalman filtering algorithm (Schmidt Kalman filter, be called for short SKF) on the basis of not estimating these deviations, consideration is dissolved into its variance in filtering algorithm, that is to say by considering the cross covariance of deviation and state, increase the precision of filtering.Simultaneously, due to what face, it is nonlinear filtering, when being carried out to Taylor series expansion, measurement equation can produce truncation error, therefore adopt the method for iteration to reduce non-linear measurement equation is carried out to the impact of the error of linearization generation on filtering, thereby reach the 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, be about to normal value bias vector b and add, 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, often be worth bias vector and satisfy condition: b _K+1=b _k, and its variance matrix B ₀Meet

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

Cross-covariance C with 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,

State estimation for Kalman filtering k step.

The corresponding error covariance matrix walked with kinetics equation (15) and measurement equation (16) k

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)$

While starting filtering calculating, need init state vector sum error covariance matrix, and establish the vectorial X of being of init state ₀And error covariance matrix is P ₀.

2. the time renewal process can be obtained by the state estimation of k step, the state one-step prediction of k+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 k+1 step

For

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. measurement renewal process

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

1) calculate the filter gain matrix of i step For

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

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) calculate i step measurement information residual error

3) calculate the state estimation of i step

${\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,

Use alternative manner to measure in renewal process, when the estimated value of gained state vector

(wherein threshold value is made as ε to meet the satisfied condition of 2 vectorial norms _Limit):

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

The time end loop calculate.

4. utilize the state estimation value measured in upgrading

And corresponding parameter, calculate the estimation error variance matrix that k+1 walks For

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)$

Cross-covariance C with 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)$

By above 4 steps, loop the real-time status estimated value that can obtain Mars power descending branch detector, comprise position vector, velocity vector and the attitude angle of detector.

The present invention is comprised of step 1, step 2, step 3 and four steps of step 4 altogether, by the tectonodynamics equation with set up the measurement equation of multi-source information integrated navigation, then utilize iteration SKF filtering algorithm to eliminate the impact of error in metrical information, and guarantee the stability of filtering algorithm, reach the efficient purpose of estimating in real time the detector navigational state.

Wherein, step 2 Chinese style (6)

Detector body coordinate wherein is tied to the transition matrix of Mars centered inertial system

For

Inverse matrix, while specifically calculating, adopt

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

Wherein, at " discretize kinetics equation (3) and measurement equation (4) " described in step 3, the method adopted is Taylor series expansion method.Taylor series are the power series expansions of an infinite function f (x) that can be micro-on mathematics:

$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! The factorial that means 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 be blocked, and only get finite term, therefore can produce corresponding truncation error.

Wherein, step 4 Chinese style (31) norm used

To calculate by following form: vector x=(x ₁, x ₂..., x _n) 2 norms be that in x, each element square sum is opened 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, only increased a little calculated amount, just by the information fusion of deviation in measurement equation in the estimation procedure to state vector, effectively revised the impact of deviation on filtering in the measurement equation, and utilize alternative manner, revised preferably the filtering error that the truncation error brought due to Taylor series is brought, improved navigation accuracy, strengthen the stability of filtering, thereby can estimate real-time and efficiently navigational state to detector.

The 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 of the present invention descending branch 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 1, Mars power descending branch

The situation more complicated of Mars power descending branch, set up accurate kinetic model very difficult, on the basis of considering IMU output, utilizes the kinetics equation of its structure power 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=[x y z] ^TMean be connected position vector under being of landing point, v=[v _xv _yv _z] ^TMean be connected velocity vector under being of landing point, Ω=[σ θ ψ] ^TMean be connected attitude angle under being of landing point, controlled quentity controlled variable σ is the roll angle of detector, and θ is the longitude of detector, and ψ is the course angle of detector.

Mean that the Mars centered inertial is tied to the transition matrix of detector body coordinate system,

Mean that the Mars geographic coordinate is tied to the transition matrix of Mars centered inertial system.

Mean in IMU by three axial linear accelerations under the body coordinate system of accelerometer output, b _aThe constant value drift that means accelerometer, η _aThe noise that means accelerometer;

Mean in IMU by three axial instantaneous angular velocity of rotations under the body coordinate system of gyro output, b _ωThe constant value drift that means gyro, η _ωThe noise that means gyro.G means the Mars acceleration of gravity of geographic coordinate system.K means attitude motion 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, the kinetics equation of power descending branch (1) can be reduced to

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

In formula, f (X) is the non-linear continuous state transfer function of system, 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$ White Gaussian noise for zero-mean.

Wherein, in concrete enforcement, the initial value of gyro and accelerometer generally is made as zero, and error generally is made as respectively 5 * 10 ^-5Rad/s and 3 * 10 ^-3m/s ².

The measurement equation of step 2, Mars power descending branch

What be fixed on microsensor MCAV output on body coordinate system is the speed of height and three axles of detector, take the measurement equation of its metrical information as Foundation Mars power descending branch:

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

In formula,

H (X)=[r _zv ^b] ^T, (5) r _zFor the Z axis coordinate (being 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,

Mean that the detector body coordinate is tied to the transition matrix of Mars centered inertial system.B is normal value bias vector.Measure noise v _mFor the white Gaussian noise of zero-mean, and uncorrelated with system noise w.

Step 3, the above-mentioned kinetics equation of discretize (3) and measurement equation (4),

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

In formula, k=1,2,3 ..., F (X _k) be the nonlinear state transfer function of f (X) after discrete, Non-linear measurement function for h (X) after discrete, 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 above of second order, obtain linearization kinetics equation afterwards

$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 above of second order, obtain linearization measurement equation afterwards

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

In formula,

By said process, just obtained kinetics equation and the measurement equation after the linearization, U in formula _kAnd Y _kFor nonrandom outer effect.

Step 4, iteration SKF filtering algorithm and navigation information output

Because the normal value deviation b in formula (4) fails accurately to know, therefore Schmidt-Kalman filtering algorithm (Schmidt Kalman filter, be called for short SKF) on the basis of not estimating these deviations, consideration is dissolved into its variance in filtering algorithm, that is to say by considering the cross covariance of deviation and state, increase the precision of filtering.Simultaneously, due to what face, it is nonlinear filtering, when being carried out to Taylor series expansion, measurement equation can produce truncation error, therefore adopt the method for iteration to reduce non-linear measurement equation is carried out to the impact of the error of linearization generation on filtering, thereby reach the 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, be about to normal value bias vector b and add, 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, often be worth bias vector and satisfy condition: b _K+1=b _k, and its variance matrix B ₀Meet

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

Cross-covariance C with 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, State estimation for Kalman filtering k step.

The corresponding error covariance matrix walked with kinetics equation (15) and measurement equation (16) k

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)$

While starting filtering calculating, need init state vector sum error covariance matrix, and establish the vectorial X of being of init state ₀And error covariance matrix is P ₀.

2. time renewal process

State estimation by the k step can obtain, the state one-step prediction of k+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 k+1 step

For

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. measurement renewal process

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

2) calculate the filter gain matrix of i step

For

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

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) calculate i step measurement information residual error

3) calculate the state estimation of i step

${\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,

Use alternative manner to measure in renewal process, when the estimated value of gained state vector

(wherein threshold value is made as ε to meet the satisfied condition of 2 vectorial norms _Limit):

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

The time end loop calculate.

4. utilize the state estimation value measured in upgrading And corresponding parameter, calculate the estimation error variance matrix that k+1 walks For

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)$

Cross-covariance C with 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)$

By above 4 steps, loop the real-time status estimated value that can obtain Mars power descending branch detector, comprise position vector, velocity vector and the attitude angle of detector.The schematic diagram of iteration SKF filtering algorithm of the present invention is referring to Fig. 2.

The present invention is comprised of step 1, step 2, step 3 and four steps of step 4 altogether, by the tectonodynamics equation with set up the measurement equation of multi-source information integrated navigation, then utilize iteration SKF filtering algorithm to eliminate the impact of error in metrical information, and guarantee the stability of filtering algorithm, reach the efficient purpose of estimating in real time the detector navigational state.

Wherein, the Mars gravity acceleration g of listed geographic coordinate system in step 1, during due to descending branch, the detector distance martian surface is very near, therefore hypothesis g is constant, and is made as g=[0 0-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, while specifically calculating, adopt

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

Wherein, at " discretize kinetics equation (3) and measurement equation (4) " described in step 3, the method adopted is Taylor series expansion method.Taylor series are the power series expansions of an infinite function f (x) that can be micro-on mathematics:

$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! The factorial that means 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 be blocked, and only get finite term, therefore can produce corresponding truncation error.

Wherein, norm used in step 4 Chinese style (31) "

To calculate by following form: vector x=(x ₁, x ₂..., x _n) 2 norms be that in x, each element square sum is opened 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, only increased a little calculated amount, just by the information fusion of deviation in measurement equation in the estimation procedure to state vector, effectively revised the impact of deviation on filtering in the measurement equation, and utilize alternative manner, revised preferably the filtering error that the truncation error brought due to Taylor series is brought, improved navigation accuracy, strengthen the stability of filtering, thereby can estimate real-time and efficiently navigational state to detector.

Claims (5)

1. the iteration SKF method of Mars power descending branch multi-source information integrated navigation, it is characterized in that: the method step is as follows:

Step 1, utilize the kinetics equation of Mars power descending branch

The situation more complicated of Mars power descending branch, set up accurate kinetic model very difficult, on the basis of considering IMU output, utilizes the kinetics equation of its structure power 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=[x y z] ^TMean be connected position vector under being of landing point, v=[v _xv _yv _z] ^TMean be connected velocity vector under being of landing point, Ω=[σ θ ψ] ^TMean be connected attitude angle under being of landing point, controlled quentity controlled variable σ is the roll angle of detector, and θ is the longitude of detector, and ψ is the course angle of detector;

Mean that the Mars centered inertial is tied to the transition matrix of detector body coordinate system,

Mean that the Mars geographic coordinate is tied to the transition matrix of Mars centered inertial system;

Mean in IMU by three axial linear accelerations under the body coordinate system of accelerometer output, b _aThe constant value drift that means accelerometer, η _aThe noise that means accelerometer;

Mean in IMU by three axial instantaneous angular velocity of rotations under the body coordinate system of gyro output, b _ωThe constant value drift that means gyro, η _ωThe noise that means gyro; G means the Mars acceleration of gravity of geographic coordinate system; K means attitude motion 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, the kinetics equation of power descending branch (1) is reduced to

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

In formula, f (X) is the non-linear continuous state transfer function of system, 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$ White Gaussian noise for zero-mean;

The measurement equation of step 2, Mars power descending branch

What be fixed on microsensor MCAV output on body coordinate system is the speed of height and three axles of detector, take the measurement equation of its metrical information as Foundation Mars power descending branch:

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

In formula,

h(X)=[r _z v ^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,

Mean that the detector body coordinate is tied to the transition matrix of Mars centered inertial system, b is normal value bias vector, measures noise v _mFor the white Gaussian noise of zero-mean, and uncorrelated with system noise w;

Step 3, the above-mentioned kinetics equation of discretize (3) and measurement equation (4),

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

In formula, k=1,2,3 ..., F (X _k) be the nonlinear state transfer function of f (X) after discrete,

Non-linear measurement function for h (X) after discrete, 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 above of second order, obtain linearization kinetics equation afterwards

$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;F}_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 above of second order, obtain linearization measurement equation afterwards

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

In formula,

By said process, just obtained kinetics equation and the measurement equation after the linearization, U in formula _kAnd Y _kFor nonrandom outer effect;

Step 4, iteration SKF filtering algorithm and navigation information output

Because the normal value deviation b in formula (4) fails accurately to know, therefore Schmidt-Kalman filtering algorithm is that SKF is on the basis of not estimating these deviations, consideration is dissolved into its variance in filtering algorithm, that is to say by considering the cross covariance of deviation and state, increases the precision of filtering; Simultaneously, due to what face, it is nonlinear filtering, when being carried out to Taylor series expansion, measurement equation can produce truncation error, therefore adopt the method for iteration to reduce non-linear measurement equation is carried out to the impact of the error of linearization generation on filtering, thereby reach the minimizing Divergent Phenomenon, improve filtering accuracy, guarantee the numerical stability of filtering; Described iteration SKF filtering algorithm performing step is as follows:

1. be augmented state vector X _k, be about to normal value bias vector b and add, 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=[H_k,I]\left[\begin{array}{c}{X}_k\\ b_k\end{array}\right]+v_k---\left(16\right)$

In formula, often be worth bias vector and satisfy condition: b _K+1=b _k, and its variance matrix B ₀Meet

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

Cross-covariance C with 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,

State estimation for Kalman filtering k step;

The corresponding error covariance matrix walked with kinetics equation (15) and measurement equation (16) k 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)$

While starting filtering calculating, need init state vector sum error covariance matrix, and establish the vectorial X of being of init state ₀And error covariance matrix is P ₀

2. time renewal process

State estimation by the k step can obtain, the state one-step prediction of k+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 k+1 step

For

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. measurement renewal process

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

1) calculate the filter gain matrix of i step

For

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

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(27\right)$

2) calculate i step measurement information residual error

3) calculate the state estimation of i step

${\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,

Use alternative manner to measure in renewal process, when the estimated value of gained state vector

Meet that 2 norms of vector meet condition-wherein threshold value is made as ε _Limit:

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

The time end loop calculate;

4. utilize the state estimation value measured in upgrading

And corresponding parameter, calculate the estimation error variance matrix that k+1 walks

For

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)$

Cross-covariance C with 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)$

By above 4 steps, loop the real-time status estimated value that namely obtains Mars power descending branch detector, comprise position vector, velocity vector and the attitude angle of detector;

By above four steps, by the tectonodynamics equation with set up the measurement equation of multi-source information integrated navigation, then utilize iteration SKF filtering algorithm to eliminate the impact of error in metrical information, and guarantee the stability of filtering algorithm, reach the efficient purpose of estimating in real time the detector navigational state.

2. the iteration SKF method of a kind of Mars power descending branch multi-source information according to claim 1 integrated navigation, is characterized in that: step 2 Chinese style (6)

Detector body coordinate wherein is tied to the transition matrix of Mars centered inertial system

For Inverse matrix, while specifically calculating, adopt

3. the iteration SKF method of a kind of Mars power descending branch multi-source information according to claim 1 integrated navigation is characterized in that: in step 3 described " in formula, k=1,2,3; ... ", while generally calculating, the k value is k=1,2,3 ..., N, wherein N was determined by filtering time and sampling period; Such as when the filtering time, being 60 seconds, the sampling period is while being 1 hertz, N=60/1=60.

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

$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! The factorial that means 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 be blocked, and only get finite term, therefore can produce corresponding truncation error.

5. the iteration SKF method of a kind of Mars power descending branch multi-source information according to claim 1 integrated navigation, is characterized in that: step 4 Chinese style (31) norm used

To calculate by following form: vector x=(x ₁, x ₂..., x _n) 2 norms be that in x, each element square sum is opened 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)$

。

Images