CN103940433B - A kind of satellite attitude determination method based on the self adaptation square root UKF algorithm improved - Google Patents

Abstract

A kind of satellite attitude determination method based on the self adaptation square root UKF algorithm improved, belongs to the attitude of satellite and determines technical field.The present invention solves when Satellite Attitude Determination System is affected by uncertain interference and noise, owing to the instability of the too big caused Satellite Attitude Determination System of the rounding error of existing EKF, UKF and SRUKF algorithm numerical computations and and satellite virtual condition low to the precision of Satellite Attitude Estimation follow the tracks of the problems such as ability is weak.The main process that realizes of this satellite attitude determination method is: utilize the self adaptation square root UKF evaluated error quaternary number and gyroscopic drift error improved；Gyro to measure value and the gyroscopic drift error estimated is utilized to substitute into attitude kinematics equations calculating attitude quaternion；Utilize the error quaternion estimated that the attitude quaternion calculated is modified；Utilize the attitude quaternion revised to carry out attitude algorithm, determine the attitude of satellite.The present invention is applicable to the attitude of satellite and determines technical field.

Description

A kind of satellite attitude determination method based on the self adaptation square root UKF algorithm improved

Technical field

The present invention relates to the high-precision satellite attitude determination method of star sensor and gyro, belong to the attitude of satellite and determine technology Field.

Background technology

Star sensor and the attitude measurement system of gyro composition, owing to its precision is higher, thus be widely used in Satellite Attitude State determines in system.The nonlinear system formed for it, the method for attitude extensively should to use nonlinear filtering technique to determine With.The advantages such as extended Kalman filter (EKF) is simple because of its method, easy realization, in the engineering processing non-linear estimations Problem is widely used.But EKF is nonlinear equation to do first approximation process, and ignores remaining higher order term, thus by non- Linear problem is converted into linear problem.When mission nonlinear is stronger, EKF runs counter to local linear it is assumed that uncared-for higher order term Big error can be brought, cause EKF arithmetic accuracy to decline, even cause the filtering divergence of EKF algorithm.Additionally EKF is in linearisation Needing during process to calculate Jacobi (Jacobian) matrix, it calculates process very complicated and easily makes mistakes.

For the problems referred to above, Julier et al. proposes Wuji Kalman filter (UKF) algorithm.Relative to EKF, UKF Using UT conversion to approximate nonlinear probability Density Distribution, have and need not calculate Jacobian matrix, estimated accuracy is more Height, obtains extensively application in recent years in the middle of attitude of flight vehicle estimation problem.But often there is house in numerical computations in UKF Enter error, nonnegative definiteness and the symmetry of system estimation error co-variance matrix may be destroyed, cause convergence of algorithm speed Slowly, the instability of algorithm is even caused.Can preferably solve for this problem square root UKF (SRUKF) filtering algorithm, the party Method uses for reference square root factorization filter thought in kalman filtering, uses the QR of matrix to decompose and Cholesky during filtering Decomposition result is directly propagated and updates the square root of covariance matrix, solves and calculates the mistake that error may cause in UKF algorithm The negative definiteness problem of difference covariance, improves computational efficiency and the numerical stability of filtering algorithm.But, when numerical value accumulated error Too big or weightsW ₀ ^c When selecting inappropriate, the square root UKF of standard yet suffers from filtering divergence problem.Additionally, EKF, UKF and SRUKF is desirable that system model, and accurately and noise statistics is known.When system exists uncertainty interference and noise work Used time, said method does not the most possess preferable estimated accuracy, tracking ability and robustness.

Summary of the invention

The purpose of the present invention is to propose to a kind of attitude of satellite side of determination based on the self adaptation square root UKF algorithm improved Method, with solve Satellite Attitude Determination System by uncertainty interference and noise affected time, due to existing EKF, UKF with The instability of the Satellite Attitude Determination System that the rounding error of SRUKF algorithm numerical computations is too big caused and to the attitude of satellite Estimate precision is low and satellite virtual condition follows the tracks of the problems such as ability is weak.

The present invention solves that above-mentioned technical problem be the technical scheme is that

Of the present invention a kind of based on the satellite attitude determination method of self adaptation square root UKF algorithm improved, be by Realize according to following steps: step one, set up gyro to measure model；Step 2, set up satellite attitude kinematics equation；Step Three, the system state equation of the state variable formed based on error quaternion and gyroscopic drift error is set up；Step 4, foundation are by mistake Difference systematic observation equation；Step 5, the self adaptation square root UKF evaluated error quaternary number utilizing improvement and gyroscopic drift error； Step 6, utilize gyro to measure value and the gyroscopic drift error that estimates to substitute into attitude kinematics equations to calculate attitude quaternary Number；The attitude quaternion calculated is modified by the error quaternion that step 7, utilization estimate；Step 8, utilization are revised Attitude quaternion carry out attitude algorithm, determine the attitude of satellite.

The invention has the beneficial effects as follows:

One, present invention improves the stability of the Satellite Attitude Determination System caused due to the instability of existing algorithm, The stability making Satellite Attitude Determination System is more preferable.

Two, on the basis of the square root UKF improved, adaptive factor μ is introduced_k+1So that square root UKF has self adaptation Property, thus in the case of system has unknown disturbances, improve Satellite Attitude Estimation precision.

Three, present invention improves UKF and the square root UKF tracking ability to unknown disturbances and mutation status, make satellite real Border status tracking ability strengthens, thus also is able to when model uncertainty and noise statistics are uncertain preferably estimate The attitude of satellite, therefore more applicable in the middle of environment complicated and changeable.

Four, relative to STF method, the present invention (IASRUKF) improve in terms of the estimated accuracy of roll angle 66%～ 67%, in terms of the estimated accuracy of the angle of pitch, improve 68%～69%, in terms of the estimated accuracy of yaw angle, improve 58% ～%59；Relative to SRUKF method, the present invention improves 97%～98% in terms of the estimated accuracy of roll angle, at the angle of pitch Estimated accuracy aspect improve 97%～98%, in terms of the estimated accuracy of yaw angle, improve 97%～98%.

Accompanying drawing explanation

Fig. 1 is the flow chart of the present invention；Fig. 2 is the self adaptation square root UKF algorithm of the improvement of attitude of satellite error system Flow chart；Fig. 3 is that the roll angle error of the IASRUKF of STF (strong tracking filfer), SRUKF (square root UKF) and the present invention is bent Line comparison diagram；Fig. 4 is the angle of pitch error of the IASRUKF of STF (strong tracking filfer), SRUKF (square root UKF) and the present invention Curve comparison figure；Fig. 5 is that the yaw angle of the IASRUKF of STF (strong tracking filfer), SRUKF (square root UKF) and the present invention is missed Difference curve comparison figure.

Detailed description of the invention

Detailed description of the invention one: defending of a kind of based on improvement the self adaptation square root UKF algorithm described in present embodiment Star attitude determination method, it is characterised in that said method comprising the steps of:

Step one, set up gyro to measure model；

Step 2, set up satellite attitude kinematics equation；

The system state equation of the state variable that step 3, foundation form based on error quaternion and gyroscopic drift error；

Step 4, set up error system observational equation；

Step 5, the self adaptation square root UKF evaluated error quaternary number utilizing improvement and gyroscopic drift error；

Step 6, utilize gyro to measure value and the gyroscopic drift error that estimates to substitute into attitude kinematics equations to calculate appearance State quaternary number；

The attitude quaternion calculated is modified by the error quaternion that step 7, utilization estimate；

The attitude quaternion that step 8, utilization are revised carries out attitude algorithm, determines the attitude of satellite.

Detailed description of the invention two: present embodiment is unlike detailed description of the invention one: set up top described in step one The detailed process of spiral shell measurement model is: in the case of the coordinate system of gyro to measure coordinate system Yu celestial body is the same coordinate system, top Spiral shell measurement model is

$\left\{\begin{array}{c}g\left(t\right)=\mathrm{\ω}\left(t\right)+\mathrm{\β}\left(t\right)+{\mathrm{\η}}_u\left(t\right)\\ \stackrel{\·}{\mathrm{\β}\left(t\right)}={\mathrm{\η}}_v\left(t\right)\end{array}\right.---\left(1\right)$

In formula, g (t) is the measurement output valve of gyro, and ω (t) is gyro true angular velocity, and β (t) is gyroscopic drift, η_u (t) and η_vT () is orthogonal Gaussian noise, meet:

$\left\{\begin{array}{c}E\left\{{\mathrm{\η}}_u\left(t\right){\mathrm{\η}}_u^T\left(\mathrm{\τ}\right)\right\}={\mathrm{\σ}}_u^2\mathrm{\δ}(t-\mathrm{\τ})I_{3\×3}\\ E\left\{{\mathrm{\η}}_v\left(t\right){\mathrm{\η}}_v^T\left(\mathrm{\τ}\right)\right\}={\mathrm{\σ}}_v^2\mathrm{\δ}(t-\mathrm{\τ})I_{3\×3}\end{array}\right.---\left(2\right)$

In formula,WithIt is respectively white noise, η_u(t) and η_vT () is mean square deviation, δ (t-τ) is Dirac function.Other Step and parameter are identical with detailed description of the invention one.

Detailed description of the invention three: present embodiment is unlike detailed description of the invention one or two: building described in step 2 The detailed process of vertical satellite attitude kinematics equation is: attitude of satellite quaternary number is defined as

Q=[q₁ q₂ q₃ q₄]^T (3)

In formula,q₄=cos (θ/2),It is respectively unit rotating vector and the anglec of rotation with θ；

The satisfied following constraint of quaternary number:

$q_1^2+q_2^2+q_3^2+q_4^2=1---\left(4\right)$

Represent that satellite attitude kinematics equation is with quaternary number:

$\frac{\mathrm{dq}\left(t\right)}{\mathrm{dt}}=\frac{1}{2}\mathrm{\Ω}\left(\mathrm{\ω}\left(t\right)\right)q\left(t\right)---\left(5\right)$

In formula, ω (t) is the true angular velocity of satellite, and

$\mathrm{\Ω}\left(\mathrm{\ω}\left(t\right)\right)={\left[\begin{array}{cc}-{[\mathrm{\ω}\left(t\right)\×]}_{3\×3}& \mathrm{\ω}\left(t\right)\\ -{\mathrm{\ω}}^T\left(t\right)& 0\end{array}\right]}_{4\×4},[\mathrm{\ω}\left(t\right)\×]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z\left(t\right)& {\mathrm{\ω}}_y\left(t\right)\\ {\mathrm{\ω}}_z\left(t\right)& 0& -{\mathrm{\ω}}_x\left(t\right)\\ -{\mathrm{\ω}}_y\left(t\right)& {\mathrm{\ω}}_x\left(t\right)& 0\end{array}\right].$ Other step and parameter are identical with detailed description of the invention one or two.

Detailed description of the invention four: present embodiment is unlike one of detailed description of the invention one to three: described in step 3 The detailed process of the system state equation setting up the state variable formed based on error quaternion and gyroscopic drift error be: four There is norm constraint in unit's number, if selected four quaternion components as state variable, then variance matrix is unusual.Therefore In order to avoid the problem that variance matrix is unusual, multiplicative quaternion is used to define true quaternary number and quaternary number calculated valueBetween Error quaternion is state variable:

$\mathrm{\δq}={\hat{q}}^{-1}\⊗q=\left[\begin{array}{cc}\mathrm{\δe}& \mathrm{\δq}\end{array}\right]---\left(6\right)$

Take the vector section δ e of error quaternion and gyroscopic drift error delta b as state variable, the state variable of system For △ x=[δ e △ b], satellite attitude kinematics equation have:

$\stackrel{\·}{q}=\frac{1}{2}q\⊗\mathrm{\ω}---\left(7\right)$

$\stackrel{\stackrel{\·}{^}}{q}=\frac{1}{2}\hat{q}\⊗\widehat{\mathrm{\ω}}---\left(8\right)$

In formula,It it is the calculated value of Satellite Angle speed；

To formula (6) derivation, and formula (7) and formula (8) are substituted into formula (6):

$\mathrm{\δ}\stackrel{\·}{q}=\frac{1}{2}[\mathrm{\δq}\⊗\mathrm{\ω}-\widehat{\mathrm{\ω}}\⊗\mathrm{\δq}]---\left(9\right)$

Definition:

$\mathrm{\δ\ω}=[\mathrm{\ω}-\widehat{\mathrm{\ω}}]=[(b-\hat{b})+{\mathrm{\η}}_u]---\left(10\right)$

Formula (10) is substituted into formula (9) obtain:

$\mathrm{\δ}\stackrel{\·}{q}=\frac{1}{2}[\mathrm{\δq}\⊗\widehat{\mathrm{\ω}}-\widehat{\mathrm{\ω}}\⊗\mathrm{\δq}]+\frac{1}{2}\mathrm{\δq}\⊗\mathrm{\δ\ω}---\left(11\right)$

Formula (11) is linearized, has:

$\mathrm{\δ}\stackrel{\·}{q}=-\widehat{\mathrm{\ω}}\×\mathrm{\δe}-\frac{1}{2}(\mathrm{\Δb}+{\mathrm{\η}}_u)---\left(12\right)$

The system state equation that then can obtain error state variable is:

$\mathrm{\Δ}\stackrel{\·}{X}\left(t\right)=F\left(t\right)\mathrm{\ΔX}\left(t\right)+G\left(t\right)\mathrm{\ϵ}\left(t\right)---\left(13\right)$

In formula,

$F\left(t\right)=\left[\begin{array}{cc}-[\mathrm{\ω}\left(t\right)\×]& \frac{1}{2}{I}_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right],G\left(t\right)=\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right],\mathrm{\ϵ}\left(t\right)=\begin{array}{c}\left[\begin{array}{c}{\mathrm{\η}}_u\\ 0\end{array}\right]\end{array}.$ Other step and parameter are identical with one of detailed description of the invention one to three.

Detailed description of the invention five: present embodiment is unlike one of detailed description of the invention one to four: described in step 4 The detailed process setting up error system observational equation be: owing to using quaternary number q as output quantity, by star sensor The quaternary number error obtained, as measuring output, takes its vector section as observed quantity, has an observational equation:

Z=Hx+v (14)

In formula, H is observing matrix, and v is observation noise；

$H=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]$

The attitude error system equation using Fourth order Runge-Kutta to form formula (13) and formula (14) carries out discretization.Its Its step and parameter are identical with one of detailed description of the invention one to four.

Detailed description of the invention six: present embodiment is unlike one of detailed description of the invention one to five: described in step 5 Utilize the self adaptation square root UKF evaluated error quaternary number improved and the detailed process of gyroscopic drift error be:

(1) init stateState error covariance matrix square root S₀For

${\hat{X}}_0=E\left[X_0\right]---\left(15\right)$

$S_0=\mathrm{chol}\left(E\right[(X_0-{\hat{X}}_0{(X_0-{\hat{X}}_0)}^T)\left]\right)---\left(16\right)$

(2) for k=1,2,3 ..., it is as follows that n realizes step:

1. sigma point is calculated

${\mathrm{\ξ}}_k=\left[\begin{array}{ccc}{\hat{X}}_k& {\hat{X}}_k+\mathrm{\γ}{S}_k& {\hat{X}}_k-\mathrm{\γ}{S}_k\end{array}\right]---\left(17\right)$

In formula,λ=α²The value of (n+ κ)-n, α is between 0.001～1, and n is the dimension of system mode vector Number, κ is the 3rd scale factor, and value is 0；

2. the time updates

The nonlinear propagation of Sigma sampled point:

ξ_i,k+1|k=F_k+1|kξ_i,k (18)

The one-step prediction value of state value calculates:

${\hat{X}}_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^m{\mathrm{\ξ}}_{k+1|k}---\left(19\right)$

The one-step prediction of observation calculates:

${\hat{Z}}_{k+1|k}=H{\hat{X}}_{k+1|k}---\left(20\right)$

Predicted value and the residual computations of actual value:

$e_{k+1}=Z_{k+1}-{\hat{Z}}_{k+1|k}---\left(21\right)$

3. self adaptation fading factor μ is calculated_k+1

${\mathrm{\&mu;}}_{k+1,i}=\left\{\begin{array}{c}{\mathrm{\&mu;}}_{k+1,i},{\mathrm{\&mu;}}_{k+1,i}\&GreaterEqual;1\\ 1,{\mathrm{\&mu;}}_{k+1,i}<1\end{array}\right.,i=\mathrm{1,2},...,n---\left(22\right)$

μ_k+1,i=trace (N_k+1)/trace(M_ii,k+！) (23)

N_k+1=V_k+1-HQ_k+1H^T-lR_k+1 (24)

M_k+1=J_k+1HH^T (25)

$J_{k+1}=\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^c({\mathrm{\&xi;}}_{k+1|k,i}-{\mathrm{\&xi;}}_{0,k+1|k}){({\mathrm{\&xi;}}_{k+1|k,i}-{\mathrm{\&xi;}}_{0,k+1|k})}^T---\left(26\right)$

$V_{k+1}=\left\{\begin{array}{c}{e}_{k+1}{e}_{k+1}^T,k=0\\ \frac{\mathrm{\&rho;}{V}_k+e_{k+1}{e}_{k+1}^T}{1+\mathrm{\&rho;}},k\&GreaterEqual;1\end{array}\right.---\left(27\right)$

4. measurement updaue

The square root of state one-step prediction covariance matrix calculates:

$S_{k+1|k}=\mathrm{qr}\left\{\right[\mathrm{diag}\left({\mathrm{\&mu;}}_{k+1}\right)\sqrt{W_1^c}({\mathrm{\&xi;}}_{1:2n,k+1|k}-{\mathrm{\&xi;}}_{0,k+1|k}),G_k\sqrt{Q_k}\left]\right\}---\left(28\right)$

Cross-covariance calculates:

$P_{\mathrm{XZ},k+1}=\left(S_{k+1|k}{S}_{k+1|k}^T\right)H^T---\left(29\right)$

The square root of output covariance matrix calculates:

U_k+1=HS_k+1|k (30)

$S_{Z,k+1}=\mathrm{qr}\left\{\right[U_{k+1},\sqrt{R_{k+1}}\left]\right\}---\left(31\right)$

State gain matrix calculates:

$K_{k+1}=(P_{\mathrm{XZ},k+1}/S_{Z,k+1}^T)/S_{Z,k+1}---\left(32\right)$

State estimation calculates:

${\hat{X}}_{k+1|k+1}={\hat{X}}_{k+1|k}+K_{k+1}{e}_{k+1}---\left(33\right)$

The square root of state error covariance matrix calculates:

$S_{k+1}=S_{k+1|k}-S_{k+1|k}{U}_{k+1}^T{S}_{z,k+1}^{-T}{(S_{z,k+1}+\sqrt{R_{k+1}})}^{-1}{U}_{k+1}---\left(34\right)$

The parameter used in said process is calculated as follows:

$W_0^m=\frac{\mathrm{\&lambda;}}{n+\mathrm{\&lambda;}}---\left(35\right)$

$W_0^c=W_0^m+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})---\left(36\right)$

Other step and parameter are identical with one of detailed description of the invention one to five.

Detailed description of the invention seven: present embodiment is unlike one of detailed description of the invention one to six: described in step 8 Attitude algorithm method be:

θ=arcsin (-2 (q₁q₃-q₂q₄)) (39)

$\mathrm{\&psi;}=\arctan (2(q_2{q}_1+q_3{q}_4)/(1-2(q_3^2+q_2^2))---\left(40\right)$

In formula,θ, ψ represent roll angle, the angle of pitch and yaw angle respectively.Other step and parameter and detailed description of the invention One of one to six identical.

In order to advantages of the present invention is described, by the present invention and strong tracking filfer (STF) method and square root UKF (SRUKF) method compares, and simulation parameter is set to: the initial attitude angular speed of satellite is: ω₀=10^-2×[2 2 2]^TDeg/s, gyroscopic drift is: β₀=[0.05 0.05 0.05]^TDeg/s, the measurement noise of gyro and the mean square deviation of drift noise It is respectively as follows: σ_u=0.5 (°)/h and σ_v=0.04 (°)/h, the mean square deviation measuring noise is: σ_q=10 ", the filtering sampling time is △ T=1s, simulation time is 200s when, adds mutation disturbance: △ x=[0,0,0,0.02,0.02,0.02]^T, in emulation The when that time being 500s, the noise statistics of system is expanded 20 times.Obtain the simulation experiment result as shown in Fig. 3～Fig. 5. By the analysis to the simulation experiment result, the estimated accuracy of the present invention is best, relative to STF method, the present invention (IASRUKF) in terms of the estimated accuracy of roll angle, improve 66%～67%, improve in terms of the estimated accuracy of the angle of pitch 68%～69%, in terms of the estimated accuracy of yaw angle, improve 58%～%59；Relative to SRUKF method, the present invention is in rolling The estimated accuracy aspect of corner improves 97%～98%, improves 97%～98% in terms of the estimated accuracy of the angle of pitch, The estimated accuracy aspect of yaw angle improves 97%～98%.

Claims (6)

1. a satellite attitude determination method based on the self adaptation square root UKF algorithm improved, it is characterised in that described method Comprise the following steps:

Step one, set up gyro to measure model；

Step 2, set up satellite attitude kinematics equation；

The system state equation of the state variable that step 3, foundation form based on error quaternion and gyroscopic drift error；

Step 4, set up error system observational equation；

Step 5, the self adaptation square root UKF evaluated error quaternary number utilizing improvement and gyroscopic drift error；

Step 6, utilize gyro to measure value and the gyroscopic drift error that estimates to substitute into attitude kinematics equations to calculate attitude four Unit's number；

The attitude quaternion calculated is modified by the error quaternion that step 7, utilization estimate；

The attitude quaternion that step 8, utilization are revised carries out attitude algorithm, determines the attitude of satellite；Wherein,

The detailed process setting up gyro to measure model described in step one is: the coordinate system at gyro to measure coordinate system with celestial body is In the case of the same coordinate system, gyro to measure model is

$\left\{\begin{array}{c}g\left(t\right)=\mathrm{\&omega;}\left(t\right)+\mathrm{\&beta;}\left(t\right)+{\mathrm{\&eta;}}_u\left(t\right)\\ \stackrel{\&CenterDot;}{\mathrm{\&beta;}\left(t\right)}={\mathrm{\&eta;}}_v\left(t\right)\end{array}\right.---\left(1\right)$

In formula, g (t) is the measurement output valve of gyro, and ω (t) is gyro true angular velocity, and β (t) is gyroscopic drift, η_u(t) and η_v T () is orthogonal Gaussian noise, meet:

$\left\{\begin{array}{c}E\left\{{\mathrm{\&eta;}}_u\left(t\right){\mathrm{\&eta;}}_u^T\left(\mathrm{\&tau;}\right)\right\}={\mathrm{\&sigma;}}_u^2\mathrm{\&delta;}(t-\mathrm{\&tau;})I_{3\&times;3}\\ E\left\{{\mathrm{\&eta;}}_v\left(t\right){\mathrm{\&eta;}}_v^T\left(\mathrm{\&tau;}\right)\right\}={\mathrm{\&sigma;}}_v^2\mathrm{\&delta;}(t-\mathrm{\&tau;})I_{3\&times;3}\end{array}\right.---\left(2\right)$

In formula,WithIt is respectively white noise, η_u(τ) and η_v(τ) being mean square deviation, δ (t-τ) is Dirac function.

A kind of satellite attitude determination method based on the self adaptation square root UKF algorithm improved the most according to claim 1, It is characterized in that the detailed process setting up satellite attitude kinematics equation described in step 2 is: attitude of satellite quaternary number is defined For

Q=[q₁ q₂ q₃ q₄]^T (3)

In formula,q₄=cos (θ/2),It is respectively unit rotating vector and the anglec of rotation with θ；

The satisfied following constraint of quaternary number:

$q_1^2+q_2^2+q_3^2+q_4^2=1---\left(4\right)$

Represent that satellite attitude kinematics equation is with quaternary number:

$\frac{dq\left(t\right)}{dt}=\frac{1}{2}\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right(t\left)\right)q\left(t\right)---\left(5\right)$

In formula, ω (t) is the true angular velocity of satellite, and

$\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right(t\left)\right)={\left[\begin{array}{cc}-{\&lsqb;\mathrm{\&omega;}\left(t\right)\&times;\&rsqb;}_{3\&times;3}& \mathrm{\&omega;}\left(t\right)\\ -{\mathrm{\&omega;}}^T\left(t\right)& 0\end{array}\right]}_{4\&times;4},\&lsqb;\mathrm{\&omega;}\left(t\right)\&times;\&rsqb;=\left[\begin{array}{ccc}0& -{\mathrm{\&omega;}}_z\left(t\right)& {\mathrm{\&omega;}}_y\left(t\right)\\ {\mathrm{\&omega;}}_z\left(t\right)& 0& -{\mathrm{\&omega;}}_x\left(t\right)\\ -{\mathrm{\&omega;}}_y\left(t\right)& {\mathrm{\&omega;}}_x\left(t\right)& 0\end{array}\right].$

A kind of satellite attitude determination method based on the self adaptation square root UKF algorithm improved the most according to claim 2, It is characterized in that the system shape setting up the state variable formed based on error quaternion and gyroscopic drift error described in step 3 The detailed process of state equation is: use multiplicative quaternion to define true quaternary number and quaternary number calculated valueBetween error quaternary Number is state variable:

$\mathrm{\&delta;}q={\hat{q}}^{-1}\&CircleTimes;q=\left[\begin{array}{cc}\mathrm{\&delta;}e& \mathrm{\&delta;}q\end{array}\right]---\left(6\right)$

Taking the vector section δ e of error quaternion and gyroscopic drift error delta b as state variable, the state variable of system is Δ x =[δ e Δ b], is had by satellite attitude kinematics equation:

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

$\stackrel{\&CenterDot;}{\hat{q}}=\frac{1}{2}\hat{q}\&CircleTimes;\widehat{\mathrm{\&omega;}}---\left(8\right)$

In formula,It it is the calculated value of Satellite Angle speed；

To formula (6) derivation, and formula (7) and formula (8) are substituted into formula (6) after derivation:

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{q}=\frac{1}{2}\&lsqb;\mathrm{\&delta;}q\&CircleTimes;\mathrm{\&omega;}-\widehat{\mathrm{\&omega;}}\&CircleTimes;\mathrm{\&delta;}q\&rsqb;---\left(9\right)$

Definition:

$\mathrm{\&delta;}\mathrm{\&omega;}=\&lsqb;\mathrm{\&omega;}-\widehat{\mathrm{\&omega;}}\&rsqb;=\&lsqb;(b-\hat{b})+{\mathrm{\&eta;}}_u\&rsqb;---\left(10\right)$

Formula (10) is substituted into formula (9) obtain:

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{q}=\frac{1}{2}\&lsqb;\mathrm{\&delta;}q\&CircleTimes;\widehat{\mathrm{\&omega;}}-\widehat{\mathrm{\&omega;}}\&CircleTimes;\mathrm{\&delta;}q\&rsqb;+\frac{1}{2}\mathrm{\&delta;}q\&CircleTimes;\mathrm{\&delta;}\mathrm{\&omega;}---\left(11\right)$

Formula (11) is linearized, has:

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{q}=-\widehat{\mathrm{\&omega;}}\&times;\mathrm{\&delta;}e-\frac{1}{2}(\mathrm{\&Delta;}b+{\mathrm{\&eta;}}_u)---\left(12\right)$

The system state equation that then can obtain error state variable is:

$\mathrm{\&Delta;}\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)\mathrm{\&Delta;}X\left(t\right)+G\left(t\right)\mathrm{\&epsiv;}\left(t\right)---\left(13\right)$

In formula,

$F\left(t\right)=\left[\begin{array}{cc}-\&lsqb;\mathrm{\&omega;}\left(t\right)\&times;\&rsqb;& \frac{1}{2}{I}_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right],G\left(t\right)=\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& I_{3\&times;3}\end{array}\right],\mathrm{\&epsiv;}\left(t\right)=\left[\begin{array}{c}{\mathrm{\&eta;}}_u\\ 0\end{array}\right].$

A kind of satellite attitude determination method based on the self adaptation square root UKF algorithm improved the most according to claim 3, It is characterized in that the detailed process setting up error system observational equation described in step 4 is: owing to using quaternary number q As output quantity, the quaternary number error obtained by star sensor, as measuring output, takes its vector section as observed quantity, has sight Survey equation:

Z=Hx+v (14)

In formula, H is observing matrix, and v is observation noise；

$H=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]$

The attitude error system equation using Fourth order Runge-Kutta to form formula (13) and formula (14) carries out discretization.

A kind of satellite attitude determination method based on the self adaptation square root UKF algorithm improved the most according to claim 4, It is characterized in that self adaptation square root UKF evaluated error quaternary number and gyroscopic drift error that utilizing described in step 5 is improved Detailed process is:

(1) init stateState error covariance matrix square root S₀For

${\hat{X}}_0=E\&lsqb;X_0\&rsqb;---\left(15\right)$

$S_0=chol(E\&lsqb;(X_0-{\hat{X}}_0){(X_0-{\hat{X}}_0)}^T\&rsqb;)---\left(16\right)$

(2) for k=1,2,3 ..., it is as follows that n realizes step:

1. sigma point is calculated

${\mathrm{\&xi;}}_k=\left[\begin{array}{ccc}{\hat{X}}_k& {\hat{X}}_k+{\mathrm{\&gamma;S}}_k& {\hat{X}}_k-{\mathrm{\&gamma;S}}_k\end{array}\right]---\left(17\right)$

In formula,λ=α²The value of (n+ κ)-n, α is between 0.001～1, and n is the dimension of system mode vector, κ Being the 3rd scale factor, value is 0；

2. the time updates

The nonlinear propagation of Sigma sampled point:

ξ_i,k+1|k=F_k+1|kξ_i,k (18)

The one-step prediction value of state value calculates:

${\hat{X}}_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^m{\mathrm{\&xi;}}_{k+1|k}---\left(19\right)$

The one-step prediction of observation calculates:

${\hat{Z}}_{k+1|k}=H{\hat{X}}_{k+1|k}---\left(20\right)$

Predicted value and the residual computations of actual value:

$e_{k+1}=Z_{k+1}-{\hat{Z}}_{k+1|k}---\left(21\right)$

3. self adaptation fading factor μ is calculated_k+1

${\mathrm{\&mu;}}_{k+1,i}=\left\{\begin{array}{c}{\mathrm{\&mu;}}_{k+1,i},{\mathrm{\&mu;}}_{k+1,i}\&GreaterEqual;1\\ 1,{\mathrm{\&mu;}}_{k+1,i}<1\end{array}\right.,i=1,2,...,n---\left(22\right)$

μ_k+1,i=trace (N_k+1)/trace(M_ii,k+！) (23)

N_k+1=V_k+1-HQ_k+1H^T-lR_k+1 (24)

M_k+1=J_k+1HH^T (25)

$J_{k+1}=\underset{i=1}{\overset{2n}{\&Sigma;}}{W}_i^c({\mathrm{\&xi;}}_{k+1|k,i}-{\mathrm{\&xi;}}_{0,k+1|k}){({\mathrm{\&xi;}}_{k+1|k,i}-{\mathrm{\&xi;}}_{0,k+1|k})}^T---\left(26\right)$

$V_{k+1}=\left\{\begin{array}{c}{e}_{k+1}{e}_{k+1}^T,k=0\\ \frac{{\mathrm{\&rho;V}}_k+e_{k+1}{e}_{k+1}^T}{1+\mathrm{\&rho;}},k\&GreaterEqual;1\end{array}\right.---\left(27\right)$

4. measurement updaue

The square root of state one-step prediction covariance matrix calculates:

$S_{k+1|k}=qr\{\&lsqb;diag\left({\mathrm{\&mu;}}_{k+1}\right)\sqrt{W^c}({\mathrm{\&xi;}}_{1:2n,k+1|k}-{\mathrm{\&xi;}}_{0,k+1|k}),G_k\sqrt{Q_k}\&rsqb;\}---\left(28\right)$

Cross-covariance calculates:

$P_{XZ,k+1}=\left(S_{k+1|k}{S}_{k+1|k}^T\right)H^T---\left(29\right)$

The square root of output covariance matrix calculates:

U_k+1=HS_k+1|k (30)

$S_{Z,k+1}=qr\{\&lsqb;U_{k+1},\sqrt{R_{k+1}}\&rsqb;\}---\left(31\right)$

State gain matrix calculates:

$K_{k+1}=(P_{XZ,k+1}/S_{Z,k+1}^T)/S_{Z,k+1}---\left(32\right)$

State estimation calculates:

${\hat{X}}_{k+1|k+1}={\hat{X}}_{k+1|k}+K_{k+1}{e}_{k+1}---\left(33\right)$

The square root of state error covariance matrix calculates:

$S_{k+1}=S_{k+1|k}-S_{k+1|k}{U}_{k+1}^T{S}_{z,k+1}^{-T}{(S_{z,k+1}+\sqrt{R_{k+1}})}^{-1}{U}_{k+1}---\left(34\right)$

The parameter used in said process is calculated as follows:

$W_0^m=\frac{\mathrm{\&lambda;}}{n+\mathrm{\&lambda;}}---\left(35\right)$

$W_0^c=W_0^m+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})---\left(36\right)$

$W_i^c=W_i^m=\frac{1}{2(n+\mathrm{\&lambda;})}---\left(37\right).$

A kind of satellite attitude determination method based on the self adaptation square root UKF algorithm improved the most according to claim 5, It is characterized in that the attitude algorithm method described in step 8 is:

θ=arcsin (-2 (q₁q₃-q₂q₄)) (39)

$\mathrm{\&psi;}=\arctan \left(2\right(q_2{q}_1+q_3{q}_4)/(1-2\left(q_3^2+q_2^2\right))---\left(40\right)$

In formula,θ, ψ represent roll angle, the angle of pitch and yaw angle respectively.