CN103884340A - Information fusion navigation method for detecting fixed-point soft landing process in deep space - Google Patents

Abstract

The invention relates to an information fusion navigation method for detecting a fixed-point soft landing process in a deep space. An autonomous navigation system of a lander is provided with an inertia measurement unit, a distance measurement sensor, a velocity measurement sensor, an optical imaging sensor and other navigation sensors, and on the basis, in the whole soft landing process, the angular velocity and the non-gravitation acceleration of the lander are measured by the inertia measurement unit, and integration and extrapolation are carried out to provide position and velocity information for the navigation system; slope distance information output by the distance measurement sensor, relative velocity information output by the velocity measurement sensor and feature point pixel coordinate information output by the optical imaging sensor are all processed by Kalman subfilters independent of one another, and the status and error covariance is locally estimated; the local estimation result is subjected to information fusion by a covariance crossing method to obtain the unified position and velocity estimated value of the lander at present, and the unified position and velocity estimated value is fed back to the subfilters for next navigation updating. The method is high in position and velocity estimation precision and good in real-time performance, has fault-tolerant capability and is easy to realize.

Description

A kind of information fusion air navigation aid of survey of deep space fixed point soft landing process

Technical field

The invention belongs to navigation field, relate to a kind of Spacecraft Autonomous Navigation method based on information fusion.

Background technology

Survey of deep space fixed point soft landing process time is short, and attitude track dynamic changes greatly, and environment not intellectual is strong, once and start and be just difficult to stop in the time of large celestial body soft landing, this has just proposed high requirement to navigational system in real-time, precision, fault-tolerant ability.

It is large that traditional navigate mode based on ground observing and controlling net exists deep space communication time delay, determines that the precision of relative position, speed is low, and the shortcoming of the detailed geography information of landing field can not be provided.Therefore, lander must rely on autonomous navigation system, obtains metrical information by all kinds of sensors, then by the navigation algorithm processing on lander, the attitude of independent real-time estimation self, position, speed and identification landing field obstacle.Because the state parameter that will estimate is many, therefore the configuration of the sensor of lander autonomous navigation system is relatively abundant, and typical scenario comprises Inertial Measurement Unit (containing gyro and accelerometer), the sensor of finding range, the sensor that tests the speed, optical imagery sensor.Inertial Measurement Unit is measured angular velocity and the non-gravitational acceleration of lander, can provide the Back ground Information of lander posture position speed by integration.Range finding sensor is measured the oblique distance of beam direction, thereby can determine the real-time height of lander.As long as test the speed, sensor has the not coplanar wave beam of 3 bundles just can determine the relative velocity of lander.Optical imagery sensor, to the known unique point imaging in celestial body surface, is processed the pixel coordinate of extract minutiae, thereby near obstacle landing point or hazardous location is carried out to imaging identification through image.Above-mentioned sensor, all effectively under prerequisite, can be greater than lander quantity of state dimension to be estimated for the metrical information dimension that navigational system provides, and therefore between different sensors, has in fact formed isomery backup.In engineering practice, in order further to ensure safety also may to increase redundancy backup, for example, be equipped with 2 distance measurement beams etc.Process the dependent metrical information of multi-source like this, navigation algorithm, except ensureing necessary navigation accuracy, also must possess good information fusion ability, fault-tolerant ability, and be easy to realize on star.

The air navigation aid that inertial navigation adds subsidiary correction is the comparatively safe simple soft landing air navigation aid of one.The method is depending on the inertial navigation result value of taking as the leading factor, in the time that the subsidiaries such as oblique distance, speed, unique point pixel coordinate are effective, this amount estimated value that it is provided with inertial navigation one by one or grouping is compared, and its difference is multiplied by coefficient permanent or matching and obtains the correction to inertial navigation result.Due to the correction factor independent design of subsidiary and there is no coupled relation, therefore the method is easy to design and application, even if single sensor failure does not affect the use that other sensor is measured yet.But the weak point of the method is to have sacrificed part navigation accuracy, the estimation providing is not optimal value.

A kind of centralized filtering method at EKF (the Extended Kalman Filter) algorithm of navigation field widespread use at present, can do optimum fusion processing to all navigation metrical informations, but the method need to be written as an equation by the measurement unification of different sensors.Its dimension of such equation is very high, thereby computation burden is very heavy, is unfavorable for the real time execution of filtering under current spaceborne computer performance condition.And the poor fault tolerance of the method, be unfavorable for fault diagnosis, be also unfavorable for processing the synchronousness problem of different sensors.

For overcoming the shortcoming of the centralized filtering Distributed filtering that has been born, this technology has developed more than 30 year so far, in numerous Distributed filtering methods, the federated filter (Federated Filter) that Carlson proposes only synthesizes the estimated information of subfilter.Subfilter is parallel construction form.Each subfilter algorithm adopts Kalman filtering algorithm, processes the metrical information of respective sensor.Federated filter is extensively paid attention to because calculated amount is little, information distribution mode flexible, information fault tolerance performance is good.But federated filter needs local state to estimate separate or known its correlativity, and very difficult as accurate Calculation local state estimation Relativity.

Summary of the invention

Technology of the present invention is dealt with problems and is: overcome the deficiencies in the prior art, a kind of information fusion air navigation aid of survey of deep space fixed point soft landing process is provided, its position and speed estimated accuracy is high, real-time good, possess fault-tolerant ability and easily realization.

Technical solution of the present invention is: a kind of information fusion air navigation aid of survey of deep space fixed point soft landing process, comprises the steps:

(1) using lander in the position r of celestial body centered inertial system and speed v as state variable X, $X=\left[\begin{array}{c}r\\ v\end{array}\right],$ The wherein estimation initial value of X

and corresponding error covariance initial value P ₀determine that by ground observing and controlling system track result provides the zero hour in landing;

(2) utilize the state of state estimation outside forecast current time lander in a moment in the inertial navigation system of installing on lander and corresponding error covariance,

${\hat{X}}_{k/k-1}={\hat{X}}_{k-1}+\left[\begin{array}{c}{\hat{v}}_{k-1}\mathrm{\Δt}+\frac{1}{2}{C}_{\mathrm{ib},k}{\hat{a}}_k\mathrm{\Δ}{t}^2-\frac{1}{2}\frac{\mathrm{\μ}}{{\left({\hat{r}}_{k-1}\right)}^3}{\hat{r}}_{k-1}\mathrm{\Δ}{t}^2\\ C_{\mathrm{ib},k}{\hat{a}}_k\mathrm{\Δt}-\frac{\mathrm{\μ}}{{\left({\hat{r}}_{k-1}\right)}^3}{\hat{r}}_{k-1}\mathrm{\Δt}\end{array}\right]$

P _k/k-1=Φ _k,k-1P _k-1(Φ _k,k-1) ^T+Q

${\mathrm{\Φ}}_{k,k-1}=I_6+\left[\begin{array}{cc}{0}_3& I_3\\ \frac{\∂(-\mathrm{\μr}/r^3)}{\∂r^T}{|}_{r={\hat{r}}_{k/k-1}}& 0_3\end{array}\right]\mathrm{\Δt}$

Wherein

for the status predication of k moment lander,

for the state estimation of k-1 moment lander,

with be respectively location estimation and the velocity estimation of k-1 moment lander, ${\hat{X}}_{k-1}=\left[\begin{array}{c}{\hat{r}}_{k-1}\\ {\hat{v}}_{k-1}\end{array}\right],$

for

mould, Δ t for navigation the update cycle, C _{ib, k}for k moment lander body is tied to the Direct cosine matrix that landing celestial body centered inertial is,

for the acceleration of accelerometer output in k moment inertance element, μ is landing celestial body gravitation constant, I ₆be 6 rank unit matrixs, I ₃be 3 rank unit matrixs, 0 ₃be 3 rank null matrix, symbol T representing matrix transposition, the mould that r is r, for the location status prediction of k moment lander, P _k/k-1for the error covariance prediction in k moment, Q is system noise variance battle array;

(3) judge whether the range finding sensor of installing on lander, the sensor that tests the speed, optical imagery sensor have exported new valid data, the number of remembering new valid data is n, if n=0, the directly output of the navigation results using the result of step (2) as current time, finishes and exits; If n is not 0, enter step (4);

(4), for each new valid data, all set up separately a sub-Kalman filter, and ask for the state estimation of each subcard Thalmann filter

and corresponding error covariance

${\hat{X}}_k^i={\hat{X}}_{k/k-1}+K_k^i[Z_k^i-h^i\left({\hat{X}}_{k/k-1}\right)]$

$P_k^i=(I-K_k^i{H}_k^i)P_{k/k-1}$

$K_k^i=P_{k/k-1}{\left(H_k^i\right)}^T{[H_k^i{P}_{k/k-1}{\left(H_k^i\right)}^T+R^i]}^{-1}$

$H_k^i=\frac{\∂h^i\left(X\right)}{\∂X}{|}_{X={\hat{X}}_{k,k-1}}$

Wherein h ⁱ(X) be i new valid data, i=1 ..., n, R ⁱbe the measurement noise variance matrix of i the corresponding sensor of new valid data,

be sensor that valid data that i is new are corresponding in the output in k moment, symbol-1 represents to ask inverse of a matrix;

(5) output of each subfilter in step (4) is carried out to information fusion, obtains navigation results the output of current time,

${\hat{X}}_k=P_k\left[\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ω}}_i{\left(P_k^i\right)}^{-1}{\hat{X}}_k^i\right]$

$P_k={\left[\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left(P_k^i\right)}^{-1}{\mathrm{\ω}}_i\right]}^{-1}$

Wherein

tr () represents to ask matrix trace, non-negative weighting coefficient and meet ω ₁+ ... + ω _n=1.

The present invention's advantage is compared with prior art:

(1) the information fusion air navigation aid of the survey of deep space fixed point soft landing process that the present invention proposes, compared with the air navigation aid that adds subsidiary correction with inertial navigation, due to the metrical informations such as oblique distance, speed, unique point coordinate have been carried out to unified information fusion, therefore the position and speed estimated accuracy of its acquisition is higher;

(2) the information fusion air navigation aid of the survey of deep space fixed point soft landing process that the present invention proposes, compared with air navigation aid based on EKF, belong to distributed frame, the equation dimension of single subfilter is low, be easy to spaceborne computer and calculate in real time, measure the information fusion that does not affect all the other measurements invalid in the situation that in part, therefore its fault-tolerant ability is strong, robustness is good, has improved the reliability of lander navigational system;

(3) the information fusion air navigation aid of the survey of deep space fixed point soft landing process that the present invention proposes, adopt distributed frame and the Kalman filtering of similar federated filter to calculate, utilize covariance bracketing method (Covariance Intersection) to merge and provide global optimum's estimation measurement data.But compared with air navigation aid based on federated filter, do not need to calculate the correlativity between multiple local estimation error that different measuring information provides, just increase the optimization computation process of an information matrix amplification process and a non-negative weighting coefficient, its estimator after merging is at least not less than the estimator that participates in merging at this side up available lowest accuracy in the precision of space direction, equation is simple, is easy to Project Realization.

Brief description of the drawings

Fig. 1 is the structure principle chart of distributed information fusion of the present invention;

Fig. 2 is the FB(flow block) of the inventive method;

Fig. 3 is the navigation position evaluated error schematic diagram in the embodiment of the present invention;

Fig. 4 is the navigation speed evaluated error schematic diagram in the embodiment of the present invention.

Embodiment

The information fusion air navigation aid that the present invention proposes adopts distributed filter structure, as shown in Figure 1.Survey of deep space fixed point soft landing task lander autonomous navigation system is equipped with the navigation sensors such as Inertial Measurement Unit (containing gyro and accelerometer), the sensor of finding range, the sensor that tests the speed, optical imagery sensor.In soft landing whole process, Inertial Measurement Unit is measured angular velocity and the non-gravitational acceleration of lander, postpones the position and speed Back ground Information that lander is provided for navigational system through integration outward, and this process is inertial navigation.After the condition of work that reaches each sensor until height, the attitude etc. of lander, range finding sensor output oblique distance information, the sensor that tests the speed output relative velocity, optical imagery sensor output characteristic point pixel coordinate information.These information are by separate Kalman's subfilter processing, give and do well and the partial estimation of error covariance.In each navigation cycle, the number of Kalman's subfilter is along with the number of effective measurement is dynamically adjusted.Partial estimation result is carried out information fusion by covariance bracketing method again, draws unified lander current location speed overall situation estimated value.So just complete one and take turns navigation renewal.

As shown in Figure 2, concrete steps are as follows for the flow process of the inventive method:

1, initialization:

The state variable X of navigation algorithm is taken as position r and the speed v of lander in celestial body centered inertial system (initial point is positioned at celestial body center, and three axles are parallel with equator, J2000 the earth's core inertial system), that is:

$X=\left[\begin{array}{c}r\\ v\end{array}\right]---\left(1\right)$

Landing the zero hour, determining that by ground observing and controlling system track result provides the state variable X estimation initial value of lander

and corresponding error covariance initial value P ₀.

2, measure validity judgement:

Secondly first judge whether sensor has exported latest data, judge that these data whether in effective range, for example, utilize minimax threshold value to judge.Only has the effective subfilter processing just entering below of measuring.If there be n measurement effectively will set up n subfilter below, with subscript, i marks, i=1 ..., n.

3, the time upgrades:

${\hat{X}}_{k/k-1}={\hat{X}}_{k-1}+\left[\begin{array}{c}{\hat{v}}_{k-1}\mathrm{\Δt}+\frac{1}{2}{C}_{\mathrm{ib},k}{\hat{a}}_k\mathrm{\Δ}{t}^2-\frac{1}{2}\frac{\mathrm{\μ}}{{\left({\hat{r}}_{k-1}\right)}^3}{\hat{r}}_{k-1}\mathrm{\Δ}{t}^2\\ C_{\mathrm{ib},k}{\hat{a}}_k\mathrm{\Δt}-\frac{\mathrm{\μ}}{{\left({\hat{r}}_{k-1}\right)}^3}{\hat{r}}_{k-1}\mathrm{\Δt}\end{array}\right]---\left(2\right)$

Wherein

for the status predication in k moment,

for the state estimation in k-1 moment,

with

be respectively location estimation and the velocity estimation of k-1 moment lander, ${\hat{X}}_{k-1}=\left[\begin{array}{c}{\hat{r}}_{k-1}\\ {\hat{v}}_{k-1}\end{array}\right],$

for

mould, Δ t for navigation the update cycle, C _{ib, k}for k moment lander body series (initial point is positioned at lander barycenter, and three axles are parallel with the lander principal axis of inertia) is to the Direct cosine matrix of celestial body centered inertial system,

for the acceleration of accelerometer output in k moment inertance element, μ is landing celestial body gravitation constant.

C _{ib, k}to utilize the angular velocity integration of gyro output in Inertial Measurement Unit to realize renewal, its computing method belong to the conventional content of inertial navigation, chapter 5 related content in " satellite orbit and attitude dynamics and control " that concrete formula can be write with reference to Zhang Renwei.

The implication of formula (2) is to utilize the state of inertial navigation by the state estimation outside forecast current time in a upper moment.

The kinetics equation of deep space soft landing process is nonlinear, therefore it is done to Taylor series expansion and gets linear term to carry out computing mode transition matrix as follows:

${\mathrm{\Φ}}_{k,k-1}=I_6+\left[\begin{array}{cc}{0}_3& I_3\\ \frac{\∂(-\mathrm{\μr}/r^3)}{\∂r^T}{|}_{r={\hat{r}}_{k/k-1}}& 0_3\end{array}\right]\mathrm{\Δt}---\left(3\right)$

Wherein I ₆be 6 rank unit matrixs, I ₃be 3 rank unit matrixs, 0 ₃be 3 rank null matrix, symbol T representing matrix transposition, the mould that r is r, for the status predication in k moment, in front 3 elements.

Utilize state-transition matrix to carry out time renewal to error covariance:

P _k/k-1=Φ _k,k-1P _k-1(Φ _k,k-1) ^T+Q (4)

Wherein P _k/k-1for the error covariance prediction in k moment; Q is system noise variance battle array, belongs to known parameters, and the system noise here refers to that all the other act on the resultant acceleration of lander power except the spherical gravitation of central body and thruster thrust.

4, inertial navigation output:

If in the 2nd step judgement, oblique distance, relative velocity, unique point pixel coordinate have one at least and effectively skip this step in measuring, directly carry out the 5th step.If measure void in whole, by the status predication that in the 3rd step, inertial navigation provides

with error covariance prediction P _k/k-1directly export as epicycle navigation result of calculation, and skip the 5th and 6 steps and directly finish epicycle navigation calculating, wait next round is navigated and is arrived computing time, again calculates since the 2nd step.

The formula of inertial navigation output is as follows

${\hat{X}}_k={\hat{X}}_{k/k-1}---\left(5\right)$

P _k=P _k/k-1 (6)

5, measure and upgrade:

Remember that i the measurement equation of effectively measuring is h ⁱ(X).

In the time being effectively measured as oblique distance, h ⁱ(X) expression formula is as follows:

$h^i\left(X\right)=s_1[(r-R)/(-\frac{r}{r}{C}_{\mathrm{ib},k}{A}_1)+b_1]---\left(7\right)$

Wherein s _lfor the constant multiplier of range finding sensor, r is the mould of position r, the reference radius that R is celestial body, A _lfor range finding sensor is at the installation matrix of lander body coordinate system, b _lfor the normal value deviation of range finding sensor.

In the time being effectively measured as relative velocity, h ⁱ(X) expression formula is as follows:

$h^i\left(X\right)=s_u[A_u{C}_{\mathrm{ib},k}^T(v-\mathrm{\ω}\×r)+b_u]---\left(8\right)$

Wherein s _ufor the constant multiplier of the sensor that tests the speed, A _ufor range finding sensor is at the installation matrix of lander body coordinate system, the spin velocity that ω is celestial body, b _ufor the normal value deviation of the sensor that tests the speed.

In the time being effectively measured as unique point pixel coordinate, h ⁱ(X) expression formula is as follows:

$h^i\left(X\right)=K_o{A}_o{C}_{\mathrm{ib},k}^T\frac{r-r_a}{\left|\right|r-r_a\left|\right|}---\left(9\right)$

Wherein K _ofor optical imagery sensor pixel coordinate conversion coefficient, A _ofor the installation matrix of optical imagery sensor under lander body coordinate system, r _afor the position of celestial body surface unique point, symbol || || represent to ask the mould of vector.

Due to h ⁱ(X) be the non-linear expressions of state variable, therefore ask local derviation to obtain the measurement matrix in linear k moment to it as follows:

$H_k^i=\frac{\∂h^i\left(X\right)}{\∂X}{|}_{X={\hat{X}}_{k,k-1}}---\left(10\right)$

Filter gain matrix by error covariance and i the subfilter in measurement matrix computations k moment is as follows:

$K_k^i=P_{k/k-1}{\left(H_k^i\right)}^T{[H_k^i{P}_{k/k-1}{\left(H_k^i\right)}^T+R^i]}^{-1}---\left(11\right)$

Wherein R ⁱbe i the measurement noise variance matrix of measuring, belong to known parameters, symbol-1 represents to ask inverse of a matrix.

By the measurement of k moment navigation sensor output

the state estimation of calculating subfilter is as follows:

${\hat{X}}_k^i={\hat{X}}_{k/k-1}+K_k^i[Z_k^i-h^i\left({\hat{X}}_{k/k-1}\right)]---\left(12\right)$

In formula (12)

part is called measurement residual error, this value premultiplication filter gain matrix

obtain state revision.

Error covariance by filter gain matrix and measurement matrix computations k moment subfilter is estimated as follows:

$P_k^i=(I-K_k^i{H}_k^i)P_{k/k-1}---\left(13\right)$

6, estimation fusion:

Calculate the mark of the error covariance matrix of subfilter:

${\mathrm{\ϵ}}_i=\mathrm{tr}\left(P_k^i\right)---\left(14\right)$

Wherein tr () represents to ask matrix trace.

Calculate the non-negative weighting coefficient of subfilter:

${\mathrm{\&omega;}}_i=\frac{1/{\mathrm{\&epsiv;}}_i}{\underset{j=1}{\overset{\mathrm{<figure-callout\; id="1"\; label="n"\; filenames="HDA0000485182110000012.png"\; state="\{\{state\}\}">n</figure-callout>}}{\mathrm{\&Sigma;}}}1/{\mathrm{\&epsiv;}}_j}---\left(15\right)$

Wherein non-negative weighting coefficient ω _imeet ω ₁+ ... + ω _n=1.

Calculate the error covariance after k time information merges:

$P_k={\left[\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\left(P_k^i\right)}^{-1}{\mathrm{\&omega;}}_i\right]}^{-1}---\left(16\right)$

Calculate the state estimation after k time information merges:

${\hat{X}}_k=P_k\left[\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}_i{\left(P_k^i\right)}^{-1}{\hat{X}}_k^i\right]---\left(17\right)$

This step is emphasis of the present invention, realizes information fusion with covariance bracketing method.

be the Output rusults in the k moment of navigational system.

In the time that navigation is upgraded next time, repeat above-mentioned steps, until navigation task finishes.

Embodiment

The preliminary orbit height of deep space fixed point soft landing is got 15km.Navigation initial position error is 1000m radially, normal direction-1000m, forward direction 1000m; Velocity error is radially 1m, normal direction-1m, forward direction 1m.Range finding scale factor error 0.1%, is often worth deviation 0.1m, measures noise 2%.The scale factor error 0.13% that tests the speed, is often worth deviation 0.01m/s, measures noise 0.16m/s.Road sign pixel coordinate error 0.1 pixel.0.2 °/h of Modelling of Random Drift of Gyroscopes, 0.2 °/h of constant value drift.Accelerometer measures noise 0.002m/s ².The work from power declines of optical guidance sensor, the sampling period is got 20s.Ranging and range rate sensor is introduced at 10km At The Height.

Soft landing process is navigated evaluated error respectively as shown in Figure 3 and Figure 4 in position and the speed of three directions in northeast, sky.As can be seen from Figure 3, after introducing road sign pixel coordinate metrical information, position estimation error is compared initial error remarkable correction, but Height Estimation error is still larger.Start to introduce range finding correction from 335s, Height Estimation error declines gradually, meets mission requirements to soft landing navigation position in latter stage precision.As can be seen from Figure 4, first speed estimation error constantly increases along with inertial navigation drift, starts that introducing has tested the speed is revised to 335s, and degree of precision is arrived in the of short duration concussion of process very rapid convergence, meets mission requirements to soft landing navigation speed in latter stage precision.This simulation results show the validity of the inventive method.

The content not being described in detail in instructions of the present invention belongs to those skilled in the art's known technology.

Claims (1)

1. an information fusion air navigation aid for survey of deep space fixed point soft landing process, is characterized in that comprising the steps:

(1) using lander in the position r of celestial body centered inertial system and speed v as state variable X, $X=\left[\begin{array}{c}r\\ v\end{array}\right],$ The wherein estimation initial value of X

and corresponding error covariance initial value P ₀determine that by ground observing and controlling system track result provides the zero hour in landing;

(2) utilize the state of state estimation outside forecast current time lander in a moment in the inertial navigation system of installing on lander and corresponding error covariance,

${\hat{X}}_{k/k-1}={\hat{X}}_{k-1}+\left[\begin{array}{c}{\hat{v}}_{k-1}\mathrm{\&Delta;t}+\frac{1}{2}{C}_{\mathrm{ib},k}{\hat{a}}_k\mathrm{\&Delta;}{t}^2-\frac{1}{2}\frac{\mathrm{\&mu;}}{{\left({\hat{r}}_{k-1}\right)}^3}{\hat{r}}_{k-1}\mathrm{\&Delta;}{t}^2\\ C_{\mathrm{ib},k}{\hat{a}}_k\mathrm{\&Delta;t}-\frac{\mathrm{\&mu;}}{{\left({\hat{r}}_{k-1}\right)}^3}{\hat{r}}_{k-1}\mathrm{\&Delta;t}\end{array}\right]$

P _k/k-1=Φ _k,k-1P _k-1(Φ _k,k-1) ^T+Q

${\mathrm{\&Phi;}}_{k,k-1}=I_6+\left[\begin{array}{cc}{0}_3& I_3\\ \frac{\&PartialD;(-\mathrm{\&mu;r}/r^3)}{\&PartialD;r^T}{|}_{r={\hat{r}}_{k/k-1}}& 0_3\end{array}\right]\mathrm{\&Delta;t}$

Wherein

for the status predication of k moment lander, for the state estimation of k-1 moment lander, with

be respectively location estimation and the velocity estimation of k-1 moment lander, ${\hat{X}}_{k-1}=\left[\begin{array}{c}{\hat{r}}_{k-1}\\ {\hat{v}}_{k-1}\end{array}\right],$

for

mould, Δ t for navigation the update cycle, C _{ib, k}for k moment lander body is tied to the Direct cosine matrix that landing celestial body centered inertial is,

for the acceleration of accelerometer output in k moment inertance element, μ is landing celestial body gravitation constant, I ₆be 6 rank unit matrixs, I ₃be 3 rank unit matrixs, 0 ₃be 3 rank null matrix, symbol T representing matrix transposition, the mould that r is r,

for the location status prediction of k moment lander, P _k/k-1for the error covariance prediction in k moment, Q is system noise variance battle array;

(3) judge whether the range finding sensor of installing on lander, the sensor that tests the speed, optical imagery sensor have exported new valid data, the number of remembering new valid data is n, if n=0, the directly output of the navigation results using the result of step (2) as current time, finishes and exits; If n is not 0, enter step (4);

(4), for each new valid data, all set up separately a sub-Kalman filter, and ask for the state estimation of each subcard Thalmann filter

and corresponding error covariance

${\hat{X}}_k^i={\hat{X}}_{k/k-1}+K_k^i[Z_k^i-h^i\left({\hat{X}}_{k/k-1}\right)]$

$P_k^i=(I-K_k^i{H}_k^i)P_{k/k-1}$

$K_k^i=P_{k/k-1}{\left(H_k^i\right)}^T{[H_k^i{P}_{k/k-1}{\left(H_k^i\right)}^T+R^i]}^{-1}$

$H_k^i=\frac{\&PartialD;h^i\left(X\right)}{\&PartialD;X}{|}_{X={\hat{X}}_{k,k-1}}$

Wherein h ⁱ(X) be i new valid data, i=1 ..., n, R ⁱbe the measurement noise variance matrix of i the corresponding sensor of new valid data,

be sensor that valid data that i is new are corresponding in the output in k moment, symbol-1 represents to ask inverse of a matrix;

(5) output of each subfilter in step (4) is carried out to information fusion, obtains navigation results the output of current time,

${\hat{X}}_k=P_k\left[\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}_i{\left(P_k^i\right)}^{-1}{\hat{X}}_k^i\right]$

$P_k={\left[\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\left(P_k^i\right)}^{-1}{\mathrm{\&omega;}}_i\right]}^{-1}$

Wherein

tr () represents to ask matrix trace, non-negative weighting coefficient and meet ω ₁+ ... + ω _n=1.

Images