CN101846510A - High-precision satellite attitude determination method based on star sensor and gyroscope - Google Patents

Abstract

The present invention discloses a high-precision satellite attitude determination method based on star sensor and gyroscope, which comprises the following steps: step 1. establishing a status equation of a satellite attitude determination system; step 2. establishing a measurement equation of the satellite attitude determination system; step 3. performing an online real-time model error estimation through predictive filtering; and step 4. performing a status estimation on a compensated model through 2-order interpolation filtering to obtain the attitude of a satellite. By applying predictive filtering to performing an online real-time model error estimation and correcting the system model, the invention overcomes the shortcoming existing in the conventional estimation process that error is processed into zero-mean white noise; and in addition, the invention can process any nonlinear system and noise conditions to obtain an estimation result of higher precision and is applicable to the field of high-precision attitude determination.

Description

A kind of high-precision satellite attitude based on star sensor and gyro is determined method

Technical field

The present invention relates to a kind of high-precision satellite attitude and determine method, belong to the spacecraft high-precision attitude and determine technical field based on star sensor and gyro.

Background technology

Attitude of flight vehicle determines that system is the important component part in the attitude of flight vehicle control system, and its precision and reliability are the important assurance and the prerequisites of the long-term normal operation of aircraft.This system mainly determines that by attitude sensor and corresponding attitude algorithm forms, and for the influence to attitude determination accuracy of the uncertainty that overcomes reference vector, the method for state estimation determines that at high-precision attitude occasion has obtained using widely.

Method at present commonly used be EKF (Extended Kalman Filter, EKF), EKF has fast convergence rate, algorithm is simple, can real-time resolving etc. advantage.But EKF is because adopted nonlinear model Taylor has been launched approximate method, non-linear when strong estimated bias bigger.And EKF need calculate Jacobian matrix, requires the function must can be little, has limited its usable range.In addition, EKF is treated to the zero-mean white noise with system noise, and this just requires the modeling must be very accurate, if the noise in the real system is not a white Gaussian noise, handles and be used as white noise, then will cause estimated accuracy seriously to descend.Nonlinear prediction filtering (Nonlinear Predict Filter, NPF) system model error or the unknown influence of importing have been considered, has estimated capacity to model error, compare with other non-linear filtering methods such as EKF, predictive filtering can On-line Estimation go out unknown model error, overcome the limitation that model error and system noise is made as white Gaussian noise, in attitude of flight vehicle estimation field, obtained application.But the predictive filtering speed of convergence is relatively slow, and the inaccurate meeting of the model error that estimates during convergence impacts filtering convergence and precision.

Summary of the invention

The objective of the invention is to determine the aspect the deficiencies in the prior art, propose a kind of high-precision satellite attitude and determine method based on star sensor and gyro in order to solve the aircraft high-precision attitude.

A kind of high-precision satellite attitude based on star sensor and gyro of the present invention is determined method, comprises following step:

Step 1: the state equation of setting up Satellite Attitude Determination System;

Step 2: the measurement equation of setting up Satellite Attitude Determination System;

Step 3: utilize predictive filtering online in real time estimation model error;

Step 4: utilize second order interpolation filtering to carry out state estimation to the model after the compensation, obtain the attitude of satellite.

The invention has the advantages that:

(1) adopted predictive filtering online in real time estimation model error and update the system model, having overcome in the conventional estimated process Error processing is the shortcoming of zero-mean white noise.

(2) adopted the second order interpolation filtering algorithm, be a kind of polynomial interpolation filtering algorithm higher than EKF precision, and do not need to calculate partial derivative, the scope of application is wider;

(3) the present invention has adopted predictive filtering to estimate and the bucking-out system model error, can handle any nonlinear system noise situations of unifying, and obtains more high-precision estimated result, is applicable to that high-precision attitude determines the field.

Description of drawings

Fig. 1 is a method flow diagram of the present invention;

Fig. 2 a is the angle of pitch graph of errors comparison diagram of method of the present invention and EKF method simulation result;

Fig. 2 b is the crab angle graph of errors comparison diagram of method of the present invention and EKF method simulation result;

Fig. 2 c is the roll angle graph of errors comparison diagram of method of the present invention and EKF method simulation result;

Fig. 3 a is the angle of pitch graph of errors comparison diagram of method of the present invention and second order interpolation filtering method simulation result;

Fig. 3 b is the crab angle graph of errors comparison diagram of method of the present invention and second order interpolation filtering method simulation result;

Fig. 3 c is the roll angle graph of errors comparison diagram of method of the present invention and second order interpolation filtering method simulation result.

Embodiment

The present invention is described in further detail below in conjunction with drawings and Examples.

The present invention is that a kind of high-precision satellite attitude based on starlight and gyro is determined method, utilize gyro and star sensor as attitude sensor, utilize the thought of predictive filtering to estimate the model error of Satellite Attitude Determination System in real time and compensate in real time, utilize the higher second order interpolation Filtering Estimation of precision to go out the attitude quaternion of satellite body coordinate system again with respect to inertial coordinates system, obtain the attitude of satellite through resolving then, flow process comprises following step as shown in Figure 1:

Step 1: the state equation of setting up Satellite Attitude Determination System;

The attitude of coming together to describe satellite with attitude dynamic equations and kinematical equation changes, and attitude dynamic equations can derive from the momentum moment formula and the theorem of rigid body, i.e. Euler-Newton method: according to the rigid body moment of momentum theorem, the attitude dynamic equations of satellite is:

$J\stackrel{\·}{\mathrm{\ω}}+\mathrm{\ω}\×\mathrm{J\ω}=G_{\mathrm{gb}}+G_{\mathrm{mb}}+G_{\mathrm{ab}}+G_{\mathrm{sb}}---\left(1\right)$

In the formula, ω=[ω _x, ω _y, ω _z] ^TBe the rotational angular velocity of satellite body coordinate system to inertial coordinates system, ω _x, ω _y, ω _zFor the satellite body coordinate system ties up to satellite body coordinate system lower edge x to inertial coordinate, y, the component of three axles of z; J is the inertial tensor matrix of satellite; G _MbBe geomagnetic torque; G _AbBe aerodynamic moment; G _SbBe the solar pressure square; G _GbBe gravity gradient torque;

G _GbBe specially:

$G_{\mathrm{gb}}=\frac{3\mathrm{\μ}}{r^5}{R}_b\×{\mathrm{JR}}_b---\left(2\right)$

In the formula, μ is a geocentric gravitational constant; R is the earth's core distance of satellite; R _bBe the position vector of satellite under the satellite body coordinate system, be specially:

$R_b=C_i^b{R}_i---\left(3\right)$

In the formula, R _iBe the position vector of satellite under inertial coordinates system,

For be tied to the transition matrix of satellite body coordinate system from inertial coordinate, be specially:

The earth's core distance

True anomaly

Eccentric anomaly

Mean anomaly M=n (t-t0); N is the flat movement velocity of satellite; t ₀For satellite arrives the perigean time; T is the time;

Be the transition matrix of geocentric orbital reference system, be specially to inertial coordinates system:

$C_o^i=\left[\begin{array}{ccc}\cos {\mathrm{\Ω}}_o\cos w_o-\sin w_o\cos i_o\sin {\mathrm{\Ω}}_o& -\cos {\mathrm{\Ω}}_o\sin w_o-\cos w_o\cos i_o\sin {\mathrm{\Ω}}_o& \sin {\mathrm{\Ω}}_o\sin i_o\\ \sin {\mathrm{\Ω}}_o\cos w_o+\sin w_o\cos i_o\cos {\mathrm{\Ω}}_o& -\sin {\mathrm{\Ω}}_o\sin w_o+\cos w_o\cos i_o\cos {\mathrm{\Ω}}_o& -\cos {\mathrm{\Ω}}_o\sin i_o\\ \sin w_o\sin i_o& \sin w_o\cos i_o& \cos i_o\end{array}\right]$

Wherein, Ω _oBe the ascending node of satellite orbit right ascension; i _oBe inclination of satellite orbit; w _oBe argument of perigee of satellite orbit.

Because G _GbThan geomagnetic torque G _Mb, aerodynamic moment G _AbWith solar pressure square G _SbInfluence to Satellite Attitude Determination System is much bigger, so with G _Mb, G _AbAnd G _SbWith resultant moment d _GExpression, as the model error of modeling not, then the attitude of satellite kinetics equation of formula (1) is written as:

$\stackrel{\·}{\mathrm{\ω}}=J^{-1}(-\mathrm{\ω}\×\mathrm{J\ω}+G_{\mathrm{gb}})+J^{-1}{d}_G---\left(4\right)$

It is the equation that attitude parameter changes in the attitude maneuver process that attitude motion is learned equation, because it is linear differential equation that the attitude motion that hypercomplex number is represented is learned equation, do not contain trigonometric function, no singular point problem, so the Satellite Attitude Movement equation that adopts hypercomplex number to describe is as follows:

$\stackrel{\·}{q}=\frac{1}{2}\mathrm{\Ω}\left(\mathrm{\ω}\right)q=\frac{1}{2}\mathrm{\Ξ}\left(q\right)\mathrm{\ω}---\left(5\right)$

In the formula,

Be the attitude quaternion of satellite body coordinate system with respect to inertial coordinates system; q ₁₃=[q ₁q ₂q ₃] ^T,

I _{3 * 3}Be unit matrix; [ω *], [q ₁₃*] represent respectively by vectorial ω, q ₁₃The antisymmetric matrix that constitutes of component,

The state equation that is made of Satellite Attitude Determination System formula (4) and formula (5) is:

$\stackrel{\·}{x}=\left[\begin{array}{c}{J}^{-1}(-\mathrm{\ω}\×\mathrm{J\ω}+G_{\mathrm{gb}})\\ \frac{1}{2}\mathrm{\Ξ}\left(q\right)\mathrm{\ω}\end{array}\right]+G\·d_G+W---\left(6\right)$

In the formula: x is a state vector, x=[ω ^T, q ^T] ^TG is the error perturbation matrix, System noise w (k)～(0, Q (k)), promptly w (k) obeys that average is 0, variance is the Gaussian distribution of Q (k).

With formula (6) brief note be:

$\stackrel{\·}{x}=f(x,d_G,w)---\left(7\right)$

Step 2: the measurement equation of setting up Satellite Attitude Determination System;

Adopt three gyros and star sensor as attitude sensor among the present invention.

1) gyro;

The measurement equation of gyro is:

$g_1\left(x\left(k\right)\right)=\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]+\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]=g_{\mathrm{\ω}}\left(x\left(k\right)\right){+v}_{\mathrm{\ω}}\left(k\right)---\left(8\right)$

In the formula, g ₁Be the measurement output of gyro, note (i=x, y z) are the rotational angular velocity of satellite body coordinate system to inertial coordinates system, v to ω i _i(i=x, y z) are the white Gaussian noise of gyro to measure.

(2) star sensor;

The present invention adopts two star sensors to measure, and the unit direction vector of the primary optical axis of two star sensitivities is respectively l in inertial coordinates system _I1And l _I2, then the measurement equation of star sensor is:

$\left\{\begin{array}{c}{g}_2=C_i^b{l}_{i1}+v_{s1}=g_{s1}\left(x\left(k\right)\right)+v_{s1}\\ g_3=C_i^b{l}_{i2}+v_{s2}=g_{s2}\left(x\left(k\right)\right)+v_{s2}\end{array}\right.---\left(9\right)$

In the formula, v _S1, v _S2Be the noise that star sensor is measured, mainly be thought of as white Gaussian noise here; g ₁, g ₂Be respectively the measurement output of two star sensors; Note

Be the transition matrix that is tied to the satellite body coordinate system by inertial coordinate, available attitude quaternion is represented, is specially:

$C_i^b=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_4^2& 2(q_1{q}_2+q_3{q}_4)& 2(q_1{q}_3-q_2{q}_4)\\ 2(q_1{q}_2-q_3{q}_4)& -q_1^2+q_2^2-q_3^2+q_4^2& 2(q_2{q}_3+q_1{q}_4)\\ 2(q_1{q}_3+q_2{q}_4)& 2(q_2{q}_3-q_1{q}_4)& -q_1^2-q_2^2+q_3^2+q_4^2\end{array}\right]---\left(10\right)$

(3) observation equation of setting up Satellite Attitude Determination System is:

$y\left(k\right)=\left[\begin{array}{c}{g}_{\mathrm{\ω}}\left(x\left(k\right)\right)\\ g_{s1}\left(x\left(k\right)\right)\\ g_{s2}\left(x\left(k\right)\right)\end{array}\right]+\left[\begin{array}{c}{v}_{\mathrm{\ω}}\left(k\right)\\ v_{s1}\left(k\right)\\ v_{s2}\left(k\right)\end{array}\right]---\left(11\right)$

With formula (11) brief note be:

y(k)＝g(x(k)，v(k)) (12)

Wherein, Be that v (k) obedience average is 0, variance is the Gaussian distribution of R (k), and w (k), and v (k) is separate.

Step 3: utilize predictive filtering online in real time estimation model error;

Model error

Estimator be:

${\hat{d}}_G\left(k\right)=-{\left({\left\{\mathrm{\Λ}\left(T\right)U\right[\hat{x}\left(k\right)\left]\right\}}^T{R}^{-1}\right\{\mathrm{\Λ}\left(T\right)U\left[\hat{x}\left(k\right)\right]\}+W)}^{-1}{\left\{\mathrm{\Λ}\left(T\right)U\right[\hat{x}\left(k\right)\left]\right\}}^T{R}^{-1}$ (13)

$x\{\hat{y}\left(k\right)+z[\hat{x}\left(k\right),T]-y(k+1)\}$

In the formula:

Estimate that for the output of gyro and two star sensors T is the filtering cycle, U is a sensitivity matrix, and Λ (T) is a diagonal matrix,

It is a column vector.W is a weighting matrix, preestablishes numerical value.

Wherein,

Be specially:

$\hat{y}\left(k\right)=g\left[\hat{x}\left(k\right)\stackrel{\‾}{v}\left(k\right)\right]---\left(14\right)$

In the formula:

Be the estimated value of k quantity of state constantly, The average of expression v (k).

Sensitivity matrix U is:

$U=\left[\begin{array}{c}{L}_G\left[L_f^0\left(g_w\right)\right]\\ L_G\left[L_f^1\left(g_{s1}\right)\right]\\ L_G\left[L_f^1\left(g_{s2}\right)\right]\end{array}\right],---\left(15\right)$

Being specially of relevant Lie derivative:

$L_G\left[L_f^0\left(g_w\right)\right]=J^{-1}$

$L_G\left[{L^1}_f\left(g_{s1}\right)\right]=\frac{\∂{L^1}_f\left(g_{s1}\right)}{\∂\hat{x}}\·G=-{\mathrm{\Ξ}}^T\left(\hat{q}\right)\mathrm{\Γ}\left(l_{i1}\right)\mathrm{\Ξ}\left(\hat{q}\right)J^{-1}$

$L_G\left[{L^1}_f\left(g_{s2}\right)\right]=\frac{\∂{L^1}_f\left(g_{s2}\right)}{\∂\hat{x}}\·G=-{\mathrm{\Ξ}}^T\left(\hat{q}\right)\mathrm{\Γ}\left(l_{i2}\right)\mathrm{\Ξ}\left(\hat{q}\right)J^{-1}$

Diagonal matrix Λ (T) is:

$\mathrm{\&Lambda;}\left(T\right)=\left[\begin{array}{ccc}{\mathrm{\&Lambda;}}_{\mathrm{<figure-callout\; id="0"\; label="w"\; filenames="HSA00000138461100023.png"\; state="\{\{state\}\}">w</figure-callout>}}& 0& 0\\ 0& {\mathrm{\&Lambda;}}_{s1}& 0\\ 0& 0& {\mathrm{\&Lambda;}}_{s2}\end{array}\right]---\left(16\right)$

Wherein: Λ _w=TI _{3 * 3}, ${\mathrm{\&Lambda;}}_{s1}=\frac{T^2}{2}\&CenterDot;I_{3\&times;3},$ ${\mathrm{\&Lambda;}}_{s2}=\frac{T^2}{2}\&CenterDot;I_{3\&times;3},;$

Column vector For:

$z[\hat{x}\left(k\right),T]=\left[\begin{array}{c}{z}_{\mathrm{\&omega;}}\\ z_{s1}\\ z_{s2}\end{array}\right]=\left[\begin{array}{c}T\&CenterDot;{L^1}_f\left(g_{\mathrm{\&omega;}}\right)\\ T\&CenterDot;{L^1}_f\left(g_{s1}\right)+\frac{T^2}{2}{L^2}_f\left(g_{s1}\right)\\ T\&CenterDot;{L^1}_f\left(g_{s2}\right)+\frac{T^2}{2}\&CenterDot;{L^2}_f\left(g_{s2}\right)\end{array}\right]---\left(17\right)$

Each rank Lie derivative is in the following formula:

${L^1}_f\left(g_{\mathrm{\&omega;}}\right)=J^{-1}[-\widehat{\mathrm{\&omega;}}\&times;J\widehat{\mathrm{\&omega;}}+G_{\mathrm{gb}}]$

${L^1}_f\left(g_{s1}\right)=-{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i1}\right)\mathrm{\&Xi;}\left(\hat{q}\right)\widehat{\mathrm{\&omega;}}$

${L^2}_f\left(g_{s1}\right)=[\widehat{\mathrm{\&omega;}}\&times;]\&CenterDot;{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i1}\right)\mathrm{\&Xi;}\left(\hat{q}\right)\widehat{\mathrm{\&omega;}}-{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i1}\right)\mathrm{\&Xi;}\left(\hat{q}\right)\&times;J^{-1}[-\widehat{\mathrm{\&omega;}}\&times;J\widehat{\mathrm{\&omega;}}+G_{\mathrm{gb}}]$

${L^1}_f\left(g_{s2}\right)=-{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i2}\right)\mathrm{\&Xi;}\left(\hat{q}\right)\widehat{\mathrm{\&omega;}}$

${L^2}_f\left(g_{s2}\right)=\left[\widehat{\mathrm{\&omega;}}\&times;\right]\&CenterDot;{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i2}\right)\mathrm{\&Xi;}\left(\hat{q}\right)\widehat{\mathrm{\&omega;}}-{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i2}\right)\mathrm{\&Xi;}\left(\hat{q}\right)\&times;J^{-1}[-\widehat{\mathrm{\&omega;}}\&times;J\widehat{\mathrm{\&omega;}}+G_{\mathrm{gb}}]$

Wherein: $\mathrm{\&Gamma;}\left(a\right)=\left[\begin{array}{cc}-\left[a\right]\&times;& -a\\ a^{\mathrm{<figure-callout\; id="0"\; label="T"\; filenames="HSA00000138461100023.png"\; state="\{\{state\}\}">T</figure-callout>}}& 0\end{array}\right].$

At last, obtain the estimated value of model error according to formula (13)

Step 4: utilize second order interpolation filtering to carry out state estimation to the model after the compensation, obtain the attitude of satellite;

Filtering interpolation is to utilize the Stirling interpolation formula that nonlinear function is similar to, and is a kind of method of estimation based on minimum mean square error criterion.

Be specially:

With the model error that obtains in the step 3

Substitution state equation (7) compensates, and is write the state equation and the measurement equation of the Satellite Attitude Determination System model after the compensation as discrete non-linear form, for:

x _k+1＝f(x _k，d _G，w _k)

(18)

y _k＝g(x _k，v _k)

If filter value

The error variance battle array

Square root be Promptly Be

Cholesky decompose, the square root of model error variance battle array Q is S _w, the square root of measuring the error variance battle array R of noise is S _v, the variance battle array of status predication error

Square root be That is:

$Q=S_w{S}_w^T,$ $R=S_v{S}_v^T,$ (19)

$\stackrel{\&OverBar;}{P}={\stackrel{\&OverBar;}{S}}_x{\stackrel{-T}{S}}_x,$ $\hat{P}={\hat{S}}_x{\hat{S}}_x^T.$

The one-step prediction of state and error variance battle array thereof:

${\stackrel{\&OverBar;}{x}}_{k+1}=\frac{h^2-n_x-n_w}{h^2}f({\hat{x}}_k,{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)$

$+\frac{1}{2h^2}\underset{p=1}{\overset{n_x}{\mathrm{\&Sigma;}}}(f({\hat{x}}_k+h{\hat{s}}_{x,p},{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)+f({\hat{x}}_k-h{\hat{s}}_{x,p},{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k))---\left(20\right)$

$+\frac{1}{2h^2}\underset{p=1}{\overset{n_v}{\mathrm{\&Sigma;}}}(f({\hat{x}}_k,{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k+h{\hat{s}}_{w,p})+f({\hat{x}}_{k,}{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k-h{\hat{s}}_{w,p}))$

Wherein,

The average of expression w (k), n _xThe dimension of expression state vector, n _wThe dimension of expression system noise will guarantee that estimated result is not have inclined to one side estimation interpolation step-length to get h ²=3.

The Cholesky of status predication error variance battle array decomposes

Matrix after the Householder conversion:

$\stackrel{\&OverBar;}{S}(k+1)=\left[S_{x\hat{x}}^{\left(1\right)}\left(k\right)S_{\mathrm{xw}}^{\left(1\right)}\left(k\right)S_{x\hat{x}}^{\left(2\right)}\left(k\right)S_{\mathrm{xw}}^{\left(2\right)}\left(k\right)\right]---\left(21\right)$

Wherein,

$S_{x\hat{x}}^{\left(1\right)}\left(k\right)=\left\{S_{x\hat{x}}^{\left(1\right)}{\left(k\right)}_{(i,j)}\right\}=\{(f_i({\hat{x}}_k+h{\hat{s}}_{x,j},{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)-f_i({\hat{x}}_k-h{\hat{s}}_{x,j},{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k))/2h\},$

$S_{xw}^{\left(1\right)}\left(k\right)=\left\{S_{xw}^{\left(1\right)}{\left(k\right)}_{(i,j)}\right\}=\{(f_i({\hat{x}}_{k,}{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k+h{\hat{s}}_{w,j})-f_i({\hat{x}}_k,{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k-h{\hat{s}}_{w,j}))/2h\},$

$S_{x\hat{x}}^{\left(2\right)}\left(k\right)=\left\{S_{x\hat{x}}^{\left(2\right)}{\left(k\right)}_{(i,j)}\right\}=\{\frac{\sqrt{h^2-1}}{2h^2}(f_i({\hat{x}}_k+h{\hat{s}}_{x,j},{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)$

$+f_i({\hat{x}}_k-h{\hat{s}}_{x,j},{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)-2f_i({\hat{x}}_k,{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)\left)\right\},$

$S_{xw}^{\left(2\right)}\left(k\right)=\left\{S_{xw}^{\left(2\right)}{\left(k\right)}_{(i,j)}\right\}=\{\frac{\sqrt{h^2-1}}{2h^2}(f_i({\hat{x}}_{k,}{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k+h{\hat{s}}_{w,j})$

$+f_i({\hat{x}}_{k,}{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k-h{\hat{s}}_{w,j})-2f_i({\hat{x}}_k,{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)\left)\right\},$

Wherein,

Expression

J row, Expression

J row, Expression S _wJ row.The one-step prediction of observed reading is:

${\stackrel{\&OverBar;}{y}}_{k+1}=\frac{h^2-n_x-n_v}{h^2}g({\stackrel{\&OverBar;}{x}}_{k+1},{\stackrel{\&OverBar;}{v}}_{k+1})$

$+\frac{1}{2h^2}\underset{p=1}{\overset{n_x}{\mathrm{\&Sigma;}}}(g({\stackrel{\&OverBar;}{x}}_{k+1}+h{\stackrel{\&OverBar;}{s}}_{x,p},{\stackrel{\&OverBar;}{v}}_{k+1})+g({\stackrel{\&OverBar;}{x}}_{k+1}-h{\stackrel{\&OverBar;}{s}}_{x,p},{\stackrel{\&OverBar;}{v}}_{k+1}))---\left(22\right)$

$+\frac{1}{2h^2}\underset{p=1}{\overset{n_v}{\mathrm{\&Sigma;}}}(g({\stackrel{\&OverBar;}{x}}_{k+1,}{\stackrel{\&OverBar;}{v}}_{k+1}+h{\stackrel{\&OverBar;}{s}}_{v,p})+g({\stackrel{\&OverBar;}{x}}_{k+1,}{\stackrel{\&OverBar;}{v}}_{k+1}-h{\stackrel{\&OverBar;}{s}}_{v,p}))$

Wherein, n _vBe to measure the noise dimension.

The Cholesky of error variance battle array to decompose be S _y(k+1) matrix after the Householder conversion:

$S_y(k+1)=\left[S_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}(k+1)S_{\mathrm{yv}}^{\left(1\right)}(k+1)S_{y\stackrel{\&OverBar;}{x}}^{\left(2\right)}(k+1)S_{\mathrm{yv}}^{\left(2\right)}(k+1)\right]---\left(23\right)$

Wherein,

$S_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}(k+1)=\left\{S_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}{(k+1)}_{(i,j)}\right\}=\{(g_i({\stackrel{\&OverBar;}{x}}_{k+1}+h{\stackrel{\&OverBar;}{s}}_{x,j},{\stackrel{\&OverBar;}{v}}_{k+1})-g_i({\stackrel{\&OverBar;}{x}}_{k+1}-h{\stackrel{\&OverBar;}{s}}_{x,j},{\stackrel{\&OverBar;}{v}}_{k+1}))/2h\},$

$S_{\mathrm{yv}}^{\left(1\right)}(k+1)=\left\{S_{\mathrm{yv}}^{\left(1\right)}{(k+1)}_{(i,j)}\right\}=\{(g_i({\stackrel{\&OverBar;}{x}}_{k+1,}{\stackrel{\&OverBar;}{v}}_{k+1}+h{\stackrel{\&OverBar;}{s}}_{v,j})-g_i({\stackrel{\&OverBar;}{x}}_{k+1,}{\stackrel{\&OverBar;}{v}}_{k+1}-h{\stackrel{\&OverBar;}{s}}_{v,j}))/2h\},$

$S_{y\stackrel{\&OverBar;}{x}}^{\left(2\right)}(k+1)=\left\{S_{y\stackrel{\&OverBar;}{x}}^{\left(2\right)}{(k+1)}_{(i,j)}\right\}=\{\frac{\sqrt{h^2-1}}{2h^2}(g_i({\stackrel{\&OverBar;}{x}}_{k+1}+h{\stackrel{\&OverBar;}{s}}_{x,j},{\stackrel{\&OverBar;}{v}}_{k+1})$

$+g_i({\stackrel{\&OverBar;}{x}}_{k+1}-h{\stackrel{\&OverBar;}{s}}_{x,j},{\stackrel{\&OverBar;}{v}}_{k+1})-2g_i({\stackrel{\&OverBar;}{x}}_{k+1},{\stackrel{\&OverBar;}{v}}_{k+1})\left)\right\},$

$S_{\mathrm{yv}}^{\left(2\right)}(k+1)=\left\{S_{\mathrm{yv}}^{\left(2\right)}{(k+1)}_{(i,j)}\right\}=\{\frac{\sqrt{h^2-1}}{2h^2}(g_i({\stackrel{\&OverBar;}{x}}_{k+1},{\stackrel{\&OverBar;}{v}}_{k+1}+h{\stackrel{\&OverBar;}{s}}_{v,j})$

$+g_i({\stackrel{\&OverBar;}{x}}_{k+1},{\stackrel{\&OverBar;}{v}}_{k+1}-h{\stackrel{\text{-}}{s}}_{v,j})-2g_i({\stackrel{\&OverBar;}{x}}_{k+1},{\stackrel{\&OverBar;}{v}}_{k+1})\left)\right\},$

In the formula:

Expression S _vJ row.

The cross covariance battle array of state and measurement is:

$P_{\mathrm{xy}}(k+1)={\stackrel{\&OverBar;}{S}}_x(k+1)S_{y\stackrel{\&OverBar;}{x}}{(k+1)}^T---\left(24\right)$

Obtain gain matrix K by formula (24) _K+1For:

K _k+1＝P _xy(k+1)[S _y(k+1)S _y(k+1) ^T] ^-1 (25)

Be updated to based on state and the estimation error variance battle array measured:

${\hat{x}}_{k+1}={\stackrel{-}{x}}_{k+1}+K_{k+1}(y_{k+1}-{\stackrel{\&OverBar;}{y}}_{k+1})---\left(26\right)$

$\hat{P}(k+1)={[{\stackrel{\&OverBar;}{S}}_x(k+1)-K_{k+1}{S}_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}(k+1)][{\stackrel{\&OverBar;}{S}}_x(k+1)-K_{k+1}{S}_{\mathrm{yx}}^{\left(1\right)}(k+1)]}^T+K_{k+1}{S}_{\mathrm{yv}}^{\left(1\right)}(k+1){\left[K_{k+1}{S}_{\mathrm{yv}}^{\left(1\right)}(k+1)\right]}^T$

$+K_{k+1}{S}_{\mathrm{yx}}^{\left(2\right)}(k+1){\left[K_{k+1}{S}_{\mathrm{yx}}^{\left(2\right)}(k+1)\right]}^T+K_{k+1}{S}_{\mathrm{yv}}^{\left(2\right)}(k+1){\left[K_{k+1}{S}_{\mathrm{yv}}^{\left(2\right)}(k+1)\right]}^T---\left(27\right)$

Obtain the state estimation value thus

Wherein Be the attitude quaternion of satellite, each attitude quaternion carried out normalized after estimating, and resolve according to formula (28) (29) (30) and to obtain attitude angle;

The angle of pitch: θ=-arcsin (2 _Q1q3-2 _Q2q4) (28)

Crab angle: $\mathrm{\&Psi;}=\arctan \left(\frac{2_{q2q3}-2_{q1q4}}{q_3^2+q_4^2-q_1^2-q_2^2}\right)---\left(29\right)$

Roll angle:

This method is because there is pair model error ability of estimation in real time, and therefore the compensation model error can obtain more high-precision Satellite Attitude Estimation in real time.

Simultaneously, this method can also be applicable to other similar systems that has model error.

When having model error, under same simulated conditions, the graph of errors of using attitude that the present invention determines satellite and application extension Kalman filtering (EKF) simulation result is shown in Fig. 2 a, Fig. 2 b, Fig. 2 c, and application the present invention determines that the graph of errors of the attitude of satellite and second order interpolation filtering (DD2) simulation result is shown in Fig. 3 a, Fig. 3 b, Fig. 3 c.

Horizontal ordinate all is simulation times among Fig. 2 a and Fig. 3 a, ordinate is the evaluated error curve (actual value and estimated value poor) of the angle of pitch, as can be seen from the figure, the angle of pitch evaluated error of EKF and DD2 is in 10 rads, and evaluated error of the present invention is in 1 rad.

Horizontal ordinate all is simulation times among Fig. 2 b and Fig. 3 b, and ordinate is the evaluated error curve of crab angle; The horizontal ordinate of Fig. 2 c and Fig. 3 c all is simulation times, and ordinate is the evaluated error curve of roll angle; As can be seen from the figure, the crab angle of EKF and DD2 and the concussion of roll angle evaluated error are more violent, even sometimes error surpasses 15 rads, and evaluated error of the present invention is because the estimation model error also compensates in real time, so the evaluated error angle is still in 2 rads; Therefore the method precision of the present invention's proposition is significantly improved.

Claims (1)

1. the high-precision satellite attitude based on starlight and gyro is determined method, it is characterized in that, adopt gyro and star sensor as attitude sensor, the model error that utilizes the predictive filtering algorithm to estimate Satellite Attitude Determination System in real time also compensates in real time, utilize second order interpolation filtering to carry out state estimation again, specifically comprise following step:

Step 1: the state equation of setting up Satellite Attitude Determination System;

The attitude dynamic equations of satellite is:

$J\stackrel{\&CenterDot;}{\mathrm{\&omega;}}+\mathrm{\&omega;}\&times;\mathrm{J\&omega;}=G_{\mathrm{gb}}+G_{\mathrm{gb}}+G_{\mathrm{gb}}+G_{\mathrm{sb}}---\left(1\right)$

In the formula, ω=[ω _x, ω _y, ω _z] ^TBe the rotational angular velocity of satellite body coordinate system to inertial coordinates system, ω _x, ω _y, ω _zFor the satellite body coordinate system ties up to satellite body coordinate system lower edge x to inertial coordinate, y, the component of three axles of z; J is the inertial tensor matrix of satellite; G _MbBe geomagnetic torque; G _AbBe aerodynamic moment; G _SbBe the solar pressure square; G _GbBe gravity gradient torque;

G _GbBe specially:

$G_{\mathrm{gb}}=\frac{3\mathrm{\&mu;}}{r^5}{R}_b\&times;J{R}_b---\left(2\right)$

In the formula, μ is a geocentric gravitational constant; R is the earth's core distance of satellite; R _bBe the position vector of satellite under the satellite body coordinate system, be specially:

$R_b=C_i^b{R}_i---\left(3\right)$

In the formula, R _iBe the position vector of satellite under inertial coordinates system,

For be tied to the transition matrix of satellite body coordinate system from inertial coordinate, be specially:

The earth's core distance True anomaly Eccentric anomaly

Mean anomaly M=n (t-t ₀); N is the flat movement velocity of satellite; t ₀For satellite arrives the perigean time; T is the time;

Be the transition matrix of geocentric orbital reference system, be specially to inertial coordinates system:

$C_o^i=\left[\begin{array}{ccc}\cos {\mathrm{\&Omega;}}_o\cos w_o-\sin w_o\cos i_o\sin {\mathrm{\&Omega;}}_o& -\cos {\mathrm{\&Omega;}}_o\sin w_o-\cos w_o\cos i_o\sin {\mathrm{\&Omega;}}_o& \sin {\mathrm{\&Omega;}}_o\sin i_o\\ \sin {\mathrm{\&Omega;}}_o\cos w_o+\sin w_o\cos i_o\cos {\mathrm{\&Omega;}}_o& -\sin {\mathrm{\&Omega;}}_o\sin w_o+\cos w_o\cos i_o\cos {\mathrm{\&Omega;}}_o& -\cos {\mathrm{\&Omega;}}_o\sin i_o\\ \sin w_o\sin i_o& \sin w_o\cos i_o& \cos i_o\end{array}\right]$

Wherein, Ω _oBe the ascending node of satellite orbit right ascension; i _oBe inclination of satellite orbit; w _oBe argument of perigee of satellite orbit;

With G _Mb, G _AbAnd G _SbResultant moment be expressed as d _G, as the model error of modeling not, then the attitude of satellite kinetics equation of formula (1) is written as:

$\stackrel{\&CenterDot;}{\mathrm{\&omega;}}=J^{-1}(-\mathrm{\&omega;}\&times;\mathrm{J\&omega;}+G_{\mathrm{gb}})+J^{-1}{d}_G---\left(4\right)$

The Satellite Attitude Movement equation that adopts hypercomplex number to describe is as follows:

$\stackrel{\&CenterDot;}{q}=\frac{1}{2}\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)q=\frac{1}{2}\mathrm{\&Xi;}\left(q\right)\mathrm{\&omega;}---\left(5\right)$

In the formula,

Be the attitude quaternion of satellite body coordinate system with respect to inertial coordinates system; q ₁₃=[q ₁q ₂q ₃] ^T,

I _{3 * 3}Be unit matrix; [ω *], [q ₁₃*] represent respectively by vectorial ω, q ₁₃The antisymmetric matrix that constitutes of component,

The state equation that is made of Satellite Attitude Determination System formula (4) and formula (5) is:

$\stackrel{\&CenterDot;}{x}=\left[\begin{array}{c}{J}^{-1}(-\mathrm{\&omega;}\&times;\mathrm{J\&omega;}+G_{\mathrm{gb}})\\ \frac{1}{2}\mathrm{\&Xi;}\left(q\right)\mathrm{\&omega;}\end{array}\right]+G\&CenterDot;d_G+W---\left(6\right)$

In the formula: x is a state vector, x=[ω ^T, q ^T] ^TG is the error perturbation matrix,

System noise w (k)～(0, Q (k)), promptly w (k) obeys that average is 0, variance is the Gaussian distribution of Q (k);

With formula (6) brief note be:

$\stackrel{\&CenterDot;}{x}=f(x,d_G,w)---\left(7\right)$

Step 2: the measurement equation of setting up Satellite Attitude Determination System;

Adopt three gyros and two star sensors as attitude sensor;

1) gyro;

The measurement equation of gyro is:

$g_1\left(x\left(k\right)\right)=\left[\begin{array}{c}{\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_z\end{array}\right]+\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]=g_{\mathrm{\&omega;}}\left(x\left(k\right)\right)+v_{\mathrm{\&omega;}}\left(k\right)---\left(8\right)$

In the formula, g ₁Be the measurement output of gyro, note

ω i is the rotational angular velocity of satellite body coordinate system to inertial coordinates system, v _iBe the white Gaussian noise of gyro to measure, i=x, y, z;

2) star sensor;

The unit direction vector of the primary optical axis of two star sensitivities is respectively l in inertial coordinates system _I1And l _I2, then the measurement equation of star sensor is:

$\left\{\begin{array}{c}{g}_2=C_i^b{I}_{i1}+v_{s1}=g_{s1}\left(x\left(k\right)\right)+v_{s1}\\ g_3=C_i^b{I}_{i2}+v_{s2}=g_{s2}\left(x\left(k\right)\right)+v_{s2}\end{array}---\left(9\right)\right.$

In the formula, v _S1, v _S2It is the white Gaussian noise that star sensor is measured; g ₁, g ₂Be respectively the measurement output of two star sensors; Note

Be the transition matrix that is tied to the satellite body coordinate system by inertial coordinate, represent, be specially with attitude quaternion:

$C_i^b=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_4^2& 2(q_1{q}_2+q_3{q}_4)& 2(q_1{q}_3-q_2{q}_4)\\ 2(q_1{q}_2-q_3{q}_4)& -q_1^2+q_2^2-q_3^2+q_4^2& 2(q_2{q}_3+q_1{q}_4)\\ 2(q_1{q}_3+q_2{q}_4)& 2(q_2{q}_3-q_1{q}_4)& -q_1^2-q_2^2+q_3^2+q_4^2\end{array}\right]---\left(10\right)$

3) observation equation of setting up Satellite Attitude Determination System is:

$y\left(k\right)=\left[\begin{array}{c}{g}_{\mathrm{\&omega;}}\left(x\left(k\right)\right)\\ g_{s1}\left(x\left(k\right)\right)\\ g_{s2}\left(x\left(k\right)\right)\end{array}\right]+\left[\begin{array}{c}{v}_{\mathrm{\&omega;}}\left(k\right)\\ v_{s1}\left(k\right)\\ v_{s2}\left(k\right)\end{array}\right]---\left(11\right)$

With formula (11) brief note be:

y(k)＝g(x(k)，v(k)) (12)

Wherein,

Be that v (k) obeys that average is 0, variance is the Gaussian distribution of R (k), and w (k), v (k) is separate;

Step 3: utilize predictive filtering online in real time estimation model error;

Model error

Estimator be:

${\hat{d}}_G\left(k\right)=-{\left({\left\{\mathrm{\&Lambda;}\left(T\right)U\right[\hat{x}\left(k\right)\left]\right\}}^T{R}^{-1}\right\{\mathrm{\&Lambda;}\left(T\right)U\left[\hat{x}\left(k\right)\right]\}+W)}^{-1}{\left\{\mathrm{\&Lambda;}\left(T\right)U\right[\hat{x}\left(k\right)\left]\right\}}^T{R}^{-1}$ (13)

$\&times;\{\hat{y}\left(k\right)+z[\hat{x}\left(k\right),T]-y(k+1)\}$

In the formula: Estimate that for the output of gyro and two star sensors T is the filtering cycle, U is a sensitivity matrix, and Λ (T) is a diagonal matrix,

It is a column vector; W is a weighting matrix, preestablishes numerical value;

Wherein,

Be specially:

$\hat{y}\left(k\right)=g[\hat{x}\left(k\right),\stackrel{\&OverBar;}{v}\left(k\right)]---\left(14\right)$

In the formula:

Estimated value for k quantity of state constantly;

The average of expression v (k);

Sensitivity matrix U is:

$U=\left[\begin{array}{c}{L}_G\left[{L^0}_f\left(g_w\right)\right]\\ L_G\left[{L^1}_f\left(g_{s1}\right)\right]\\ L_G\left[{L^1}_f\left(g_{s2}\right)\right]\end{array}\right],---\left(15\right)$

Being specially of relevant Lie derivative:

$L_G\left[{L^0}_f\left(g_w\right)\right]=J^{-1}$

$L_G\left[{L^1}_f\left(g_{s1}\right)\right]=\frac{\&PartialD;{L^1}_f\left(g_{s1}\right)}{\&PartialD;\hat{x}}\&CenterDot;G=-{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i1}\right)\mathrm{\&Xi;}\left(\hat{q}\right)J^{-1}$

$L_G\left[{L^1}_f\left(g_{s2}\right)\right]=\frac{\&PartialD;{L^1}_f\left(g_{s2}\right)}{\&PartialD;\hat{x}}\&CenterDot;G=-{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i2}\right)\mathrm{\&Xi;}\left(\hat{q}\right)J^{-1}$

Diagonal matrix Λ (T) is:

$\mathrm{\&Lambda;}\left(T\right)=\left[\begin{array}{ccc}{\mathrm{\&Lambda;}}_w& 0& 0\\ 0& {\mathrm{\&Lambda;}}_{s1}& 0\\ 0& 0& {\mathrm{\&Lambda;}}_{s2}\end{array}\right]---\left(16\right)$

Wherein: Λ _w=TI _{3 * 3}, ${\mathrm{\&Lambda;}}_{s1}=\frac{T^2}{2}\&CenterDot;I_{3\&times;3},$ ${\mathrm{\&Lambda;}}_{s2}=\frac{T^2}{2}\&CenterDot;I_{3\&times;3},;$ Column vector For:

$z[\hat{x}\left(k\right),T]=\left[\begin{array}{c}{z}_{\mathrm{\&omega;}}\\ z_{s1}\\ z_{s2}\end{array}\right]=\left[\begin{array}{c}T\&CenterDot;{L^1}_f\left(g_{\mathrm{\&omega;}}\right)\\ T\&CenterDot;{L^1}_f\left(g_{s1}\right)+\frac{T^2}{2}\&CenterDot;{L^2}_f\left(g_{s1}\right)\\ T\&CenterDot;{L^1}_f\left(g_{s2}\right)+\frac{T^2}{2}\&CenterDot;{L^2}_f\left(g_{s2}\right)\end{array}\right]---\left(17\right)$

Each rank Lie derivative is in the following formula:

${L^1}_f\left(g_{\mathrm{\&omega;}}\right)=J^{-1}[-\widehat{\mathrm{\&omega;}}\&times;J\widehat{\mathrm{\&omega;}}+G_{\mathrm{gb}}]$

${L^1}_f\left(g_{s1}\right)=-{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i1}\right)\mathrm{\&Xi;}\left(\hat{q}\right)\widehat{\mathrm{\&omega;}}$

${L^2}_f\left(g_{s1}\right)=[\widehat{\mathrm{\&omega;}}\&times;]\&CenterDot;{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i1}\right)\mathrm{\&Xi;}\left(\hat{q}\right)\widehat{\mathrm{\&omega;}}-{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i1}\right)\mathrm{\&Xi;}\left(\hat{q}\right)\&times;J^{-1}[-\widehat{\mathrm{\&omega;}}\&times;J\widehat{\mathrm{\&omega;}}+G_{\mathrm{gb}}]$

${L^1}_f\left(g_{s2}\right)=-{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i2}\right)\mathrm{\&Xi;}\left(\hat{q}\right)\widehat{\mathrm{\&omega;}}$

${L^2}_f\left(g_{s2}\right)=[\widehat{\mathrm{\&omega;}}\&times;]\&CenterDot;{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i2}\right)\mathrm{\&Xi;}\left(\hat{q}\right)\widehat{\mathrm{\&omega;}}-{\mathrm{\&Xi;}}^T\left(\hat{q}\right)\mathrm{\&Gamma;}\left(l_{i2}\right)\mathrm{\&Xi;}\left(\hat{q}\right)\&times;J^{-1}[-\widehat{\mathrm{\&omega;}}\&times;J\widehat{\mathrm{\&omega;}}+G_{\mathrm{gb}}]$

Wherein: $\mathrm{\&Gamma;}\left(a\right)=\left[\begin{array}{cc}-[a\&times;]& -a\\ a^T& 0\end{array}\right];$

At last, obtain the estimated value of model error according to formula (13)

Step 4: utilize second order interpolation filtering to carry out state estimation to the model after the compensation, obtain the attitude of satellite;

Be specially:

With the model error that obtains in the step 3

Substitution state equation (7) compensates, and is write the state equation and the measurement equation of the Satellite Attitude Determination System model after the compensation as discrete non-linear form, for:

x _k+1＝f(x _k，d _G，w _k) (18)

y _k＝g(x _k，v _k)

If filter value

The error variance battle array

Square root be

Promptly

Be

Cholesky decompose, the square root of model error variance battle array Q is S _w, the square root of measuring the variance battle array R of noise is S _v, the variance battle array of status predication error

Square root be That is:

$Q=S_w{S}_w^T,$ $R=S_v{S}_v^T,$ (19)

$\stackrel{\&OverBar;}{P}={\stackrel{\&OverBar;}{S}}_x{\stackrel{\&OverBar;}{S}}_x^T,$ $\hat{P}={\hat{S}}_x{\hat{S}}_x^T.$

The one-step prediction of state and error variance battle array thereof:

${\stackrel{\&OverBar;}{x}}_{k+1=\frac{h^2-n_x-n_w}{h^2}f({\hat{x}}_k,{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)}$

$+\frac{1}{2h^2}\underset{p=1}{\overset{n_x}{\mathrm{\&Sigma;}}}(f({\hat{x}}_k+h{\hat{s}}_{x,p},{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)+f({\hat{x}}_k-h{\hat{s}}_{x,p},{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k))---\left(20\right)$

$+\frac{1}{2h^2}\underset{p=1}{\overset{n_v}{\mathrm{\&Sigma;}}}(f({\hat{x}}_{k,}{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k+h{\hat{s}}_{w,p})+f({\hat{x}}_{k,}{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k-h{\hat{s}}_{w,p}))$

Wherein,

The average of expression w (k), n _xThe dimension of expression state vector, n _wThe dimension of expression system noise, interpolation step-length h ²=3;

The Cholesky of status predication error variance battle array decomposes

Matrix after the Householder conversion:

$\stackrel{\_}{S}(k+1)=\left[S_{x\hat{x}}^{\left(1\right)}\left(k\right)S_{\mathrm{xw}}^{\left(1\right)}\left(k\right)S_{x\hat{x}}^{\left(2\right)}\left(k\right)S_{\mathrm{xw}}^{\left(2\right)}\left(k\right)\right]---\left(21\right)$

Wherein,

$S_{x\hat{x}}^{\left(1\right)}\left(k\right)=\left\{S_{x\hat{x}}^{\left(1\right)}{\left(k\right)}_{(i,j)}\right\}=\{(f_i({\hat{x}}_k+h{\hat{s}}_{x,j},{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)-f_i({\hat{x}}_k-h{\hat{s}}_{x,j},{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k))/2h\},$

$S_{\mathrm{xw}}^{\left(1\right)}\left(k\right)=\left\{S_{\mathrm{xw}}^{\left(1\right)}{\left(k\right)}_{(i,j)}\right\}=\{(f_i({\hat{x}}_k,{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k+h{\hat{s}}_{w,j})-f_i({\hat{x}}_k,{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k-h{\hat{s}}_{w,j}))/2h\},$

$S_{x\hat{x}}^{\left(2\right)}\left(k\right)=\left\{S_{x\hat{x}}^{\left(2\right)}{\left(k\right)}_{(i,j)}\right\}=\{\frac{\sqrt{h^2-1}}{2h^2}(f_i({\hat{x}}_k+h{\hat{s}}_{x,j},{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)$

$+f_i({\hat{x}}_k-h{\hat{s}}_{x,j},{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)-2f_i({\hat{x}}_k,{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)\left)\right\},$

$S_{xw}^{\left(2\right)}\left(k\right)=\left\{S_{xw}^{\left(2\right)}{\left(k\right)}_{(i,j)}\right\}=\{\frac{\sqrt{h^2-1}}{2h^2}(f_i({\hat{x}}_{k,}{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k+h{\hat{s}}_{w,j})$

$+f_i({\hat{x}}_{k,}{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k-h{\hat{s}}_{w,j})-2f_i({\hat{x}}_k,{\hat{d}}_G,{\stackrel{\&OverBar;}{w}}_k)\left)\right\},$

Wherein,

Expression

J row,

Expression

J row,

Expression S _wJ row; The one-step prediction of observed reading is:

${\stackrel{\&OverBar;}{y}}_{k+1}=\frac{h^2-n_x-n_v}{h^2}g({\stackrel{\&OverBar;}{x}}_{k+1},{\stackrel{\&OverBar;}{v}}_{k+1})$

$+\frac{1}{2h^2}\underset{p=1}{\overset{n_x}{\mathrm{\&Sigma;}}}(g({\stackrel{\&OverBar;}{x}}_{k+1}+h{\stackrel{\&OverBar;}{s}}_{x,p},{\stackrel{\&OverBar;}{v}}_{k+1})+g({\stackrel{\&OverBar;}{x}}_{k+1}-h{\stackrel{\&OverBar;}{s}}_{x,p},{\stackrel{\&OverBar;}{v}}_{k+1}))---\left(22\right)$

$+\frac{1}{2h^2}\underset{p=1}{\overset{n_v}{\mathrm{\&Sigma;}}}(g({\stackrel{\&OverBar;}{x}}_{k+1,}{\stackrel{\&OverBar;}{v}}_{k+1}+h{\stackrel{\&OverBar;}{s}}_{v,p})+g({\stackrel{\&OverBar;}{x}}_{k+1,}{\stackrel{\&OverBar;}{v}}_{k+1}-h{\stackrel{\&OverBar;}{s}}_{v,p}))$

Wherein, n _vBe to measure the noise dimension;

The Cholesky of error variance battle array to decompose be S _y(k+1) matrix after the Householder conversion:

$S_y(k+1)=\left[S_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}(k+1)S_{\mathrm{yv}}^{\left(1\right)}(k+1)S_{y\stackrel{\&OverBar;}{x}}^{\left(2\right)}(k+1)S_{\mathrm{yv}}^{\left(2\right)}(k+1)\right]---\left(23\right)$

Wherein,

$S_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}(k+1)=\left\{S_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}{(k+1)}_{(i,j)}\right\}=\{(g_i({\stackrel{\&OverBar;}{x}}_{k+1}+h{\stackrel{\&OverBar;}{s}}_{x,j},{\stackrel{\&OverBar;}{v}}_{k+1})-g_i({\stackrel{\&OverBar;}{x}}_{k+1}-h{\stackrel{\&OverBar;}{s}}_{x,j},{\stackrel{\&OverBar;}{v}}_{k+1}))/2h\},$

$S_{\mathrm{yv}}^{\left(1\right)}(k+1)=\left\{S_{\mathrm{yv}}^{\left(1\right)}{(k+1)}_{(i,j)}\right\}=\{(g_i({\stackrel{\&OverBar;}{x}}_{k+1,}{\stackrel{\&OverBar;}{v}}_{k+1}+h{\stackrel{\&OverBar;}{s}}_{v,j})-g_i({\stackrel{\&OverBar;}{x}}_{k+1,}{\stackrel{\&OverBar;}{v}}_{k+1}-h{\stackrel{\&OverBar;}{s}}_{v,j}))/2h\},$

$S_{y\stackrel{\&OverBar;}{x}}^{\left(2\right)}(k+1)=\left\{S_{y\stackrel{\&OverBar;}{x}}^{\left(2\right)}{(k+1)}_{(i,j)}\right\}=\{\frac{\sqrt{h^2-1}}{2h^2}(g_i({\stackrel{\&OverBar;}{x}}_{k+1}+h{\stackrel{\&OverBar;}{s}}_{x,j},{\stackrel{\&OverBar;}{v}}_{k+1})$

$+g_i({\stackrel{\&OverBar;}{x}}_{k+1}-h{\stackrel{\&OverBar;}{s}}_{x,j},{\stackrel{\&OverBar;}{v}}_{k+1})-2g_i({\stackrel{\&OverBar;}{x}}_{k+1},{\stackrel{\&OverBar;}{v}}_{k+1})\left)\right\},$

$S_{\mathrm{yv}}^{\left(2\right)}(k+1)=\left\{S_{\mathrm{yv}}^{\left(2\right)}{(k+1)}_{(i,j)}\right\}=\{\frac{\sqrt{h^2-1}}{2h^2}(g_i({\stackrel{\&OverBar;}{x}}_{k+1},{\stackrel{\&OverBar;}{v}}_{k+1}+h{\stackrel{\&OverBar;}{s}}_{v,j})$

$+g_i({\stackrel{\&OverBar;}{x}}_{k+1,}{\stackrel{\&OverBar;}{v}}_{k+1}-h{\stackrel{\&OverBar;}{s}}_{v,j})-2g_i({\stackrel{\&OverBar;}{x}}_{k+1},{\stackrel{\&OverBar;}{v}}_{k+1})\left)\right\},$

In the formula:

Expression S _vJ row;

The cross covariance battle array of state and measurement is:

$P_{\mathrm{xy}}(k+1)={\stackrel{\&OverBar;}{S}}_x(k+1)S_{y\stackrel{\&OverBar;}{x}}{(k+1)}^T---\left(24\right)$

Obtain gain matrix K by formula (24) _K+1For:

K _k+1＝P _xy(k+1)[S _y(k+1)S _y(k+1) ^T] ^-1 (25)

Be updated to based on state and the estimation error variance battle array measured:

${\hat{x}}_{k+1}={\stackrel{-}{x}}_{k+1}+K_{k+1}(y_{k+1}-{\stackrel{\&OverBar;}{y}}_{k+1})---\left(26\right)$

$\hat{P}(k+1)={[{\stackrel{\&OverBar;}{S}}_x(k+1)-K_{k+1}{S}_{y\stackrel{\&OverBar;}{x}}^{\left(1\right)}(k+1)][{\stackrel{\&OverBar;}{S}}_x(k+1)-K_{k+1}{S}_{\mathrm{yx}}^{\left(1\right)}(k+1)]}^T+K_{k+1}{S}_{\mathrm{yv}}^{\left(1\right)}(k+1){\left[K_{k+1}{S}_{\mathrm{yv}}^{\left(1\right)}(k+1)\right]}^T$

$+K_{k+1}{S}_{\mathrm{yx}}^{\left(2\right)}(k+1){\left[K_{k+1}{S}_{\mathrm{yx}}^{\left(2\right)}(k+1)\right]}^T+K_{k+1}{S}_{\mathrm{yv}}^{\left(2\right)}(k+1){\left[K_{k+1}{S}_{\mathrm{yv}}^{\left(2\right)}(k+1)\right]}^T---\left(27\right)$

Obtain the state estimation value thus

Wherein

Be the attitude quaternion of satellite, each attitude quaternion carried out normalized after estimating, and resolve according to formula (28) (29) (30) and to obtain attitude angle;

The angle of pitch: θ=-arcsin (2 _Q1q3-2 _Q2q4) (28)

Crab angle: $\mathrm{\&Psi;}=\arctan \left(\frac{2_{q2q3}-2_{q1q4}}{q_3^2+q_4^2-q_1^2-q_2^2}\right)---\left(29\right)$

Roll angle:

Images