CN103411614A - Iteration SKF (Schmidt Kalman Filter) method of multi-source information integrated navigation of Mars power descent stage - Google Patents
Iteration SKF (Schmidt Kalman Filter) method of multi-source information integrated navigation of Mars power descent stage Download PDFInfo
- Publication number
- CN103411614A CN103411614A CN2013103415473A CN201310341547A CN103411614A CN 103411614 A CN103411614 A CN 103411614A CN 2013103415473 A CN2013103415473 A CN 2013103415473A CN 201310341547 A CN201310341547 A CN 201310341547A CN 103411614 A CN103411614 A CN 103411614A
- Authority
- CN
- China
- Prior art keywords
- formula
- mars
- equation
- state
- filtering
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Landscapes
- Navigation (AREA)
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
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:
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
Getting state vector is X=[r
Tv
TΩ
T]
T, the kinetics equation of power descending branch (1) can be reduced to
In formula, f (X) is the non-linear continuous state transfer function of system, system noise
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
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,
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
In formula, often be worth bias vector and satisfy condition: b
K+1=b
k, and its variance matrix B
0Meet
B
0=Cov{b
0}=Cov{b
k} (17)
Cross-covariance C with deviation and state
kMeet
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
While starting filtering calculating, need init state vector sum error covariance matrix, and establish the vectorial X of being of init state
0And error covariance matrix is P
0.
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
Can obtain the one-step prediction varivance matrix P of state and deviation
K+1|kAnd C
K+1|k
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.
2) calculate i step measurement information residual error
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):
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
Cross-covariance C with deviation and state
K+1For
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:
In formula, n! The factorial that means n, and f
(n)(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
1, x
2..., x
n) 2 norms be that in x, each element square sum is opened radical sign again, namely
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:
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
Getting state vector is X=[r
Tv
TΩ
T]
T, the kinetics equation of power descending branch (1) can be reduced to
In formula, f (X) is the non-linear continuous state transfer function of system, system noise
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
2.
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
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
In formula,
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
In formula, often be worth bias vector and satisfy condition: b
K+1=b
k, and its variance matrix B
0Meet
B
0=Cov{b
0}=Cov{b
k} (17)
Cross-covariance C with deviation and state
kMeet
And initial value is C
0=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
While starting filtering calculating, need init state vector sum error covariance matrix, and establish the vectorial X of being of init state
0And error covariance matrix is P
0.
2. time renewal process
Can obtain the one-step prediction varivance matrix P of state and deviation
K+1|kAnd C
K+1|k
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.
Wherein, owing to not needing estimated bias, therefore the gain matrix of injunction bias term is zero.
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):
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
Cross-covariance C with deviation and state
K+1For
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:
In formula, n! The factorial that means n, and f
(n)(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
1, x
2..., x
n) 2 norms be that in x, each element square sum is opened radical sign again, namely
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:
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
Getting state vector is X=[r
Tv
TΩ
T]
T, the kinetics equation of power descending branch (1) is reduced to
In formula, f (X) is the non-linear continuous state transfer function of system, system noise
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
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
In formula,
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
In formula, often be worth bias vector and satisfy condition: b
K+1=b
k, and its variance matrix B
0Meet
B
0=Cov{b
0}=Cov{b
k} (17)
Cross-covariance C with deviation and state
kMeet
The corresponding error covariance matrix walked with kinetics equation (15) and measurement equation (16) k
For
While starting filtering calculating, need init state vector sum error covariance matrix, and establish the vectorial X of being of init state
0And error covariance matrix is P
0
2. time renewal process
Obtain the one-step prediction varivance matrix P of state and deviation
K+1|kAnd C
K+1|k
3. measurement renewal process
Use alternative manner to measure renewal: work as i=1,2,3 ... the time, carry out as follows cycle calculations:
Wherein, owing to not needing estimated bias, therefore the gain matrix of injunction bias term is zero;
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:
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
Cross-covariance C with deviation and state
K+1For
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:
In formula, n! The factorial that means n, and f
(n)(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
1, x
2..., x
n) 2 norms be that in x, each element square sum is opened radical sign again, namely
。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310341547.3A CN103411614B (en) | 2013-08-07 | 2013-08-07 | The iteration SKF method of Mars power dropping section multi-source information integrated navigation |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310341547.3A CN103411614B (en) | 2013-08-07 | 2013-08-07 | The iteration SKF method of Mars power dropping section multi-source information integrated navigation |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103411614A true CN103411614A (en) | 2013-11-27 |
CN103411614B CN103411614B (en) | 2015-11-18 |
Family
ID=49604642
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310341547.3A Expired - Fee Related CN103411614B (en) | 2013-08-07 | 2013-08-07 | The iteration SKF method of Mars power dropping section multi-source information integrated navigation |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103411614B (en) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103884333A (en) * | 2014-03-31 | 2014-06-25 | 北京控制工程研究所 | Autonomous navigation initial benchmark capturing method for detecting in deep space |
CN104266650A (en) * | 2014-10-25 | 2015-01-07 | 哈尔滨工业大学 | Method for atmospheric entry section navigation of mars lander based on sampling point inheritance strategy |
CN105300387A (en) * | 2015-11-03 | 2016-02-03 | 北京航空航天大学 | Nonlinear non-Gaussian ranking filtering method for Martian atmosphere entering section |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101173858A (en) * | 2007-07-03 | 2008-05-07 | 北京控制工程研究所 | Three-dimensional posture fixing and local locating method for lunar surface inspection prober |
US20100036612A1 (en) * | 2008-07-24 | 2010-02-11 | Vance Leonard D | System and method of passive and autonomous navigation of space vehicles using an extended kalman filter |
CN102168981A (en) * | 2011-01-13 | 2011-08-31 | 北京航空航天大学 | Independent celestial navigation method for Mars capturing section of deep space probe |
CN102168980A (en) * | 2011-01-13 | 2011-08-31 | 北京航空航天大学 | Independent celestial navigation method of deep space probe based on minor planet intersection |
CN102175241A (en) * | 2011-01-13 | 2011-09-07 | 北京航空航天大学 | Autonomous astronomical navigation method of Mars probe in cruise section |
-
2013
- 2013-08-07 CN CN201310341547.3A patent/CN103411614B/en not_active Expired - Fee Related
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101173858A (en) * | 2007-07-03 | 2008-05-07 | 北京控制工程研究所 | Three-dimensional posture fixing and local locating method for lunar surface inspection prober |
US20100036612A1 (en) * | 2008-07-24 | 2010-02-11 | Vance Leonard D | System and method of passive and autonomous navigation of space vehicles using an extended kalman filter |
CN102168981A (en) * | 2011-01-13 | 2011-08-31 | 北京航空航天大学 | Independent celestial navigation method for Mars capturing section of deep space probe |
CN102168980A (en) * | 2011-01-13 | 2011-08-31 | 北京航空航天大学 | Independent celestial navigation method of deep space probe based on minor planet intersection |
CN102175241A (en) * | 2011-01-13 | 2011-09-07 | 北京航空航天大学 | Autonomous astronomical navigation method of Mars probe in cruise section |
Non-Patent Citations (3)
Title |
---|
伍凯等: "基于多模型自适应估计的火星大气层进入段导航方法", 《中国宇航学会深空探测技术专业委员会第九届学术年会论文集》 * |
彭玉明等: "火星大气进入UKF导航算法", 《中国宇航学会深空探测技术专业委员会第七届学术年会论文集》 * |
陈晓等: "基于器间测量的火星进入过程实时高精度导航", 《航天返回与遥感》 * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103884333A (en) * | 2014-03-31 | 2014-06-25 | 北京控制工程研究所 | Autonomous navigation initial benchmark capturing method for detecting in deep space |
CN103884333B (en) * | 2014-03-31 | 2017-03-15 | 北京控制工程研究所 | A kind of survey of deep space independent navigation initial baseline catching method |
CN104266650A (en) * | 2014-10-25 | 2015-01-07 | 哈尔滨工业大学 | Method for atmospheric entry section navigation of mars lander based on sampling point inheritance strategy |
CN104266650B (en) * | 2014-10-25 | 2016-11-02 | 哈尔滨工业大学 | A kind of Mars landing device air approach section air navigation aid based on sampled point inheritance strategy |
CN105300387A (en) * | 2015-11-03 | 2016-02-03 | 北京航空航天大学 | Nonlinear non-Gaussian ranking filtering method for Martian atmosphere entering section |
CN105300387B (en) * | 2015-11-03 | 2018-04-10 | 北京航空航天大学 | A kind of martian atmosphere approach section nonlinear and non-Gaussian order filtering method |
Also Published As
Publication number | Publication date |
---|---|
CN103411614B (en) | 2015-11-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103776446B (en) | A kind of pedestrian's independent navigation computation based on double MEMS-IMU | |
Mulder et al. | Non-linear aircraft flight path reconstruction review and new advances | |
CN100587641C (en) | A kind of attitude determination system that is applicable to the arbitrary motion mini system | |
CN101706287B (en) | Rotating strapdown system on-site proving method based on digital high-passing filtering | |
CN101706284B (en) | Method for increasing position precision of optical fiber gyro strap-down inertial navigation system used by ship | |
CN103278163A (en) | Nonlinear-model-based SINS/DVL (strapdown inertial navigation system/doppler velocity log) integrated navigation method | |
CN103616030A (en) | Autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction | |
CN103674034B (en) | Multi-beam test the speed range finding revise robust navigation method | |
CN104132662A (en) | Closed-loop Kalman filter inertial positioning method based on zero velocity update | |
CN104655131A (en) | Initial inertial navigation alignment method based on terated strong tracking spherical simplex radial cubature Kalman filter (ISTSSRCKF) | |
CN103900574B (en) | Attitude estimation method based on iteration volume Kalman filter | |
CN102735267B (en) | Measuring method for inertial measurement device in sled testing | |
CN104075715A (en) | Underwater navigation and positioning method capable of combining terrain and environment characteristics | |
CN102508954A (en) | Full-digital simulation method and device for global positioning system (GPS)/strapdown inertial navigation system (SINS) combined navigation | |
CN105737823A (en) | GPS/SINS/CNS integrated navigation method based on five-order CKF | |
CN106767797A (en) | A kind of inertia based on dual quaterion/GPS Combinated navigation methods | |
CN103884340B (en) | A kind of information fusion air navigation aid of survey of deep space fixed point soft landing process | |
CN105091907A (en) | Estimation method of installation error of DVL direction in SINS and DVL combination | |
CN103591956B (en) | A kind of deep space probe autonomous navigation method based on Analysis on Observability | |
CN103438890B (en) | Based on the planetary power descending branch air navigation aid of TDS and image measurement | |
CN103076026A (en) | Method for determining speed measurement error of Doppler velocity log (DVL) in strapdown inertial navigation system | |
CN105005099A (en) | Atmospheric parameter calculation method based on strapdown inertial navigation and flight control system | |
CN104359496A (en) | High-precision attitude correction method based on vertical deviation compensation | |
CN107270937A (en) | A kind of offline wavelet de-noising Rapid Alignment Technology | |
CN103411614B (en) | The iteration SKF method of Mars power dropping section multi-source information integrated navigation |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20151118 Termination date: 20170807 |
|
CF01 | Termination of patent right due to non-payment of annual fee |