CN104020671A - Robustness recursion filtering method for aircraft attitude estimation under the condition of measurement interference - Google Patents

Abstract

The invention belongs to the technical field of aircraft attitude estimation by use of a robustness filtering technology, and relates to a robustness recursion filtering method for aircraft attitude estimation under the condition of measurement interference. The method comprises: acquiring the output data of a gyro and a star sensor during the motion process of an aircraft; establishing an aircraft non-linear state space model based on attitude quaternion under the condition of measurement interference; updating time, and solving a step state prediction value and an upper bound of a prediction variance; performing robustness recursion filtering measurement updating, solving an optimum filtering gain, accordingly solving a state estimation value and the upper bound of the variance at k+1 time, and performing forced normalization constraining on a quaternion portion in state estimation at the k+1 time; and outputting the attitude quaternion and a gyro drifting result, and finishing attitude estimation. According to the invention, a robustness filtering design based on a minimum variance is employed, such that an optimum filtering gain design under the condition of minimum variance significance can be realized, and the attitude estimation precision of a system can be improved.

Description

A kind of robust recursive filtering method of estimating for attitude of flight vehicle under interference that measures

Technical field

The invention belongs to the technical field of utilizing robust filtering technology to carry out attitude of flight vehicle estimation, relate to a kind of robust recursive filtering method of estimating for attitude of flight vehicle under interference that measures.

Background technology

Posture estimation system by gyro and star sensor combination is widely used in aircraft because accuracy of attitude determination is high.Eulerian angle, correction Douglas Rodríguez parameter, direction cosine, hypercomplex number etc. are the main attitude characterising parameters of aircraft.Hypercomplex number is simple owing to calculating, and without the computing of trigonometric function, can avoid the singularity problem of Eulerian angle again simultaneously, and therefore, hypercomplex number is often used as the attitude characterising parameter in posture estimation system.Hypercomplex number attitude estimation model for this system, many based on EKF (Extended Kalman Filter, EKF) attitude method of estimation is suggested, as additivity EKF (Additive Extended Kalman Filter, AEKF) and the property taken advantage of EKF (Multiplicative Extended Kalman Filter, MEKF).Yet EKF is only suitable for the accurate known system model with additive noise.If there is the uncertain situation of model in system model, the performance of this algorithm will be subject to serious impact.Therefore, the uncertain nonlinear filtering algorithm of many band models is developed, as H _∞filtering, collection value nonlinear filtering, robust filtering design etc., wherein, robust filtering design based on minimum variance is proved to be a kind of effective processing means for resolution system band model uncertain condition, yet the robust filtering of great majority based on minimum variance is all only to have the uncertain situation of a kind of model for system.If in system, exist two kinds or two or more models uncertain, above-mentioned Robust filtering algorithms will lose efficacy.

For posture estimation system, the multiplicative noise of existence and white Gaussian noise coupling in system state equation, the Unknown Variance of this noise, therefore be regarded as a kind of state model uncertain, a kind of robust EKF (Robust Extended Kalman Filter, REKF) be suggested, but do not consider the uncertain situation of other models of posture estimation system.Except multiplicative noise, because aircraft exists shake or the impact of vibrating in operational process, unknown measurement is disturbed and will inevitably be appeared in posture estimation system, thereby has caused measuring error.Although this error in measurement can reduce the performance of posture estimation system and cause attitude information inaccurate, in attitude, estimate in filtering algorithm that research is seldom for the unknown situation about disturbing that measures of this band.There is research to point out this unknown measurement interference to be regarded as to the definite Gaussian noise of average and variance, yet in actual applications, this unknown prior imformation that measures interference cannot be known often, only can know its span, therefore, it should be to regard a kind of as to have the measurement model of scope uncertain.In order to improve estimated accuracy and the robustness of posture estimation system, be necessary that research exists multiplicative noise and the unknown robust filtering design problem disturbing in these two kinds of uncertain situations of model that measures in posture estimation system.

Summary of the invention

The object of the invention is, in order to improve the attitude precision of estimating and the robustness that strengthens system, to have proposed a kind of robust recursive filtering method (Robust Recursive Filter, RRF) of estimating for attitude of flight vehicle under interference that measures.

The object of the present invention is achieved like this:

(1) the output data of gyro and star sensor in collection aircraft movements process;

(2) set up and measure the aircraft Nonlinear state space model based on attitude quaternion under interference;

(2.1) set up the state equation of attitude of flight vehicle estimating system;

By attitude quaternion q _kwith gyroscopic drift β _kthe state variable that composition dimension is n $x_k={\left[\begin{array}{cc}{q}_k^T& {\mathrm{\β}}_k^T\end{array}\right]}^T,$ The flight state equation of foundation based on attitude quaternion is:

$x_{k+1}=f(x_k,{\widetilde{\mathrm{\ω}}}_k)+\underset{i=1}{\overset{s}{\mathrm{\Σ}}}{A}_{\mathrm{ik}}{\mathrm{\η}}_{\mathrm{ik}}{x}_k+w_k$

$f(x_k,{\widetilde{\mathrm{\ω}}}_k)=\left[\begin{array}{c}(\cos \left(\frac{\left|\right|{\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k\left|\right|\mathrm{\Δt}}{2}\right)I_{4\×4}+\sin \left(\frac{\left|\right|{\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k\left|\right|\mathrm{\Δt}}{2}\right)\frac{\mathrm{\Ω}({\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k)}{\left|\right|{\widetilde{\mathrm{\ω}}}_k-{\mathrm{\β}}_k\left|\right|})q_k\\ {\mathrm{\β}}_k\end{array}\right];$ Q _kfor k attitude quaternion constantly; β _kfor k gyroscopic drift value constantly; for k gyro outputting measurement value constantly; Δ t is the sampling time of gyro; || || for asking norm symbol; A=[a ₁a ₂a ₃] ^tfor trivector, $\mathrm{\Ω}\left(a\right)=\left[\begin{array}{cc}-[a\×]& a\\ -a^T& 0\end{array}\right],[a\×]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right];w_k=\left[\begin{array}{c}{0}_{4\×1}\\ {\mathrm{\η}}_u\end{array}\right]$ Additive noise, variance $Q_k=E\left(w_k{w}_k^T\right)=\left[\begin{array}{cc}{0}_{4\×4}& 0_{4\×3}\\ 0_{3\×4}& \mathrm{\Δt}{\mathrm{\σ}}_u^2{I}_{3\×3}\end{array}\right],$ η _ufor noise, η are measured in gyroscopic drift _uvariance be s=3; η _ikthat average is zero, the white Gaussian noise that variance is 1; A _ikmatrix for appropriate dimension:

$A_{\mathrm{ik}}=-\frac{\mathrm{\Δt}{\mathrm{\σ}}_v}{2}\left[\begin{array}{cc}{A}_{\mathrm{ik}}^1& 0_{4\×3}\\ 0_{3\×4}& 0_{3\×3}\end{array}\right]$

$A_{1k}^1=\left[\begin{array}{cccc}0& 0& 0& 1\\ 0& 0& 1& 0\\ 0& -1& 0& 0\\ -1& 0& 0& 0\end{array}\right];A_{2k}^1=\left[\begin{array}{cccc}0& 0& -1& 0\\ 0& 0& 0& 1\\ 1& 0& 0& 0\\ 0& -1& 0& 0\end{array}\right];A_{3k}^1=\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& -1& 0\end{array}\right];$ σ _vfor gyro to measure noise;

(2.2) set up and measure the measurement equation that disturbs lower attitude of flight vehicle estimating system;

Measure the measurement model that disturbs lower star sensor:

for k moment star sensor measuring value; reference vector for star sensor; I is the number that star sensor observes fixed star; for the white Gaussian noise of zero-mean, variance be for vector is disturbed in unknown measurement; A(q _k) be hypercomplex number q _kattitude Description Matrix, $q_k={\left[\begin{array}{cc}{\mathrm{\ρ}}_k^T& q_4\end{array}\right]}^T,$ A(q _k) be:

$A\left(q_k\right)=(q_4^2-{{\mathrm{\ρ}}_k}^T{\mathrm{\ρ}}_k)I_{3\×3}+2{\mathrm{\ρ}}_k{{\mathrm{\ρ}}_k}^T-2q_4[{\mathrm{\ρ}}_k\×]$

Unknown measurement interference matrix is converted into:

$M_i=\left[\begin{array}{cccccc}0& 0& {\mathrm{\σ}}_{\mathrm{iy}}& 0& {\mathrm{\σ}}_{\mathrm{iz}}& 0\\ {\mathrm{\σ}}_{\mathrm{ix}}& 0& 0& 0& 0& {\mathrm{\σ}}_{\mathrm{iz}}\\ 0& {\mathrm{\σ}}_{\mathrm{ix}}& 0& {\mathrm{\σ}}_{\mathrm{iy}}& 0& 0\end{array}\right];$ Δ _i＝diag([Δ _ix Δ _ix Δ _iy Δ _iy Δ _iz Δ _iz])； $E_i={\left[\begin{array}{cccccc}0& 0& 0& 1& 0& -1\\ 0& -1& 0& 0& 1& 0\\ 1& 0& -1& 0& 0& 0\end{array}\right]}^T;$ σ _ij, j=x, y, z is known normal number;

Adopt the measurement data of 3 stars, i.e. i=3, the measurement equation of attitude of flight vehicle estimating system is set up as:

z _k＝h(x _k)+MΔg(x _k)+v _k

$z_k=\left[\begin{array}{c}{z}_k^1\\ z_k^2\\ z_k^3\end{array}\right]$ The measuring value of etching system during for k, dimension is m; $h\left(x_k\right)=\left[\begin{array}{c}A\left(q_k\right){\stackrel{\→}{r}}^1\\ A\left(q_k\right){\stackrel{\→}{r}}^2\\ A\left(q_k\right){\stackrel{\→}{r}}^3\end{array}\right]$ For measuring nonlinear function; g(x _k)=Eh (x _k); $M=\left[\begin{array}{ccc}{M}_1& & \\ & M_2& \\ & & M_3\end{array}\right];E=\left[\begin{array}{ccc}{E}_1& & \\ & E_2& \\ & & E_3\end{array}\right];\mathrm{\Δ}=\left[\begin{array}{ccc}{\mathrm{\Δ}}_1& & \\ & {\mathrm{\Δ}}_2& \\ & & {\mathrm{\Δ}}_3\end{array}\right],$ Δ Δ ^t≤ I _{18 * 18}; v _kzero-mean white Gaussian noise, v _kvariance be

(3) carry out time renewal, try to achieve the upper bound of a step status predication and prediction variance;

K state estimation value is constantly the variance upper bound is Σ _k, try to achieve a step status predication value and be:

${\hat{x}}_{k+1|k}=f({\hat{x}}_k,{\widetilde{\mathrm{\ω}}}_k)$

One step status predication error is:

${\tilde{x}}_{k+1|k}=x_{k+1}-{\hat{x}}_{k+1|k}=f(x_k,{\widetilde{\mathrm{\ω}}}_k)-f({\hat{x}}_k,{\widetilde{\mathrm{\ω}}}_k)+\underset{i=1}{\overset{s}{\mathrm{\Σ}}}{A}_{\mathrm{ik}}{\mathrm{\η}}_{\mathrm{ik}}{x}_k+w_k$

Will place carries out Taylor expansion to be had:

$f(x_k,{\widetilde{\mathrm{\ω}}}_k)=f({\hat{x}}_k,{\widetilde{\mathrm{\ω}}}_k)+F_k{\tilde{x}}_k+A_k{\mathrm{\β}}_k{L}_k{\tilde{x}}_k$

a _kfor known ratio matrix; β _kfor unknown matrix, meet l _kadjusting matrix for known, is made as conventionally

One step status predication error distortions is:

${\tilde{x}}_{k+1|k}=(F_k+A_k{\mathrm{\β}}_k{L}_k){\tilde{x}}_k+\underset{i=1}{\overset{s}{\mathrm{\Σ}}}{A}_{\mathrm{ik}}{\mathrm{\η}}_{\mathrm{ik}}{x}_k+w_k$

One step status predication variance is:

$\begin{array}{c}{P}_{k+1|k}=E\left[(F_k+A_k{\mathrm{\β}}_k{L}_k){\tilde{x}}_k{\tilde{x}}_k^T{(F_k+A_k{\mathrm{\β}}_k{L}_k)}^T\right]+\underset{i=1}{\overset{s}{\mathrm{\Σ}}}{A}_{\mathrm{ik}}E\left(x_k{x}_k^T\right)A_{\mathrm{ik}}^T+Q_k\\ =(F_k+A_k{\mathrm{\β}}_k{L}_k){\mathrm{\Σ}}_k{(F_k+A_k{\mathrm{\β}}_k{L}_k)}^T+\underset{i=1}{\overset{s}{\mathrm{\Σ}}}{A}_{\mathrm{ik}}E\left(x_k{x}_k^T\right)A_{\mathrm{ik}}^T+Q_k\\ \≤F_k{({\mathrm{\Σ}}_k^{-1}-{\mathrm{\λL}}_k^T{L}_k)}^{-1}{F}_k^T+{\mathrm{\λ}}^{-1}{A}_k{A}_k^T+\underset{i=1}{\overset{s}{\mathrm{\Σ}}}{A}_{\mathrm{ik}}[(1+\mathrm{\ϵ}){\mathrm{\Σ}}_k+(1+{\mathrm{\ϵ}}^{-1}){\hat{x}}_k{\hat{x}}_k^T]A_{\mathrm{ik}}^T+Q_k\end{array}$

ε and λ are known positive number, and λ meets

${\mathrm{\λ}}^{-1}{I}_{n\×n}-L_k{\mathrm{\Σ}}_k{L}_k^T>0;$

The upper bound of one step status predication variance is:

${\mathrm{\Σ}}_{k+1|k}=F_k{({\mathrm{\Σ}}_k^{-1}-\mathrm{\λ}{L}_k^T{L}_k)}^{-1}{F}_k^T+{\mathrm{\λ}}^{-1}{A}_k{A}_k^T+\underset{i=1}{\overset{s}{\mathrm{\Σ}}}{A}_{\mathrm{ik}}[(1+\mathrm{\ϵ}){\mathrm{\Σ}}_k+(1+{\mathrm{\ϵ}}^{-1}){\hat{x}}_k{\hat{x}}_k^T]A_{\mathrm{ik}}^T+Q_k$

(4) carry out robust Recursive Filtering and measure renewal, try to achieve optimum filter gain, and then obtain the state estimation value in the k+1 moment and the upper bound of variance, hypercomplex number in k+1 state estimation constantly is partly carried out to compulsory normalization constraint;

One step measurement predictor is:

${\hat{z}}_{k+1}=h\left({\hat{x}}_{k+1|k}\right)$

One step measures predicated error:

${\tilde{z}}_{k+1}=z_{k+1}-h\left({\hat{x}}_{k+1|k}\right)=h\left(x_{k+1}\right)-h\left({\hat{x}}_{k+1|k}\right)+\mathrm{M\Δg}\left(x_{k+1}\right)+v_{k+1}$

By h (x _k+1) place carries out Taylor expansion to be had:

$h\left(x_{k+1}\right)=h\left({\hat{x}}_{k+1|k}\right)+H_{k+1}{\tilde{x}}_{k+1|k}+C_{k+1}{\mathrm{\α}}_{k+1}{L}_{k+1}{\tilde{x}}_{k+1|k}$

In formula, $H_{k+1}=\∂h\left(x_{k+1}\right)/\∂x_{k+1}{|}_{x_{k+1}={\hat{x}}_{k+1|k}};$ C _k+1for known ratio matrix; α _k+1for unknown matrix, meet

One step measures predicated error and can be deformed into:

${\tilde{z}}_{k+1}=(H_{k+1}+C_{k+1}{\mathrm{\α}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}+\mathrm{M\Δg}\left(x_{k+1}\right)+v_{k+1}$

K+1 state estimation is constantly:

${\hat{x}}_{k+1}={\hat{x}}_{k+1|k}+K_{k+1}{\tilde{z}}_{k+1}$

K _k+1for filter gain to be asked;

K+1 evaluated error is constantly:

${\tilde{x}}_{k+1}=x_{k+1}-{\hat{x}}_{k+1}=(I_{n\×n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\α}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}-K_{k+1}[\mathrm{M\Δg}\left(x_k\right)+v_{k+1}]$

K+1 evaluated error covariance matrix is constantly:

$\begin{array}{c}{P}_{k+1}=E\left[{\tilde{x}}_{k+1}{\tilde{x}}_{k+1}^T\right]=(I_{n\×n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\α}}_{k+1}{L}_{k+1}){\mathrm{\Σ}}_{k+1|k}{(I_{n\×n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\α}}_{k+1}{L}_{k+1})}^T\\ +E\left[K_{k+1}\mathrm{M\Δg}\left(x_{k+1}\right)g^T\left(x_{k+1}\right){\mathrm{\Δ}}^T{M}^T{K}_{k+1}^T\right]+K_{k+1}{R}_{k+1}{K}_{k+1}^T\\ -E\left[(I_{n\×n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\α}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{g}^T\left(x_{k+1}\right){\mathrm{\Δ}}^T{M}^T{K}_{k+1}^T\right]\\ -E\left[K_{k+1}\mathrm{M\Δg}\left(x_{k+1}\right){\tilde{x}}_{k+1|k}^T{(I_{n\×n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\α}}_{k+1}{L}_{k+1})}^T\right]\end{array}$

$\begin{array}{c}-(I_{n\×n}-K_{k+1}{H}_{k+1}-k_{k+1}{C}_{k+1}{\mathrm{\α}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{g}^T\left(x_{k+1}\right){\mathrm{\Δ}}^T{M}^T{K}_{k+1}^T\\ -K_{k+1}\mathrm{M\Δg}\left(x_{k+1}\right){\tilde{x}}_{k+1|k}^T{(I_{n\×n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\α}}_{k+1}{L}_{k+1})}^T\\ \≤{\mathrm{\ϵ}}_1(I_{n\×n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\α}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{\tilde{x}}_{k+1|k}^T{(I_{n\×n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\α}}_{k+1}{L}_{k+1})}^T\\ +{{\mathrm{\ϵ}}_1}^{-1}{K}_{k+1}\mathrm{M\Δg}\left(x_{k+1}\right)g^T\left(x_{k+1}\right){\mathrm{\Δ}}^T{M}^T{K}_{k+1}^T\end{array}$

Evaluated error covariance matrix is deformed into:

$\begin{array}{c}{P}_{k+1}\≤(1+{\mathrm{\ϵ}}_1)(I_{n\×n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\α}}_{k+1}{L}_{k+1}){\mathrm{\Σ}}_{k+1|k}{(I_{n\×n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\α}}_{k+1}{L}_{k+1})}^T\\ +(1+{{\mathrm{\ϵ}}_1}^{-1})K_{k+1}\mathrm{ME}\left[\mathrm{\Δg}\left(x_{k+1}\right)g^T\left(x_{k+1}\right){\mathrm{\Δ}}^T\right]M^T{K}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\\ \≤(1+{\mathrm{\ϵ}}_1)[(I_{n\×n}-K_{k+1}{H}_{k+1}){({\mathrm{\Σ}}_{k+1|k}^{-1}-{\mathrm{\μL}}_{k+1}^T{L}_{k+1})}^{-1}{(I_{n\×n}-K_{k+1}{H}_{k+1})}^T+{\mathrm{\μ}}^{-1}{K}_{k+1}{C}_{k+1}{C}_{k+1}^T{K}_{k+1}^T]\\ +(1+{{\mathrm{\ϵ}}_1}^{-1})K_{k+1}{\mathrm{MM}}^T{K}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\end{array}$

μ is known positive number, meets ${\mathrm{\μ}}^{-1}{I}_{n\×n}-L_{k+1}{P}_{k+1|k}{L}_{k+1}^T>0;$

The upper bound of k+1 evaluated error is constantly:

$\begin{array}{c}{\mathrm{\Σ}}_{k+1}=(1+{\mathrm{\ϵ}}_1)[(I_{n\×n}-K_{k+1}{H}_{k+1}){({\mathrm{\Σ}}_{k+1|k}^{-1}-{\mathrm{\μL}}_{k+1}^T{L}_{k+1})}^{-1}{(I_{n\×n}-K_{k+1}{H}_{k+1})}^T+{\mathrm{\μ}}^{-1}{K}_{k+1}{C}_{k+1}{C}_{k+1}^T{K}_{k+1}^T]\\ +(1+{{\mathrm{\ϵ}}_1}^{-1})K_{k+1}{\mathrm{MM}}^T{k}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\end{array}$

Order trying to achieve optimal filtering gain is:

$\begin{array}{c}{K}_{k+1}=(1+{\mathrm{\ϵ}}_1){({\mathrm{\Σ}}_{k+1|k}^{-1}-{\mathrm{\μL}}_{k+1}^T{L}_{k+1})}^{-1}{H}_{k+1}^T\\ \×\{(1+{\mathrm{\ϵ}}_1)H_{k+1}{({\mathrm{\Σ}}_{k+1|k}^{-1}-{\mathrm{\μL}}_{k+1}^T{L}_{k+1})}^{-1}{H}_{k+1}^T+{\mathrm{\μ}}^{-1}{C}_{k+1}{C}_{k+1}^T+(1+{{\mathrm{\ϵ}}_1}^{-1}){\mathrm{MM}}^T+R_{k+1}{\}}^{-1}\end{array}$

Obtain k+1 state estimation value constantly upper bound Σ with variance _k+1;

|| q _k|| ₂=1, then by k+1 state estimation value constantly in hypercomplex number be partly normalized constraint;

(5) be M the working time of posture estimation system, if k=M exports the result of attitude quaternion and gyroscopic drift, completes attitude and estimate; If k < is M, represent that filtering does not complete, repeating step (3) is to step (4), until posture estimation system end of run.

The middle gyro to measure noise criteria of step (2) is poor is σ _v=2.6875 * 10 ^-7rad/s ^1/2, gyroscopic drift noise criteria is poor is σ _u=8.9289 * 10 ^-10rad/s ^3/2, the sampling period of gyro is Δ t=0.25s; It is σ that star sensor is measured noise criteria poor _s=18 ", output frequency is 1Hz; The reference vector of star sensor is made as ${\stackrel{\&RightArrow;}{r}}^1={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^T,$ ${\stackrel{\&RightArrow;}{r}}^2={\left[\begin{array}{ccc}0& 1& 0\end{array}\right]}^T$ and ${\stackrel{\&RightArrow;}{r}}^3={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T;$ Initial gyroscopic drift β=[0.1 0.1 0.1] ^t°/h; σ _ij=10 ", j=x, y, z;

In step (3), Initial state estimation value is ${\hat{x}}_{0|0}={\left[\begin{array}{ccccccc}0& 0& 0& 1& 0& 0& 0\end{array}\right]}^T;$ Initial variance battle array is made as P _0|0=diag ([(0.1 °) ²(0.1 °) ²(0.1 °) ²(0.1 °) ²(0.2 °/h) ²(0.2 °/h) ²(0.2 °/h) ²]; A _k=0 _{n * n}; For λ=0.0001, ε=0.1.

C in step (4) _k+1=0 _{n * n}; μ=0.0001; ε ₁=0.1.

M=1000 in step (5).

Beneficial effect of the present invention is:

(1) the present invention considers that nonlinear attitude estimating system state model exists multiplicative noise and measurement model to have the situation of unknown disturbances, is conducive to improve the robustness of system simultaneously;

(2) the present invention adopts the robust filtering design based on minimum variance, can realize optimal filtering gain design under minimum variance meaning, has the attitude estimated accuracy of utilizing raising system.

Accompanying drawing explanation

Fig. 1 is an independent experiment attitude quaternion error result figure of existing AEKF algorithm;

Fig. 2 is an independent experiment attitude quaternion error result figure of existing REKF algorithm;

Fig. 3 is an independent experiment attitude quaternion error result figure of the RRF algorithm of invention;

Fig. 4 is RRF algorithm and the AEKF of invention, the attitude angle root-mean-square error result comparison diagram of REKF algorithm;

Fig. 5 is method flow diagram of the present invention.

Embodiment

Below in conjunction with drawings and Examples, the present invention is described in further detail.

The present invention proposes, and a kind of to measure the robust recursive filtering method of estimating for attitude of flight vehicle under interference be to carry out emulation experiment by Matlab simulation software, compare with the estimated performance of existing filtering algorithm, as additivity EKF (AEKF) and robust EKF (REKF).Simulation hardware environment is Intel (R) Core (TM) i5-2410M CPU2.30GHz, 4G RAM, Windows7 operating system.If Fig. 1 is to as shown in Fig. 3, solid line represents hypercomplex number evaluated error, the root mean square of corresponding element in dotted line Representative errors covariance matrix, significantly can find out, the error of the quaternionic vector part of AEKF and REKF algorithm exceeds the scope of calculating variance, the hypercomplex number evaluated error of RRF algorithm of the present invention all, in calculating the scope of variance, illustrates that RRF algorithm has good robustness.This is because AEKF algorithm and REKF algorithm are not all considered unknown measurement to be disturbed, and RRF algorithm has compensated multiplicative noise in design process and unknown measurement disturbed these two kinds of uncertain impacts of model.From the root-mean-square error shown in Fig. 4, can find out relatively, under measuring and disturbing, RRF algorithm has higher estimated accuracy than AEKF algorithm and REKF algorithm.

。The present invention considers that nonlinear attitude estimating system state model exists multiplicative noise and measurement model to have the situation of unknown disturbances, set up the attitude of flight vehicle estimation model under two kinds of uncertain existence of model, the structure of while based on EKF, utilize robust Recursive Design to find the upper bound scope of predicting variance and estimation variance, the filter gain of recycling minimum variance Theoretical Design optimum.

Measure a robust recursive filtering method of estimating for attitude of flight vehicle under interference, flow process as shown in Figure 5, comprises following step:

Step 1: the output data of gyro and star sensor in collection aircraft movements process;

Step 2: set up and measure the aircraft Nonlinear state space model based on attitude quaternion under interference;

Step 2.1 is set up the state equation of attitude of flight vehicle estimating system;

By attitude quaternion q _kwith gyroscopic drift β _kthe state variable that composition dimension is n $x_k={\left[\begin{array}{cc}{q}_k^T& {\mathrm{\&beta;}}_k^T\end{array}\right]}^T,$ According to the kinematical equation of aircraft, the flight state equation of setting up based on attitude quaternion is:

$x_{k+1}=f(x_k,{\widetilde{\mathrm{\&omega;}}}_k)+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}{\mathrm{\&eta;}}_{\mathrm{ik}}{x}_k+w_k---\left(1\right)$

In formula: $f(x_k,{\widetilde{\mathrm{\&omega;}}}_k)=\left[\begin{array}{c}(\cos \left(\frac{\left|\right|{\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k\left|\right|\mathrm{\&Delta;t}}{2}\right)I_{4\&times;4}+\sin \left(\frac{\left|\right|{\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k\left|\right|\mathrm{\&Delta;t}}{2}\right)\frac{\mathrm{\&Omega;}({\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k)}{\left|\right|{\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k\left|\right|})q_k\\ {\mathrm{\&beta;}}_k\end{array}\right];$ Q _kfor k attitude quaternion constantly; β _kfor k gyroscopic drift value constantly; for k gyro outputting measurement value constantly; Δ t is the sampling time of gyro; || || for asking norm symbol; Suppose a=[a ₁a ₂a ₃] ^tfor trivector, $\mathrm{\&Omega;}\left(a\right)=\left[\begin{array}{cc}-[a\&times;]& a\\ -a^T& 0\end{array}\right],[a\&times;]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right];$ $w_k=\left[\begin{array}{c}{0}_{4\&times;1}\\ {\mathrm{\&eta;}}_u\end{array}\right]$ Additive noise, variance $Q_k=E\left(w_k{w}_k^T\right)=\left[\begin{array}{cc}{0}_{4\&times;4}& 0_{4\&times;3}\\ 0_{3\&times;4}& \mathrm{\&Delta;t}{\mathrm{\&sigma;}}_u^2{I}_{3\&times;3}\end{array}\right],$ η _ufor gyroscopic drift, measure noise, its variance is s=3; η _ikthat average is zero, the white Gaussian noise that variance is 1; A _ikmatrix for appropriate dimension, can be represented as:

$A_{\mathrm{ik}}=-\frac{\mathrm{\&Delta;t}{\mathrm{\&sigma;}}_v}{2}\left[\begin{array}{cc}{A}_{\mathrm{ik}}^1& 0_{4\&times;3}\\ 0_{3\&times;4}& 0_{3\&times;3}\end{array}\right]---\left(2\right)$

In formula: $A_{1k}^1=\left[\begin{array}{cccc}0& 0& 0& 1\\ 0& 0& 1& 0\\ 0& -1& 0& 0\\ -1& 0& 0& 0\end{array}\right];A_{2k}^1=\left[\begin{array}{cccc}0& 0& -1& 0\\ 0& 0& 0& 1\\ 1& 0& 0& 0\\ 0& -1& 0& 0\end{array}\right];A_{3k}^1=\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& -1& 0\end{array}\right];$ σ _vfor gyro to measure noise;

Step 2.2 is set up and is measured the measurement equation that disturbs lower attitude of flight vehicle estimating system;

Measure and disturb the measurement model of lower star sensor to be:

In formula, for k moment star sensor measuring value; reference vector for star sensor; I is the number that star sensor observes fixed star; for the white Gaussian noise of zero-mean, its variance is for vector is disturbed in unknown measurement; A(q _k) be hypercomplex number q _kattitude Description Matrix, if $q_k={\left[\begin{array}{cc}{\mathrm{\&rho;}}_k^T& q_4\end{array}\right]}^T,$ A(q _k) can be represented as:

$A\left(q_k\right)=(q_4^2-{{\mathrm{\&rho;}}_k}^T{\mathrm{\&rho;}}_k)I_{3\&times;3}+2{\mathrm{\&rho;}}_k{{\mathrm{\&rho;}}_k}^T-2q_4[{\mathrm{\&rho;}}_k\&times;]---\left(4\right)$

Unknown measurement interference matrix is converted into:

In formula, $M_i=\left[\begin{array}{cccccc}0& 0& {\mathrm{\&sigma;}}_{\mathrm{iy}}& 0& {\mathrm{\&sigma;}}_{\mathrm{iz}}& 0\\ {\mathrm{\&sigma;}}_{\mathrm{ix}}& 0& 0& 0& 0& {\mathrm{\&sigma;}}_{\mathrm{iz}}\\ 0& {\mathrm{\&sigma;}}_{\mathrm{ix}}& 0& {\mathrm{\&sigma;}}_{\mathrm{iy}}& 0& 0\end{array}\right];$ Δ _i＝diag([Δ _ix Δ _ix Δ _iy Δ _iy Δ _iz Δ _iz])； $E_i={\left[\begin{array}{cccccc}0& 0& 0& 1& 0& -1\\ 0& -1& 0& 0& 1& 0\\ 1& 0& -1& 0& 0& 0\end{array}\right]}^T;$ σ _ij, j=x, y, z is known normal number;

Due to the normal measurement data that adopts 3 stars in engineering, i.e. i=3, the measurement equation of attitude of flight vehicle estimating system is set up as:

z _k＝h(x _k)+MΔg(x _k)+v _k (6)

In formula, $z_k=\left[\begin{array}{c}{z}_k^1\\ z_k^2\\ z_k^3\end{array}\right]$ The measuring value of etching system during for k, its dimension is m; $h\left(x_k\right)=\left[\begin{array}{c}A\left(q_k\right){\stackrel{\&RightArrow;}{r}}^1\\ A\left(q_k\right){\stackrel{\&RightArrow;}{r}}^2\\ A\left(q_k\right){\stackrel{\&RightArrow;}{r}}^3\end{array}\right]$ For measuring nonlinear function; g(x _k)=Eh (x _k); $M=\left[\begin{array}{ccc}{M}_1& & \\ & M_2& \\ & & M_3\end{array}\right];E=\left[\begin{array}{ccc}{E}_1& & \\ & E_2& \\ & & E_3\end{array}\right];\mathrm{\&Delta;}=\left[\begin{array}{ccc}{\mathrm{\&Delta;}}_1& & \\ & {\mathrm{\&Delta;}}_2& \\ & & {\mathrm{\&Delta;}}_3\end{array}\right],$ Δ Δ ^t≤ I _{188 * 8}; v _kbe zero-mean white Gaussian noise, its variance is

By the measurement equation shown in the state equation shown in formula (1) and formula (6), just formed and measured the aircraft Nonlinear state space model based on attitude quaternion under interference;

Step 3: the flight state spatial model under disturbing for above-mentioned measurement, the state estimation value in the known k moment and the upper bound of variance, take EKF as structural framing, utilizes the design of robust Recursive Filtering, the time of carrying out renewal, the upper bound of trying to achieve a step status predication and predicting variance;

Suppose that k state estimation value is constantly the variance upper bound is ∑ _k, according to EKF, try to achieve a step status predication value and be:

${\hat{x}}_{k+1|k}=f({\hat{x}}_k,{\widetilde{\mathrm{\&omega;}}}_k)---\left(7\right)$

One step status predication error is:

${\tilde{x}}_{k+1|k}=x_{k+1}-{\hat{x}}_{k+1|k}=f(x_k,{\widetilde{\mathrm{\&omega;}}}_k)-f({\hat{x}}_k,{\widetilde{\mathrm{\&omega;}}}_k)+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}{\mathrm{\&eta;}}_{\mathrm{ik}}{x}_k+w_k---\left(8\right)$

Will place carries out Taylor expansion to be had:

$f(x_k,{\widetilde{\mathrm{\&omega;}}}_k)=f({\hat{x}}_k,{\widetilde{\mathrm{\&omega;}}}_k)+F_k{\tilde{x}}_k+A_k{\mathrm{\&beta;}}_k{L}_k{\tilde{x}}_k---\left(9\right)$

In formula, a _kfor known ratio matrix; β _kfor unknown matrix, meet l _kadjusting matrix for known, is made as conventionally

Formula (9) is updated in formula (8), and a step status predication error distortions is:

${\tilde{x}}_{k+1|k}=(F_k+A_k{\mathrm{\&beta;}}_k{L}_k){\tilde{x}}_k+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}{\mathrm{\&eta;}}_{\mathrm{ik}}{x}_k+w_k---\left(10\right)$

One step status predication variance is:

$\begin{array}{c}{P}_{k+1|k}=E\left[(F_k+A_k{\mathrm{\&beta;}}_k{L}_k){\tilde{x}}_k{\tilde{x}}_k^T{(F_k+A_k{\mathrm{\&beta;}}_k{L}_k)}^T\right]+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}E\left(x_k{x}_k^T\right)A_{\mathrm{ik}}^T+Q_k\\ =(F_k+A_k{\mathrm{\&beta;}}_k{L}_k){\mathrm{\&Sigma;}}_k{(F_k+A_k{\mathrm{\&beta;}}_k{L}_k)}^T+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}+E\left(x_k{x}_k^T\right)A_{\mathrm{ik}}^T{+Q}_k\\ \&le;F_k{({\mathrm{\&Sigma;}}_k^{-1}-\mathrm{\&lambda;}{L}_k^T{L}_k)}^{-1}{F}_k^T+{\mathrm{\&lambda;}}^{-1}{A}_k{A}_k^T+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}[(1+\mathrm{\&epsiv;}){\mathrm{\&Sigma;}}_k+(1+{\mathrm{\&epsiv;}}^{-1}){\hat{x}}_k{\hat{x}}_k^T]A_{\mathrm{ik}}^T+Q_k\end{array}---\left(11\right)$

In formula, ε and λ are known positive number, and λ meets

According to formula (11), can obtain, the upper bound of a step status predication variance is:

${\mathrm{\&Sigma;}}_{k+1|k}=F_k{({\mathrm{\&Sigma;}}_k^{-1}-\mathrm{\&lambda;}{L}_k^T{L}_k)}^{-1}{F}_k^T+{\mathrm{\&lambda;}}^{-1}{A}_k{A}_k^T+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}[(1+\mathrm{\&epsiv;}){\mathrm{\&Sigma;}}_k+(1+{\mathrm{\&epsiv;}}^{-1}){\hat{x}}_k{\hat{x}}_k^T]A_{\mathrm{ik}}^T+Q_k---\left(12\right)$

Step 4: the upper bound that utilizes an above-mentioned step status predication of trying to achieve and prediction variance, carry out robust Recursive Filtering and measure renewal, try to achieve optimum filter gain, and then obtain state estimation value constantly of k+1 and the upper bound of variance, due to hypercomplex number normalization constraint, hypercomplex number in k+1 state estimation constantly is partly carried out to compulsory normalization constraint simultaneously;

Known according to formula (6), a step measurement predictor is:

${\hat{z}}_{k+1}=h\left({\hat{x}}_{k+1|k}\right)---\left(13\right)$

One step measures predicated error:

${\tilde{z}}_{k+1}=z_{k+1}-h\left({\hat{x}}_{k+1|k}\right)=h\left(x_{k+1}\right)-h\left({\hat{x}}_{k+1|k}\right)+\mathrm{M\&Delta;g}\left(x_{k+1}\right)+v_{k+1}---\left(14\right)$

By h (x _k+1) place carries out Taylor expansion to be had:

$h\left(x_{k+1}\right)=h\left({\hat{x}}_{k+1|k}\right)+H_{k+1}{\tilde{x}}_{k+1|k}+C_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}{\tilde{x}}_{k+1|k}---\left(15\right)$

In formula, c _k+1for known ratio matrix; α _k+1for unknown matrix, meet

Formula (15) is updated in formula (14), and a step measures predicated error and can be deformed into:

${\tilde{z}}_{k+1}=(H_{k+1}+C_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}+\mathrm{M\&Delta;g}\left(x_{k+1}\right)+v_{k+1}---\left(16\right)$

Theoretical according to EKF, k+1 state estimation is constantly:

${\hat{x}}_{k+1}={\hat{x}}_{k+1|k}+K_{k+1}{\tilde{z}}_{k+1}---\left(17\right)$

In formula, k _k+1for filter gain to be asked;

K+1 evaluated error constantly can be expressed as:

${\tilde{x}}_{k+1}=x_{k+1}-{\hat{x}}_{k+1}=(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}-K_{k+1}[\mathrm{M\&Delta;g}\left(x_k\right)+v_{k+1}]---\left(18\right)$

K+1 evaluated error covariance matrix is constantly:

$P_{k+1}=E\left[{\tilde{x}}_{k+1}{\tilde{x}}_{k+1}^T\right]=\begin{array}{c}(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\mathrm{\&Sigma;}}_{k+1|k}{(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1})}^T\\ +E\left[K_{k+1}\mathrm{M\&Delta;g}{\left(x_{k+1}\right)}_{}{g}^T\left(x_{k+1}\right){\mathrm{\&Delta;}}^T{M}^T{K}_{k+1}^T\right]+K_{k+1}{R}_{k+1}{K}_{k+1}^T\\ -E\left[(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{g}^T\left(x_{k+1}\right){\mathrm{\&Delta;}}^T{M}^T{K}_{k+1}^T\right]\\ -E\left[K_{k+1}\mathrm{M\&Delta;g}\left(x_{k+1}\right){\tilde{x}}_{k+1|k}^T{(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1})}^T\right]\end{array}---\left(19\right)$

If ε ₁for little positive number, following inequality is permanent to be set up:

$\begin{array}{c}-(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{g}^T\left(x_{k+1}\right){\mathrm{\&Delta;}}^T{M}^T{K}_{k+1}^T\\ -K_{k+1}\mathrm{M\&Delta;g}\left(x_{k+1}\right){\tilde{x}}_{k+1|k}^T{(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1})}^T\\ \&le;{\mathrm{\&epsiv;}}_1(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{{\tilde{x}}_{k+1|k}^T(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1})}^T\\ +{{\mathrm{\&epsiv;}}_1}^{-1}{K}_{k+1}\mathrm{M\&Delta;g}\left(x_{k+1}\right){g^T\left(x_{k+1}\right)\mathrm{\&Delta;}}^T{M}^T{K}_{k+1}^T\end{array}---\left(20\right)$

By in formula (20) substitution formula (19), evaluated error covariance matrix is deformed into:

$\begin{array}{c}{P}_{k+1}\&le;(1+{\mathrm{\&epsiv;}}_1)(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\mathrm{\&Sigma;}}_{k+1|k}{(I_{n\&times;n}-k_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1})}^T\\ +(1+{{\mathrm{\&epsiv;}}_1}^{-1})K_{k+1}\mathrm{ME}\left[\mathrm{\&Delta;g}\left(x_{k+1}\right)g^T\left(x_{k+1}\right){\mathrm{\&Delta;}}^T\right]M^T{K}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\\ \&le;(1+{\mathrm{\&epsiv;}}_1)[(I_{n\&times;n}-K_{k+1}{H}_{k+1}){({\mathrm{\&Sigma;}}_{k+1|k}^{-1}-\mathrm{\&mu;}{L}_{k+1}^T{L}_{k+1})}^{-1}{(I_{n\&times;n}-K_{k+1}{H}_{k+1})}^T+{\mathrm{\&mu;}}^{-1}{K}_{k+1}{C}_{k+1}{C}_{k+1}^T{K}_{k+1}^T]\\ +(1+{{\mathrm{\&epsiv;}}_1}^{-1})K_{k+1}{\mathrm{MM}}^T{K}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\end{array}---\left(21\right)$

In formula, μ is known positive number, meets

Therefore, the upper bound of k+1 evaluated error is constantly:

$\begin{array}{c}{\mathrm{\&Sigma;}}_{k+1}=(1+{\mathrm{\&epsiv;}}_1)[(I_{n\&times;n}-K_{k+1}{H}_{k+1}){({\mathrm{\&Sigma;}}_{k+1|k}^{-1}-\mathrm{\&mu;}{L}_{k+1}^T{L}_{k+1})}^{-1}{(I_{n\&times;n}-K_{k+1}{H}_{k+1})}^T+{\mathrm{\&mu;}}^{-1}{K}_{k+1}{C}_{k+1}{C}_{k+1}^T{K}_{k+1}^T]\\ +(1+{{\mathrm{\&epsiv;}}_1}^{-1})K_{k+1}{\mathrm{MM}}^T{K}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\end{array}---\left(22\right)$

Utilize minimum variance theoretical, order trying to achieve optimal filtering gain is:

$\begin{array}{c}{K}_{k+1}=(1+{\mathrm{\&epsiv;}}_1){({\mathrm{\&Sigma;}}_{k+1|k}^{-1}-\mathrm{\&mu;}{L}_{k+1}^T{L}_{k+1})}^{-1}{H}_{k+1}^T\\ \&times;{\{(1+{\mathrm{\&epsiv;}}_1)H_{k+1}{({\mathrm{\&Sigma;}}_{k+1|k}^{-1}-\mathrm{\&mu;}{L}_{k+1}^T{L}_{k+1})}^{-1}{H}_{k+1}^T+{\mathrm{\&mu;}}^{-1}{C}_{k+1}{C}_{k+1}^T+(1+{{\mathrm{\&epsiv;}}_1}^{-1}){\mathrm{MM}}^T+R_{k+1}\}}^{-1}\end{array}---\left(23\right)$

Finally above-mentioned optimal filtering gain of trying to achieve is updated in formula (17) and formula (22), obtains k+1 state estimation value constantly the upper bound with variance

Because state variable hypercomplex number partly meets hypercomplex number normalization constraint || q _k|| ₂=1, therefore again by k+1 state estimation value constantly in hypercomplex number be partly normalized constraint;

Step 5: be M the working time of posture estimation system, if k=M exports the result of attitude quaternion and gyroscopic drift, completes attitude and estimates; If k < is M, represent that filtering does not complete, repeating step three is to step 4, until posture estimation system end of run.

Beneficial effect of the present invention is described as follows:

MATLAB emulation experiment, under following simulated conditions, the method is carried out to emulation experiment:

Gyro to measure noise criteria is poor is σ _v=2.6875 * 10 ^-7rad/s ^1/2, gyroscopic drift noise criteria is poor is σ _u=8.9289 * 10 ^-10rad/s ^3/2, the sampling period of gyro is Δ t=0.25s; It is σ that star sensor is measured noise criteria poor _s=18 ", output frequency is 1Hz; The reference vector of star sensor is made as ${\stackrel{\&RightArrow;}{r}}^1\text{=}{\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^T,{\stackrel{\&RightArrow;}{r}}^2={\left[\begin{array}{ccc}0& 1& 0\end{array}\right]}^T\mathrm{and}{\stackrel{\&RightArrow;}{r}}^3={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T;$ Initial gyroscopic drift β=[0.1 0.1 0.1] ^t°/h; Initial state estimation value is ${\hat{x}}_{0|0}={\left[\begin{array}{ccccccc}0& 0& 0& 1& 0& 0& 0\end{array}\right]}^T;$ Initial variance battle array is made as P _0|0=diag ([(0.1 °) ²(0.1 °) ²(0.1 °) ²(0.1 °) ²(0.2 °/h) ²(0.2 °/h) ²(0.2 °/h) ²]; Ignore the linearizing higher order term of nonlinear function, A is set _k=0 _{n * n}, C _k+1=0 _{m * n}; In order to ensure estimated accuracy, λ=μ=0.0001 is set, ε=ε ₁=0.1; σ _ij=10 ", j=x, y, z; Simulation time is made as 1000s.

Claims (5)

1. measure a robust recursive filtering method of estimating for attitude of flight vehicle under interference, it is characterized in that:

(1) the output data of gyro and star sensor in collection aircraft movements process;

(2) set up and measure the aircraft Nonlinear state space model based on attitude quaternion under interference;

(2.1) set up the state equation of attitude of flight vehicle estimating system;

By attitude quaternion q _kwith gyroscopic drift β _kthe state variable that composition dimension is n $x_k={\left[\begin{array}{cc}{q}_k^T& {\mathrm{\&beta;}}_k^T\end{array}\right]}^T,$ The flight state equation of foundation based on attitude quaternion is:

$x_{k+1}=f(x_k,{\widetilde{\mathrm{\&omega;}}}_k)+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}{\mathrm{\&eta;}}_{\mathrm{ik}}{x}_k+w_k$

$f(x_k,{\widetilde{\mathrm{\&omega;}}}_k)=\left[\begin{array}{c}(\cos \left(\frac{\left|\right|{\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k\left|\right|\mathrm{\&Delta;t}}{2}\right)I_{4\&times;4}+\sin \left(\frac{\left|\right|{\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k\left|\right|\mathrm{\&Delta;t}}{2}\right)\frac{\mathrm{\&Omega;}({\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k)}{\left|\right|{\widetilde{\mathrm{\&omega;}}}_k-{\mathrm{\&beta;}}_k\left|\right|})q_k\\ {\mathrm{\&beta;}}_k\end{array}\right];$ Q _kfor k attitude quaternion constantly; β _kfor k gyroscopic drift value constantly; for k gyro outputting measurement value constantly; Δ t is the sampling time of gyro; || || for asking norm symbol; A=[a ₁a ₂a ₃] ^tfor trivector, $\mathrm{\&Omega;}\left(a\right)=\left[\begin{array}{cc}-[a\&times;]& a\\ -a^T& 0\end{array}\right],[a\&times;]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& {-a}_1\\ -a_2& a_1& 0\end{array}\right];w_k=\left[\begin{array}{c}{0}_{4\&times;1}\\ {\mathrm{\&eta;}}_u\end{array}\right]$ Additive noise, variance $Q_k=E\left(w_k{w}_k^T\right)=\left[\begin{array}{cc}{0}_{4\&times;4}& 0_{4\&times;3}\\ 0_{3\&times;4}& \mathrm{\&Delta;t}{\mathrm{\&sigma;}}_u^2{I}_{3\&times;3}\end{array}\right],$ η _ufor noise, η are measured in gyroscopic drift _uvariance be s=3: η _ikthat average is zero, the white Gaussian noise that variance is 1; A _ikmatrix for appropriate dimension:

$A_{\mathrm{ik}}=-\frac{\mathrm{\&Delta;t}{\mathrm{\&sigma;}}_v}{2}\left[\begin{array}{cc}{A}_{\mathrm{ik}}^1& 0_{4\&times;3}\\ 0_{3\&times;4}& 0_{3\&times;3}\end{array}\right]$

$A_{1k}^1=\left[\begin{array}{cccc}0& 0& 0& 1\\ 0& 0& 1& 0\\ 0& -1& 0& 0\\ -1& 0& 0& 0\end{array}\right];A_{2k}^1=\left[\begin{array}{cccc}0& 0& -1& 0\\ 0& 0& 0& 1\\ 1& 0& 0& 0\\ 0& -1& 0& 0\end{array}\right];A_{3k}^1=\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& -1& 0\end{array}\right];$ σ _vfor gyro to measure noise;

(2.2) set up and measure the measurement equation that disturbs lower attitude of flight vehicle estimating system;

Measure the measurement model that disturbs lower star sensor:

for k moment star sensor measuring value; reference vector for star sensor; I is the number that star sensor observes fixed star; for the white Gaussian noise of zero-mean, variance be for vector is disturbed in unknown measurement; A(q _k) be hypercomplex number q _kattitude Description Matrix, $q_k={\left[\begin{array}{cc}{\mathrm{\&rho;}}_k^T& q_4\end{array}\right]}^T,$ A(q _k) be:

$A\left(q_k\right)=(q_4^2-{\mathrm{\&rho;}}_k^T{\mathrm{\&rho;}}_k)I_{3\&times;3}+2{\mathrm{\&rho;}}_k{{\mathrm{\&rho;}}_k}^T-2q_4[{\mathrm{\&rho;}}_k\&times;]$

Unknown measurement interference matrix is converted into:

$M_i=\left[\begin{array}{cccccc}0& 0& {\mathrm{\&sigma;}}_{\mathrm{iy}}& 0& {\mathrm{\&sigma;}}_{\mathrm{iz}}& 0\\ {\mathrm{\&sigma;}}_{\mathrm{ix}}& 0& 0& 0& 0& {\mathrm{\&sigma;}}_{\mathrm{iz}}\\ 0& {\mathrm{\&sigma;}}_{\mathrm{ix}}& 0& {\mathrm{\&sigma;}}_{\mathrm{iy}}& 0& 0\end{array}\right];$ Δ _i＝diag([Δ _ix Δ _ix Δ _iy Δ _iy Δ _iz Δ _iz])； j＝x，y，z； $E_i={\left[\begin{array}{cccccc}0& 0& 0& 1& 0& -1\\ 0& -1& 0& 0& 1& 0\\ 1& 0& -1& 0& 0& 0\end{array}\right]}^T;$ σ _ij, j=x, y, z is known normal number;

Adopt the measurement data of 3 stars, i.e. i=3, the measurement equation of attitude of flight vehicle estimating system is set up as:

z _k＝h(x _k)+MΔg(x _k)+v _k

$z_k=\left[\begin{array}{c}{z}_k^1\\ z_k^2\\ z_k^3\end{array}\right]$ The measuring value of etching system during for k, dimension is m; $h\left(x_k\right)=\left[\begin{array}{c}A\left(q_k\right){\stackrel{\&RightArrow;}{r}}^1\\ A\left(q_k\right){\stackrel{\&RightArrow;}{r}}^2\\ A\left(q_k\right){\stackrel{\&RightArrow;}{r}}^3\end{array}\right]$ For measuring nonlinear function; g(x _k)=Eh (x _k); $M=\left[\begin{array}{ccc}{M}_1& & \\ & M_2& \\ & & M_3\end{array}\right];E=\left[\begin{array}{ccc}{E}_1& & \\ & E_2& \\ & & E_3\end{array}\right];\mathrm{\&Delta;}=\left[\begin{array}{ccc}{\mathrm{\&Delta;}}_1& & \\ & {\mathrm{\&Delta;}}_2& \\ & & {\mathrm{\&Delta;}}_3\end{array}\right],$ Δ Δ ^t≤ I _{18 * 18}; v _kzero-mean white Gaussian noise, v _kvariance be

(3) carry out time renewal, try to achieve the upper bound of a step status predication and prediction variance;

K state estimation value is constantly the variance upper bound is Σ _k, try to achieve a step status predication value and be:

${\hat{x}}_{k+1|k}=f({\hat{x}}_k,{\widetilde{\mathrm{\&omega;}}}_k)$

One step status predication error is:

${\tilde{x}}_{k+1|k}=x_{k+1}-{\hat{x}}_{k+1|k}=f(x_k,{\widetilde{\mathrm{\&omega;}}}_k)-f({\hat{x}}_k,{\widetilde{\mathrm{\&omega;}}}_k)+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}{\mathrm{\&eta;}}_{\mathrm{ik}}{x}_k+w_k$

Will place carries out Taylor expansion to be had:

$f(x_k,{\widetilde{\mathrm{\&omega;}}}_k)=f({\hat{x}}_k,{\widetilde{\mathrm{\&omega;}}}_k)+F_k{\tilde{x}}_k+A_k{\mathrm{\&beta;}}_k{L}_k{\tilde{x}}_k$

a _kfor known ratio matrix; β _kfor unknown matrix, meet l _kadjusting matrix for known, is made as conventionally

One step status predication error distortions is:

${\tilde{x}}_{k+1|k}=(F_k+A_k{\mathrm{\&beta;}}_k{L}_k){\tilde{x}}_k+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}{\mathrm{\&eta;}}_{\mathrm{ik}}{x}_k+w_k$

One step status predication variance is:

$\begin{array}{c}{P}_{k+1|k}=E\left[(F_k+A_k{\mathrm{\&beta;}}_k{L}_k){\tilde{x}}_k{\tilde{x}}_k^T{(F_k+A_k{\mathrm{\&beta;}}_k{L}_k)}^T\right]+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}E\left(x_k{x}_k^T\right)A_{\mathrm{ik}}^T+Q_k\\ =(F_k+A_k{\mathrm{\&beta;}}_k{L}_k){\mathrm{\&Sigma;}}_k{(F_k+A_k{\mathrm{\&beta;}}_k{L}_k)}^T+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}E\left(x_k{x}_k^T\right)A_{\mathrm{ik}}^T+Q_k\\ \&le;F_k{({\mathrm{\&Sigma;}}_k^{-1}-\mathrm{\&lambda;}{L}_k^T{L}_k)}^{-1}{F}_k^T+{\mathrm{\&lambda;}}^{-1}{A}_k{A}_k^T+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}[(1+\mathrm{\&epsiv;}){\mathrm{\&Sigma;}}_k+(1+{\mathrm{\&epsiv;}}^{-1}){\hat{x}}_k{\hat{x}}_k^T]A_{\mathrm{ik}}^T+Q_k\end{array}$

ε and λ are known positive number, and λ meets

${\mathrm{\&lambda;}}^{-1}{I}_{n\&times;n}-L_k{\mathrm{\&Sigma;}}_k{L}_k^T>0;$

The upper bound of one step status predication variance is:

${\mathrm{\&Sigma;}}_{k+1|k}=F_k{({\mathrm{\&Sigma;}}_k^{-1}-\mathrm{\&lambda;}{L}_k^T{L}_k)}^{-1}{F}_k^T+{\mathrm{\&lambda;}}^{-1}{A}_k{A}_k^T+\underset{i=1}{\overset{s}{\mathrm{\&Sigma;}}}{A}_{\mathrm{ik}}[(1+\mathrm{\&epsiv;}){\mathrm{\&Sigma;}}_k+(1+{\mathrm{\&epsiv;}}^{-1}){\hat{x}}_k{\hat{x}}_k^T]A_{\mathrm{ik}}^T+Q_k$

(4) carry out robust Recursive Filtering and measure renewal, try to achieve optimum filter gain, and then obtain the state estimation value in the k+1 moment and the upper bound of variance, hypercomplex number in k+1 state estimation constantly is partly carried out to compulsory normalization constraint;

One step measurement predictor is:

${\hat{z}}_{k+1}=h\left({\hat{x}}_{k+1|k}\right)$

One step measures predicated error:

${\tilde{z}}_{k+1}=z_{k+1}-h\left({\hat{x}}_{k+1|k}\right)=h\left(x_{k+1}\right)-h\left({\hat{x}}_{k+1|k}\right)+\mathrm{M\&Delta;g}\left(x_{k+1}\right)+v_{k+1}$

By h (x _k+1) place carries out Taylor expansion to be had:

$h\left(x_{k+1}\right)=h\left({\hat{x}}_{k+1|k}\right)+H_{k+1}{\tilde{x}}_{k+1|k}+C_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}{\tilde{x}}_{k+1|k}$

In formula, c _k+1for known ratio matrix; α _k+1for unknown matrix, meet

One step measures predicated error and can be deformed into:

${\tilde{z}}_{k+1}=(H_{k+1}+C_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}+\mathrm{M\&Delta;g}\left(x_{k+1}\right)+v_{k+1}$

K+1 state estimation is constantly:

${\hat{x}}_{k+1}={\hat{x}}_{k+1|k}+K_{k+1}{\tilde{z}}_{k+1}$

K _k+1for filter gain to be asked;

K+1 evaluated error is constantly:

${\tilde{x}}_{k+1}=x_{k+1}-{\hat{x}}_{k+1}=(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}-K_{k+1}[\mathrm{M\&Delta;g}\left(x_k\right)+v_{k+1}]$

K+1 evaluated error covariance matrix is constantly:

$\begin{array}{c}{P}_{k+1}=E\left[{\tilde{x}}_{k+1}{\tilde{x}}_{k+1}^T\right]=(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\mathrm{\&Sigma;}}_{k+1|k}{(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1})}^T\\ +E\left[K_{k+1}\mathrm{M\&Delta;g}\left(x_{k+1}\right)g^T\left(x_{k+1}\right){\mathrm{\&Delta;}}^T{M}^T{K}_{k+1}^T\right]+K_{k+1}{R}_{k+1}{K}_{k+1}^T\\ -E\left[(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{g}^T\left(x_{k+1}\right){\mathrm{\&Delta;}}^T{M}^T{K}_{k+1}^T\right]\\ -E\left[K_{k+1}\mathrm{M\&Delta;g}\left(x_{k+1}\right){\tilde{x}}_{k+1|k}^T{(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1})}^T\right]\end{array}$

$\begin{array}{c}-(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{g}^T\left(x_{k+1}\right){\mathrm{\&Delta;}}^T{M}^T{K}_{k+1}^T\\ {-K}_{k+1}\mathrm{M\&Delta;g}\left(x_{k+1}\right){\tilde{x}}_{k+1|k}^T{(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1})}^T\\ \&le;{\mathrm{\&epsiv;}}_1(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{\tilde{x}}_{k+1|k}^T{(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1})}^T\\ +{{\mathrm{\&epsiv;}}_1}^{-1}{K}_{k+1}\mathrm{M\&Delta;g}\left(x_{k+1}\right)g^T\left(x_{k+1}\right){\mathrm{\&Delta;}}^T{M}^T{K}_{k+1}^T\end{array}$

Evaluated error covariance matrix is deformed into:

$\begin{array}{c}{P}_{k+1}\&le;(1+{\mathrm{\&epsiv;}}_1)(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1}){\mathrm{\&Sigma;}}_{k+1|k}{(I_{n\&times;n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\mathrm{\&alpha;}}_{k+1}{L}_{k+1})}^T\\ +(1+{{\mathrm{\&epsiv;}}_1}^{-1})K_{k+1}\mathrm{ME}\left[\mathrm{\&Delta;g}\left(x_{k+1}\right)g^T\left(x_{k+1}\right){\mathrm{\&Delta;}}^T\right]M^T{K}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\\ \&le;(1+{\mathrm{\&epsiv;}}_1)[(I_{n\&times;n}-K_{k+1}{H}_{k+1}){({\mathrm{\&Sigma;}}_{k+1|k}^{-1}-\mathrm{\&mu;}{L}_{k+1}^T{L}_{k+1})}^{-1}{(I_{n\&times;n}-K_{k+1}{H}_{k+1})}^T+{\mathrm{\&mu;}}^{-1}{K}_{k+1}{C}_{k+1}{C}_{k+1}^T{K}_{k+1}^T]\\ +(1+{{\mathrm{\&epsiv;}}_1}^{-1})K_{k+1}{\mathrm{MM}}^T{K}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\end{array}$

μ is known positive number, meets ${\mathrm{\&mu;}}^{-1}{I}_{n\&times;n}-L_{k+1}{P}_{k+1|k}{L}_{k+1}^T>0;$

The upper bound of k+1 evaluated error is constantly:

$\begin{array}{c}{\mathrm{\&Sigma;}}_{k+1}=(1+{\mathrm{\&epsiv;}}_1)[(I_{n\&times;n}-K_{k+1}{H}_{k+1}){({\mathrm{\&Sigma;}}_{k+1|k}^{-1}-\mathrm{\&mu;}{L}_{k+1}^T{L}_{k+1})}^{-1}{(I_{n\&times;n}-K_{k+1}{H}_{k+1})}^T+{\mathrm{\&mu;}}^{-1}{K}_{k+1}{C}_{k+1}{C}_{k+1}^T{K}_{k+1}^T]\\ +(1+{{\mathrm{\&epsiv;}}_1}^{-1})K_{k+1}{\mathrm{MM}}^T{K}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\end{array}$

Order trying to achieve optimal filtering gain is:

$\begin{array}{c}{K}_{k+1}=(1+{\mathrm{\&epsiv;}}_1){({\mathrm{\&Sigma;}}_{k+1|k}^{-1}-\mathrm{\&mu;}{L}_{k+1}^T{L}_{k+1})}^{-1}{H}_{k+1}^T\\ \&times;{\{(1+{\mathrm{\&epsiv;}}_1)H_{k+1}{({\mathrm{\&Sigma;}}_{k+1|k}^{-1}-\mathrm{\&mu;}{L}_{k+1}^T{L}_{k+1})}^{-1}{H}_{k+1}^T+{\mathrm{\&mu;}}^{-1}{C}_{k+1}{C}_{k+1}^T+(1+{{\mathrm{\&epsiv;}}_1}^{-1}){\mathrm{MM}}^T+R_{k+1}\}}^{-1}\end{array}$

Obtain k+1 state estimation value constantly upper bound Σ with variance _k+1;

‖ q _k‖ ₂=1, then by k+1 state estimation value constantly in hypercomplex number be partly normalized constraint;

(5) be M the working time of posture estimation system, if k=M exports the result of attitude quaternion and gyroscopic drift, completes attitude and estimate; If k < is M, represent that filtering does not complete, repeating step (3) is to step (4), until posture estimation system end of run.

2. a kind of robust recursive filtering method of estimating for attitude of flight vehicle under interference that measures according to claim 1, is characterized in that: the poor σ of being of gyro to measure noise criteria in step (2) _v=26875 * 10 ^-7rad/s ^1/2, gyroscopic drift noise criteria is poor is σ _u=89289 * 10 ^-10rad/s ^3/2, the sampling period of gyro is Δ t=025s; It is σ that star sensor is measured noise criteria poor _s=18 ", output frequency is 1Hz; The reference vector of star sensor is made as ${\stackrel{\&RightArrow;}{r}}^1={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^T,{\stackrel{\&RightArrow;}{r}}^2={\left[\begin{array}{ccc}0& 1& 0\end{array}\right]}^T$ and ${\stackrel{\&RightArrow;}{r}}^3={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T;$ Initial gyroscopic drift β=[0.1 0.1 0.1] ^t°/h; σ _ij=10 ", j=x, y, z.

3. a kind of robust recursive filtering method of estimating for attitude of flight vehicle under interference that measures according to claim 1 and 2, is characterized in that: in step (3), Initial state estimation value is ${\hat{x}}_{0|0}={\left[\begin{array}{ccccccc}0& 0& 0& 1& 0& 0& 0\end{array}\right]}^T;$ Initial variance battle array is made as P ₀₀=diag ([(0.1 °) ²(0.1 °) ²(0.1 °) ²(0.1 °) ²(0.2 °/h) ²(0.2 °/h) ²(0.2 °/h) ²]; A _k=0 _{n * n}; For λ=0.0001, ε=01.

4. a kind of robust recursive filtering method of estimating for attitude of flight vehicle under interference that measures according to claim 3, is characterized in that: C in step (4) _k+1=0 _{m * n}; μ=0.0001; ε ₁=0.1.

5. a kind of robust recursive filtering method of estimating for attitude of flight vehicle under interference that measures according to claim 4, is characterized in that: M=1000 in step (5).