CN103149936A - Combined attitude determination method for UPF (user port function) algorithm optimized by DNA (deoxyribonucleic acid) algorithm - Google Patents

Abstract

The invention discloses a combined attitude determination method for a UPF (user port function) algorithm optimized by a DNA (deoxyribonucleic acid) algorithm. The method comprises the following steps of compensating inertial measurement information by adopting gyro output data, and obtaining carrier attitude information through an attitude solution; obtaining special interval astronomic attitude information by utilizing astronomic measurement information and through a determination algorithm; and finally fusing the astronomic attitude information and the carrier attitude information by utilizing a UPF algorithm optimized by the DNA algorithm, solving system nonlinear and noise non-Gaussian problems, solving high precision carrier attitude information, estimating gyro drift, and feeding back and correcting the carrier attitude and compensating the gyro drift; finally realizing eliminating inert in real-time and on-line correction for the gyro random error of the astronomic combination navigation system based on the astronomic measurement information, and finishing long-term and high-precision combined attitude determination for a spacecraft..

Description

A kind of based on the integrated attitude determination method by the UPF algorithm of DNA algorithm optimization

Technical field

The present invention relates to the integrated attitude determination technical field of spacecraft, particularly a kind of based on the integrated attitude determination method by the UPF algorithm of DNA algorithm optimization, can be used for the high precision integrated attitude determination of various spacecrafts.

Background technology

For satisfying the active demand of space-based earth observation, weapon precision strike and space exploration exploitation, the spacecrafts such as all kinds of earth satellites, deep space probe, manned spaceship, ballistic missile and carrier rocket must possess autonomous operation and managerial ability, and high-precision autonomous attitude determination is the core technology bottleneck of spacecraft autonomous operation and management.At present, the high-precision independent of spacecraft is decided appearance, can't rely on any navigation means independently to realize.The pure-inertial guidance system can provide continuously independently, in real time, comprehensive navigation information, and precision is high in short-term, but its error accumulates with the working time, is difficult to satisfy the long-time high-precision fixed appearance requirement of spacecraft; Celestial navigation can provide high-precision attitude information, and error is accumulation in time not, but is subject to the weather conditions restriction, and output information is discontinuous; The two being combined, has complementary advantages, consist of inertia/astronomical integrated attitude determination system, is to realize that spacecraft is long-time, the most effectively means of high-precision fixed appearance.

At inertia/astronomical integrated attitude determination technical elements, all adopted EKF EKF (Extended Kalman Filter) method, but EKF is only applicable to filtering error and the very little situation of predicated error in the past.The Unscented Kalman filtering UKF (Unscented Kalman Filter) that proposes in recent years is the improvement algorithm of a kind of EKF, effectively solved the nonlinear problem of system, but its deficiency is the system that is not suitable for the non-Gaussian distribution of noise.Particle filter (Particle Filter, PF) embody increasing superiority owing to adopting Monte Carlo sampling (Monte Carlo sampling) structure on non-linear, non-Gaussian Systems status tracking, but its shortcoming is to have degradation phenomena, eliminates degradation phenomena and mainly depends on two gordian techniquies: suitably choose the importance density function and resample.For the former improving one's methods, can use EKPF (Extented Kalman Particle Filter), without mark particle filter (Unscented Particle Filter, UPF) carry out the selection of importance density function, wherein the UPF algorithm is to utilize UKF to obtain a kind of particle filter method of particle importance density function, owing to having comprised up-to-date measurement information in this importance density function, therefore has better performance.for improving one's methods of the latter, resampling algorithm commonly used has cumulative distribution resampling (Binary search), system's resampling (Systematic resampling), residual gravity sampling (Residual resampling) etc., these algorithms have solved the degenerate problem of particle by the validity that increases particle, but can affect in actual applications the robustness of system, after resampling is completed, the particle that importance degree is high is repeatedly chosen by resampling, this has lost the diversity of particle to a certain extent, in case the consequence that causes thus is that track rejection or tracking accuracy are inadequate, the possibility that system restrains automatically is very little, for this reason, a lot of scholars have proposed genetic particle filtering (GPF) algorithm, although the GPF algorithm has increased again the diversity of particle when guaranteeing particle validity, still there is the problem of the slow and poor robustness of filtering speed.

Summary of the invention

Technology of the present invention is dealt with problems and is: overcome the deficiencies in the prior art, propose a kind of based on the integrated attitude determination method by the UPF algorithm of DNA algorithm optimization, non-linear and the non-Gauss's problem of noise of resolution system, with the high-precision attitude information of quick acquisition, and can estimate exactly gyroscopic drift, realize that all kinds spacecraft is long-time, high-precision integrated attitude determination.

Principle of the present invention is: at first utilize gyro output data that the moment of inertia measurement information is compensated, by attitude algorithm, obtain attitude of carrier information; Next utilizes astronomical measurement information, obtains the astronomical attitude information of specific interval by deterministic algorithm; Utilize at last the UPF algorithm by the DNA algorithm optimization that astronomical attitude information is merged mutually with attitude of carrier information, non-linear and the non-Gauss's problem of noise of resolution system, find the solution high precision attitude of carrier information, estimate gyroscopic drift, and feedback compensation attitude of carrier and compensation gyroscopic drift compensation; Realize that finally spacecraft is long-time, high-precision integrated attitude determination.

Technical solution of the present invention is:

A kind ofly it is characterized in that based on the integrated attitude determination method by the UPF algorithm of DNA algorithm optimization, said method comprising the steps of:

Step 1, utilize inertial gyroscope to measure the inertia measurement data, utilize gyro output data that described inertia measurement data are compensated, then by attitude algorithm, obtain attitude of carrier information;

Step 2, utilize astronomical measurement information, by deterministic algorithm, find the solution astronomical attitude information, described astronomical attitude information comprises course, pitching and roll three attitude angle information;

Step 3, utilize the UPF algorithm of DNA algorithm optimization that astronomical attitude information is merged mutually with attitude of carrier information, find the solution high-precision attitude of carrier information, estimate gyroscopic drift, and feedback compensation attitude of carrier information and gyroscopic drift is compensated, finally complete the high precision integrated attitude determination to spacecraft.

Wherein, described step 3 specifically comprises the following steps:

(1) during t=0, initialization:

To initial priori probability density p (x ₀) sample, generate N and obey p (x ₀) particle that distributes

Its average and variance satisfy:

${\stackrel{\‾}{x}}_0^{\left(i\right)}=E\left[x_0^{\left(i\right)}\right]$

$P_0^{\left(i\right)}=E\left[(x_0^{\left(i\right)}-{\stackrel{\‾}{x}}_0^{\left(i\right)}){(x_0^{\left(i\right)}-{\stackrel{\‾}{x}}_0^{\left(i\right)})}^T\right];$

(2) t 〉=1 o'clock, step is as follows:

1. sampling step

With Unscented Kalman filtering new particle more Obtain

Sampling ${\hat{x}}_k^{\left(i\right)}~$ $q\left(x_k^{\left(i\right)}\right|x_{.0:k-1}^{\left(i\right)},y_{1:k})=N({\stackrel{\‾}{x}}_k^{\left(i\right)},P_k^{\left(i\right)}),i=1,\mathrm{\Λ},N;$

2. Determining Weights

${\tilde{w}}_k^{\left(i\right)}={\tilde{w}}_{k-1}^{\left(i\right)}\frac{p\left(y_k\right|{\hat{x}}_k^{\left(i\right)})p\left({\hat{x}}_k^{\left(i\right)}{|x_{k-1}^{\left(i\right)}}^{}\right)}{q\left({\hat{x}}_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},y_{1:k})}$

Normalized weight: ${w_k}^{\left(i\right)}={w_k^{\%}}^{\left(i\right)}/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w_k^{\%}}^{\left(i\right)};$

3. step resamples

From discrete distribution

In carry out N time and resample, obtain one group of new particle

Be still p (x _k| y _0:k) approximate representation;

4. organize new particle from this that obtains by the DNA algorithm and choose excellent particle, pick out the low particle that waits, to solve the exhausted problem of particle, the step of utilizing the DNA algorithm to be optimized is as follows:

The a initialization

It is N that order allows the number of times of iteration _max, DNA soup capacity is N, iteration count N _c=1, recombination probability P is set _cWith the variation probability P _m, dimension is n;

B produces N DNA encoding sequence at random according to the DNA encoding strategy, and the formation capacity is the DNA soup of N;

C obtains X according to decoding policy decoding dna coding one by one _j, described X _jBe the state variable of corresponding random N the DNA encoding sequence that produces, calculate each individual adaptive value F (DNA according to following formula _i), DNA _i∈ D, D are the set of DNA decoding institute total, make N _cEqual N _c+ 1, if N _c＞N _max, turn step f;

F(DNA _i)＝C-f(X _j)

In formula, C preferably gets f (X _j) maximal value; f(X _j) be fitness function, with

Relevant;

D carries out descending sort with N individuality according to the fitness value size, obtains: { F ₁, F ₂, L, F _N, F wherein _iBe equivalent to F (DNA _i), and F is arranged ₁＞F ₂＞L＞F _N

E forms daughter DNA soup according to selecting with the replicate run strategy, and to DNA encoding sequence in daughter DNA soup recombinate, variation and inversion operation, then turn step c;

F makes F _max=max (F ₁, F ₂, L, F _N), f (X _j)=C-F _maxBe minimum, i.e. f (X _j) middle corresponding X value, be optimum solution;

5. the X value is the optimal estimation value of state variable, includes attitude information and the gyroscopic drift information of carrier, directly exports the optimal estimation value of attitude of carrier;

According to the minimum variance criterion, the optimal estimation value of attitude of carrier

Be exactly the average that condition distributes, be shown below:

${\hat{x}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i{x}_k^i$

p _kBe the k state variance battle array in step, be shown below:

$p_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i(x_k^i-{\hat{x}}_k){(x_k^i-{\hat{x}}_k)}^T.$

Preferably, described step 1 specifically comprises the following steps:

Step 1.1. sets initial attitude Calculate initial attitude hypercomplex number q (0) battle array:

Step 1.2. upgrades the attitude quaternion matrix:

$q(n+1)=\{\cos \frac{\mathrm{\Δ\φ}}{2}I+\frac{\sin \frac{\mathrm{\Δ\φ}}{2}}{\mathrm{\Δ\φ}}[\mathrm{\Δ\Φ}\left]\right\}q\left(n\right)$

Wherein, n is the n moment, and I is unit quaternion, Δ φ=[Δ φ _XΔ φ _YΔ φ _Z], described Δ φ is the gyro output angle increment, [ΔΦ] is:

$\left[\mathrm{\Δ\Φ}\right]=\left[\begin{array}{cccc}0& -\mathrm{\Δ}{\mathrm{\φ}}_X& {-\mathrm{\Δ\φ}}_Y& {-\mathrm{\Δ\φ}}_Z\\ {\mathrm{\Δ\φ}}_X& 0& {\mathrm{\Δ\φ}}_Z& {-\mathrm{\Δ\φ}}_Y\\ {\mathrm{\Δ\φ}}_Y& {-\mathrm{\Δ\φ}}_Z& 0& {\mathrm{\Δ\φ}}_X\\ {\mathrm{\Δ\φ}}_Z& {\mathrm{\Δ\φ}}_Y& {-\mathrm{\Δ\φ}}_X& 0\end{array}\right];$

Step 1.3. is by q (n+1)=[q ₁q ₂q ₃q ₄] ^T, calculating attitude cosine battle array C is:

$C=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]=\left[\begin{array}{ccc}{q}_4^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_4{q}_3)& 2(q_1{q}_3-q_4{q}_2)\\ 2(q_1{q}_2-q_4{q}_3)& q_4^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_4{q}_1)\\ 2(q_1{q}_3+q_4{q}_2)& 2(q_2{q}_3-q_4{q}_1)& q_4^2-q_1^2-q_2^2+q_3^2\end{array}\right];$

Step 1.4. finds the solution attitude of carrier information by attitude cosine battle array C, and described attitude of carrier information comprises course, pitching and roll three attitude angle information.

Preferably, the course in step 1.4, pitching and roll three attitude angle

Solution formula as follows:

The pitching angle theta value is: θ=sin ^-1(C ₂₃);

Course angle

Being calculated as follows shown in table of value:

Being calculated as follows shown in table of roll angle γ value:

C 13The value judgement	C 33The value judgement	Roll angle γ value
			＝0	＜0	-π
＞0	＜0	atan -1(-C 13/C 33)-π
			＞0	＝0	-π/2
Arbitrary value	＞0	atan -1(-C 13/C 33)
			＜0	＝0	π/2
＜0	＜0	atan -1(-C 13/C 33)+π

。

Preferably, described step 2 specifically comprises the following steps:

The matrix w of step 2.1. definition 3 * 3, v, B and S, 3 * 1 column vector z, a and scalar σ, 4 * 1 column vector q, $B=\underset{k=1}{\overset{3}{\mathrm{\Σ}}}{a}_i{w}_i{v}_i^T,$ S＝B+B ^T， $z=\underset{i=1}{\overset{3}{\mathrm{\Σ}}}{a}_i(w_i\×v_i),$ σ＝tr(B)；

Wherein,

W=[w ₁w ₂w ₃], the coordinate vector of starlight in the star sensor coordinate system of three stars that it is observed constantly for k, v=[v ₁v ₂v ₃], it is the k reference vector of starlight in geocentric inertial coordinate system of these three stars constantly, a=[a ₁a ₂a ₃] ^T, it is non-negative weighting coefficient, q=[q ₁q ₂q ₃q ₄] ^T, it is attitude quaternion to be found the solution; Concrete:

$w=\left[w_1{w}_2{w}_3\right]=\left[\begin{array}{ccc}{w}_{1x}& w_{2x}& w_{3x}\\ w_{1y}& w_{2y}& w_{3y}\\ w_{1z}& w_{2z}& w_{3z}\end{array}\right],$ $v=\left[v_1{v}_2{v}_3\right]==\left[\begin{array}{ccc}{v}_{1x}& v_{2x}& v_{3x}\\ v_{1y}& v_{2y}& v_{3y}\\ v_{1z}& v_{2z}& v_{3z}\end{array}\right],$

$B=\left[\begin{array}{ccc}{a}_1{w}_{1x}{v}_{1x}+a_2{w}_{2x}{v}_{2x}+a_3{w}_{3x}{v}_{3x}& a_1{w}_{1x}{v}_{1y}+a_2{w}_{2x}{v}_{2y}+a_3{w}_{3x}{v}_{3y}& a_1{w}_{1x}{v}_{1z}+a_2{w}_{2x}{v}_{2z}+a_3{w}_{3x}{v}_{3z}\\ a_1{w}_{1y}{v}_{1x}+a_2{w}_{2y}{v}_{2x}+a_3{w}_{3y}{v}_{3x}& a_1{w}_{1y}{v}_{1y}+a_2{w}_{2y}{v}_{2y}+a_3{w}_{3y}{v}_{3y}& a_1{w}_{1y}{v}_{1z}+a_2{w}_{2y}{v}_{2z}+a_3{w}_{3y}{v}_{3z}\\ a_1{w}_{1z}{v}_{1x}+a_2{w}_{2z}{v}_{2x}+a_3{w}_{3z}{v}_{3x}& a_1{w}_{1z}{v}_{1y}+a_2{w}_{2z}{v}_{2y}+a_3{w}_{3z}{v}_{3y}& a_1{w}_{1z}{v}_{1z}+a_2{w}_{2z}{v}_{2z}+a_3{w}_{3z}{v}_{3z}\end{array}\right]$

$S=2\×\left[\begin{array}{ccc}{a}_1{w}_{1x}{v}_{1x}+a_2{w}_{2x}{v}_{2x}+a_3{w}_{3x}{v}_{3x}& a_1{w}_{1x}{v}_{1y}+a_2{w}_{2x}{v}_{2y}+a_3{w}_{3x}{v}_{3y}& a_1{w}_{1x}{v}_{1z}+a_2{w}_{2x}{v}_{2z}+a_3{w}_{3x}{v}_{3z}\\ a_1{w}_{1y}{v}_{1x}+a_2{w}_{2y}{v}_{2x}+a_3{w}_{3y}{v}_{3x}& a_1{w}_{1y}{v}_{1y}+a_2{w}_{2y}{v}_{2y}+a_3{w}_{3y}{v}_{3y}& a_1{w}_{1y}{v}_{1z}+a_2{w}_{2y}{v}_{2z}+a_3{w}_{3y}{v}_{3z}\\ a_1{w}_{1z}{v}_{1x}+a_2{w}_{2z}{v}_{2x}+a_3{w}_{3z}{v}_{3x}& a_1{w}_{1z}{v}_{1y}+a_2{w}_{2z}{v}_{2y}+a_3{w}_{3z}{v}_{3y}& a_1{w}_{1z}{v}_{1z}+a_2{w}_{2z}{v}_{2z}+a_3{w}_{3z}{v}_{3z}\end{array}\right]$

$z=\underset{i=1}{\overset{3}{\mathrm{\Σ}}}{a}_i(w_i\×v_i)={[B_{23}-B_{32}{B}_{31}-B_{13}{B}_{12}-B_{21}]}^T$

Definition attitude matrix K battle array is as follows:

$K=\left[\begin{array}{cc}S-\mathrm{\σI}& z\\ z^T& \mathrm{\σ}\end{array}\right]$

The corresponding eigenvector of eigenvalue of maximum of attitude matrix K battle array is the optimal estimation under the Minimum Mean Square Error meaning, and expression formula is: Kq=λ _maxQ, λ _maxBe the eigenvalue of maximum of attitude matrix K, q is the corresponding eigenvector of eigenvalue of maximum, namely finds the solution the gained attitude quaternion;

Step 2.2. is by q=[q ₁q ₂q ₃q ₄] ^T, calculating attitude cosine battle array C ' is:

$C^{\′}=\left[\begin{array}{ccc}{C}_{11}^{\′}& C_{12}^{\′}& C_{13}^{\′}\\ C_{21}^{\′}& C_{22}^{\′}& C_{23}^{\′}\\ C_{31}^{\′}& C_{32}^{\′}& C_{33}^{\′}\end{array}\right]=\left[\begin{array}{ccc}{q}_4^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_4{q}_3)& 2(q_1{q}_3-q_4{q}_2)\\ 2(q_1{q}_2-q_4{q}_3)& q_4^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_4{q}_1)\\ 2(q_1{q}_3+q_4{q}_2)& 2(q_2{q}_3-q_4{q}_1)& q_4^2-q_1^2-q_2^2+q_3^2\end{array}\right];$

Step 2.3. draws high precision astronomical attitude information, i.e. course, pitching and roll three attitude angle information by attitude cosine battle array C '.

Preferably, the described course in step 2.3, pitching and roll three attitude angle

Solution formula as follows:

The pitching angle theta value is: θ '=sin ^-1(C ' ₂₃);

Course angle

Being calculated as follows shown in table of value:

Being calculated as follows shown in table of roll angle γ ' value:

C′ 13The value judgement	C′ 33The value judgement	Roll angle γ ' value
			＝0	＜0	-π
＞0	＜0	atan -(-C′ 13/C′ 33)-π
			＞0	＝0	-π/2
Arbitrary value	＞0	atan -1(-C′ 13/C′ 33)
			＜0	＝0	π/2
＜0	＜0	atan -1(-C′ 13/C′ 33)+π

。

The present invention's advantage compared with prior art is: the present invention has overcome the conventional combination method for determining posture in accuracy of attitude determination and the low deficiency of gyroscopic drift estimated accuracy, utilize UPF to efficiently solve the non-linear and non-Gauss's of noise of system problem, utilize the DNA algorithm that the particle of Unscented particle filter is optimized, the particle that has effectively solved particle filter is degenerated and the deficient problem of particle, realize rapidity and the validity of excellent particle selection, improved speed and the precision of integrated attitude determination; The moment of inertia measurement information is merged mutually with astronomical measurement information, further improved the precision of integrated attitude determination, realized the accurate estimation to gyroscopic drift, satisfied that spacecraft is long-time, the requirement of high precision integrated attitude determination.

Description of drawings

Fig. 1 is based on the schematic diagram by the integrated attitude determination method of the UPF algorithm of DNA algorithm optimization according to the embodiment of the present invention, a kind of.

Embodiment

As shown in Figure 1, concrete implementation step of the present invention is as follows:

Step 1, utilize inertial gyroscope to measure the inertia measurement data, utilize gyro output data that described inertia measurement data are compensated, then by attitude algorithm, obtain attitude of carrier information;

Preferably, utilize by information fusion algorithm and estimate the gyroscopic drift that obtains

Compensate the ε in described inertia measurement data ω=K * V+ ε+ζ.

Described step 1 specifically comprises the following steps:

Step 1.1. sets initial attitude

Calculate initial attitude hypercomplex number q (0) battle array:

Step 1.2. upgrades the attitude quaternion matrix:

$q(n+1)=\{\cos \frac{\mathrm{\Δ\φ}}{2}I+\frac{\sin \frac{\mathrm{\Δ\φ}}{2}}{\mathrm{\Δ\φ}}[\mathrm{\Δ\Φ}\left]\right\}q\left(n\right)$

Wherein, n is the n moment, and I is unit quaternion, Δ φ=[Δ φ _XΔ φ _YΔ φ _Z], described Δ φ is the gyro output angle increment, [ΔΦ] is:

$\left[\mathrm{\Δ\Φ}\right]=\left[\begin{array}{cccc}0& -\mathrm{\Δ}{\mathrm{\φ}}_X& {-\mathrm{\Δ\φ}}_Y& {-\mathrm{\Δ\φ}}_Z\\ {\mathrm{\Δ\φ}}_X& 0& {\mathrm{\Δ\φ}}_Z& {-\mathrm{\Δ\φ}}_Y\\ {\mathrm{\Δ\φ}}_Y& {-\mathrm{\Δ\φ}}_Z& 0& {\mathrm{\Δ\φ}}_X\\ {\mathrm{\Δ\φ}}_Z& {\mathrm{\Δ\φ}}_Y& {-\mathrm{\Δ\φ}}_X& 0\end{array}\right];$

Step 1.3. is by q (n+1)=[q ₁q ₂q ₃q ₄] ^T, calculating attitude cosine battle array C is:

$C=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]=\left[\begin{array}{ccc}{q}_4^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_4{q}_3)& 2(q_1{q}_3-q_4{q}_2)\\ 2(q_1{q}_2-q_4{q}_3)& q_4^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_4{q}_1)\\ 2(q_1{q}_3+q_4{q}_2)& 2(q_2{q}_3-q_4{q}_1)& q_4^2-q_1^2-q_2^2+q_3^2\end{array}\right];$

Step 1.4. is found the solution the real-time attitude information of carrier by attitude cosine battle array C, the real-time attitude information of described carrier comprises course, pitching and roll three attitude angle information:

Course, pitching and roll three attitude angle

Solution formula as follows:

The pitching angle theta value is: θ=sin ^-1(C ₂₃);

Course angle

Being calculated as follows shown in table of value:

Being calculated as follows shown in table of roll angle γ value:

C 13The value judgement	C 33The value judgement	Roll angle γ value
			＝0	＜0	-π
＞0	＜0	atan -1(-C 13/C 33)-π
			＞0	＝0	-π/2
Arbitrary value	＞0	atan -1(-C 13/C 33)
			＜0	＝0	π/2
＜0	＜0	atan -1(-C 13/C 33)+π

Step 2, utilize astronomical measurement information, by deterministic algorithm, find the solution astronomical attitude information, described astronomical attitude information comprises course, pitching and roll three attitude angle information;

Described step 2 specifically comprises the following steps:

The matrix w of step 2.1. definition 3 * 3, v, B and S, 3 * 1 column vector z, a and scalar σ, 4 * 1 column vector q, $B=\underset{k=1}{\overset{3}{\mathrm{\Σ}}}{a}_i{w}_i{v}_i^T,$ S＝B+B ^T， $z=\underset{i=1}{\overset{3}{\mathrm{\Σ}}}{a}_i(w_i\×v_i),$ σ＝tr(B)；

Wherein,

W=[w ₁w ₂w ₃], the coordinate vector of starlight in the star sensor coordinate system of three stars that it is observed constantly for k, v=[v ₁v ₂v ₃], it is the k reference vector of starlight in geocentric inertial coordinate system of these three stars constantly, a=[a ₁a ₂a ₃] ^T, it is non-negative weighting coefficient, q=[q ₁q ₂q ₃q ₄] ^T, it is attitude quaternion to be found the solution; Concrete:

$w=\left[w_1{w}_2{w}_3\right]=\left[\begin{array}{ccc}{w}_{1x}& w_{2x}& w_{3x}\\ w_{1y}& w_{2y}& w_{3y}\\ w_{1z}& w_{2z}& w_{3z}\end{array}\right],$ $v=\left[v_1{v}_2{v}_3\right]==\left[\begin{array}{ccc}{v}_{1x}& v_{2x}& v_{3x}\\ v_{1y}& v_{2y}& v_{3y}\\ v_{1z}& v_{2z}& v_{3z}\end{array}\right],$

$B=\left[\begin{array}{ccc}{a}_1{w}_{1x}{v}_{1x}+a_2{w}_{2x}{v}_{2x}+a_3{w}_{3x}{v}_{3x}& a_1{w}_{1x}{v}_{1y}+a_2{w}_{2x}{v}_{2y}+a_3{w}_{3x}{v}_{3y}& a_1{w}_{1x}{v}_{1z}+a_2{w}_{2x}{v}_{2z}+a_3{w}_{3x}{v}_{3z}\\ a_1{w}_{1y}{v}_{1x}+a_2{w}_{2y}{v}_{2x}+a_3{w}_{3y}{v}_{3x}& a_1{w}_{1y}{v}_{1y}+a_2{w}_{2y}{v}_{2y}+a_3{w}_{3y}{v}_{3y}& a_1{w}_{1y}{v}_{1z}+a_2{w}_{2y}{v}_{2z}+a_3{w}_{3y}{v}_{3z}\\ a_1{w}_{1z}{v}_{1x}+a_2{w}_{2z}{v}_{2x}+a_3{w}_{3z}{v}_{3x}& a_1{w}_{1z}{v}_{1y}+a_2{w}_{2z}{v}_{2y}+a_3{w}_{3z}{v}_{3y}& a_1{w}_{1z}{v}_{1z}+a_2{w}_{2z}{v}_{2z}+a_3{w}_{3z}{v}_{3z}\end{array}\right]$

$S=2\×\left[\begin{array}{ccc}{a}_1{w}_{1x}{v}_{1x}+a_2{w}_{2x}{v}_{2x}+a_3{w}_{3x}{v}_{3x}& a_1{w}_{1x}{v}_{1y}+a_2{w}_{2x}{v}_{2y}+a_3{w}_{3x}{v}_{3y}& a_1{w}_{1x}{v}_{1z}+a_2{w}_{2x}{v}_{2z}+a_3{w}_{3x}{v}_{3z}\\ a_1{w}_{1y}{v}_{1x}+a_2{w}_{2y}{v}_{2x}+a_3{w}_{3y}{v}_{3x}& a_1{w}_{1y}{v}_{1y}+a_2{w}_{2y}{v}_{2y}+a_3{w}_{3y}{v}_{3y}& a_1{w}_{1y}{v}_{1z}+a_2{w}_{2y}{v}_{2z}+a_3{w}_{3y}{v}_{3z}\\ a_1{w}_{1z}{v}_{1x}+a_2{w}_{2z}{v}_{2x}+a_3{w}_{3z}{v}_{3x}& a_1{w}_{1z}{v}_{1y}+a_2{w}_{2z}{v}_{2y}+a_3{w}_{3z}{v}_{3y}& a_1{w}_{1z}{v}_{1z}+a_2{w}_{2z}{v}_{2z}+a_3{w}_{3z}{v}_{3z}\end{array}\right]$

$z=\underset{i=1}{\overset{3}{\mathrm{\Σ}}}{a}_i(w_i\×v_i)={[B_{23}-B_{32}{B}_{31}-B_{13}{B}_{12}-B_{21}]}^T$

Definition attitude matrix K battle array is as follows:

$K=\left[\begin{array}{cc}S-\mathrm{\σI}& z\\ z^T& \mathrm{\σ}\end{array}\right]$

The corresponding eigenvector of eigenvalue of maximum of attitude matrix K battle array is the optimal estimation under the Minimum Mean Square Error meaning, and expression formula is: Kq=λ _maxQ, λ _maxBe the eigenvalue of maximum of attitude matrix K, q is the corresponding eigenvector of eigenvalue of maximum, namely finds the solution the gained attitude quaternion;

Step 2.2. is by q=[q ₁q ₂q ₃q ₄] ^T, calculating attitude cosine battle array C ' is:

$C^{\′}=\left[\begin{array}{ccc}{C}_{11}^{\′}& C_{12}^{\′}& C_{13}^{\′}\\ C_{21}^{\′}& C_{22}^{\′}& C_{23}^{\′}\\ C_{31}^{\′}& C_{32}^{\′}& C_{33}^{\′}\end{array}\right]=\left[\begin{array}{ccc}{q}_4^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_4{q}_3)& 2(q_1{q}_3-q_4{q}_2)\\ 2(q_1{q}_2-q_4{q}_3)& q_4^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_4{q}_1)\\ 2(q_1{q}_3+q_4{q}_2)& 2(q_2{q}_3-q_4{q}_1)& q_4^2-q_1^2-q_2^2+q_3^2\end{array}\right];$

Step 2.3. draws the astronomical attitude information of high precision by attitude cosine battle array C ', i.e. course, pitching and roll three attitude angle information, described course, pitching and roll three attitude angle

Solution formula as follows:

Pitching angle theta ' be worth is: θ '=sin ^-1(C ' ₂₃);

Course angle

Being calculated as follows shown in table of value:

Being calculated as follows shown in table of roll angle γ ' value:

C′ 13The value judgement	C′ 33The value judgement	Roll angle γ ' value
			＝0	＜0	-π
＞0	＜0	atan -1(-C′ 13/C′ 33)-π
			＞0	＝0	-π/2
Arbitrary value	＞0	atan -(-C′ 13/C′ 33)
			＜0	＝0	π/2
＜0	＜0	atan -1(-C′ 13/C′ 33)+π

Step 3, utilize the UPF algorithm of DNA algorithm optimization that astronomical attitude information is merged mutually with attitude of carrier information, find the solution high-precision attitude of carrier information, estimate gyroscopic drift, and feedback compensation attitude of carrier information and gyroscopic drift is compensated, finally complete the high precision integrated attitude determination to spacecraft.

Wherein, described step 3 specifically comprises the following steps:

(1) during t=0, initialization:

To initial priori probability density p (x ₀) sample, generate N and obey p (x ₀) particle that distributes Its average and variance satisfy:

${\stackrel{\‾}{x}}_0^{\left(i\right)}=E\left[x_0^{\left(i\right)}\right]$

$P_0^{\left(i\right)}=E\left[(x_0^{\left(i\right)}-{\stackrel{\‾}{x}}_0^{\left(i\right)}){(x_0^{\left(i\right)}-{\stackrel{\‾}{x}}_0^{\left(i\right)})}^T\right];$

(2) t 〉=1 o'clock, step is as follows:

1. sampling step

With Unscented Kalman filtering new particle more Obtain

Sampling ${\hat{x}}_k^{\left(i\right)}~$

$q\left(x_k^{\left(i\right)}\right|x_{.0:k-1}^{\left(i\right)},y_{1:k})=N({\stackrel{\‾}{x}}_k^{\left(i\right)},P_k^{\left(i\right)}),i=1,\mathrm{\Λ},N;$

2. Determining Weights

${\tilde{w}}_k^{\left(i\right)}={\tilde{w}}_{k-1}^{\left(i\right)}\frac{p\left(y_k\right|{\hat{x}}_k^{\left(i\right)})p\left({\hat{x}}_k^{\left(i\right)}{|x_{k-1}^{\left(i\right)}}^{}\right)}{q\left({\hat{x}}_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},y_{1:k})}$

Normalized weight: ${w_k}^{\left(i\right)}={w_k^{\%}}^{\left(i\right)}/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w_k^{\%}}^{\left(i\right)};$

3. step resamples

From discrete distribution

In carry out N time and resample, obtain one group of new particle

Be still p (x _k| y _0:k) approximate representation;

4. organize new particle from this that obtains by the DNA algorithm and choose excellent particle, pick out the low particle that waits, to solve the exhausted problem of particle, the step of utilizing the DNA algorithm to be optimized is as follows:

The a initialization

It is N that order allows the number of times of iteration _max, DNA soup capacity is N, iteration count N _c=1, recombination probability P is set _cWith the variation probability P _m, dimension is n;

B produces N DNA encoding sequence at random according to the DNA encoding strategy, and the formation capacity is the DNA soup of N;

C obtains X according to decoding policy decoding dna coding one by one _j, described X _jBe the state variable of corresponding random N the DNA encoding sequence that produces, calculate each individual adaptive value F (DNA according to following formula _i), DNA _i∈ D, D are the set of DNA decoding institute total, make N _cEqual N _c+ 1, if N _c＞N _max, turn step f;

F (DNA _i)=C one f (X _j)

In formula, C preferably gets f (X _j) maximal value; f(X _j) be fitness function, with

Relevant;

D carries out descending sort with N individuality according to the fitness value size, obtains: { F ₁, F ₂, L, F _N, F wherein _iBe equivalent to F (DNA _i), and F is arranged ₁＞F ₂＞L＞F _N

E forms daughter DNA soup according to selecting with the replicate run strategy, and to DNA encoding sequence in daughter DNA soup recombinate, variation and inversion operation, then turn step c;

F makes F _max=max (F ₁, F ₂, L, F _N), f (X _j)=C-F _maxBe minimum, i.e. f (X _j) middle corresponding X value, be optimum solution;

5. the X value is the optimal estimation value of state variable, includes attitude information and the gyroscopic drift information of carrier, directly exports the optimal estimation value of attitude of carrier;

According to the minimum variance criterion, the optimal estimation value of attitude of carrier

Be exactly the average that condition distributes, be shown below:

${\hat{x}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i{x}_k^i$

p _kBe the k state variance battle array in step, be shown below:

$p_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i(x_k^i-{\hat{x}}_k){(x_k^i-{\hat{x}}_k)}^T.$

Above content is only preferred embodiment of the present invention, for those of ordinary skill in the art, according to thought of the present invention, all will change in specific embodiments and applications, and this description should not be construed as limitation of the present invention.

Claims (5)

1. one kind based on the integrated attitude determination method by the UPF algorithm of DNA algorithm optimization, it is characterized in that, said method comprising the steps of:

Step 1, utilize inertial gyroscope to measure the inertia measurement data, utilize gyro output data that described inertia measurement data are compensated, then by attitude algorithm, obtain attitude of carrier information;

Step 2, utilize astronomical measurement information, by deterministic algorithm, find the solution astronomical attitude information, described astronomical attitude information comprises course, pitching and roll three attitude angle information;

Step 3, utilize the UPF algorithm of DNA algorithm optimization that astronomical attitude information is merged mutually with attitude of carrier information, find the solution high-precision attitude of carrier information, estimate gyroscopic drift, and feedback compensation attitude of carrier information and gyroscopic drift is compensated, finally complete the high precision integrated attitude determination to spacecraft.

Wherein, described step 3 specifically comprises the following steps:

(1) during t=0, initialization:

To initial priori probability density p (x ₀) sample, generate N and obey p (x ₀) particle that distributes

Its average and variance satisfy:

${\stackrel{\‾}{x}}_0^{\left(i\right)}=E\left[x_0^{\left(i\right)}\right]$

$P_0^{\left(i\right)}=E\left[(x_0^{\left(i\right)}-{\stackrel{\‾}{x}}_0^{\left(i\right)}){(x_0^{\left(i\right)}-{\stackrel{\‾}{x}}_0^{\left(i\right)})}^T\right];$

(2) t 〉=1 o'clock, step is as follows:

1. sampling step

With Unscented Kalman filtering new particle more

Obtain

Sampling ${\hat{x}}_k^{\left(i\right)}~$ $q\left(x_k^{\left(i\right)}\right|x_{.0:k-1}^{\left(i\right)},y_{1:k})=N({\stackrel{\‾}{x}}_k^{\left(i\right)},P_k^{\left(i\right)}),i=1,\mathrm{\Λ},N;$

2. Determining Weights

${\tilde{w}}_k^{\left(i\right)}={\tilde{w}}_{k-1}^{\left(i\right)}\frac{p\left(y_k\right|{\hat{x}}_k^{\left(i\right)})p\left({\hat{x}}_k^{\left(i\right)}{|x_{k-1}^{\left(i\right)}}^{}\right)}{q\left({\hat{x}}_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},y_{1:k})}$

Normalized weight: ${w_k}^{\left(i\right)}={w_k^{\%}}^{\left(i\right)}/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w_k^{\%}}^{\left(i\right)};$

3. step resamples

From discrete distribution

In carry out N time and resample, obtain one group of new particle Be still p (x _k| _Y0:k) approximate representation;

4. organize new particle from this that obtains by the DNA algorithm and choose excellent particle, pick out the low particle that waits, to solve the exhausted problem of particle, the step of utilizing the DNA algorithm to be optimized is as follows:

The a initialization

It is N that order allows the number of times of iteration _max, DNA soup capacity is N, iteration count N _c=1, recombination probability P is set _cWith the variation probability P _m, dimension is n;

B produces N DNA encoding sequence at random according to the DNA encoding strategy, and the formation capacity is the DNA soup of N;

C obtains X according to decoding policy decoding dna coding one by one _j, described X _jBe the state variable of corresponding random N the DNA encoding sequence that produces, calculate each individual adaptive value F (DNA according to following formula _i), DNA _i∈ D, D are the set of DNA decoding institute total, make N _cEqual N _c+ 1, if N _c＞N _max, turn step f;

F(DNA _i)＝C-f(X _j)

In formula, C preferably gets f (X _j) maximal value; f(X _j) be fitness function, with

Relevant;

D carries out descending sort with N individuality according to the fitness value size, obtains: { F ₁, F ₂, L, F _N, F wherein _iBe equivalent to F (DNA _i), and F is arranged ₁＞F ₂＞L＞F _N

E forms daughter DNA soup according to selecting with the replicate run strategy, and to DNA encoding sequence in daughter DNA soup recombinate, variation and inversion operation, then turn step c;

F makes F _max=max (F ₁, F ₂, L, F _N), f (X _j)=C-F _maxBe minimum, i.e. f (X _j) middle corresponding X value, be optimum solution;

5. the X value is the optimal estimation value of state variable, includes attitude information and the gyroscopic drift information of carrier, directly exports the optimal estimation value of attitude of carrier;

According to the minimum variance criterion, the optimal estimation value of attitude of carrier

Be exactly the average that condition distributes, be shown below:

${\hat{x}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i{x}_k^i$

p _kBe the k state variance battle array in step, be shown below:

$p_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i(x_k^i-{\hat{x}}_k){(x_k^i-{\hat{x}}_k)}^T.$

2. according to claim 1 based on the integrated attitude determination method by the UPF algorithm of DNA algorithm optimization, it is characterized in that:

Described step 1 specifically comprises the following steps:

Step 1.1. sets initial attitude

Calculate initial attitude hypercomplex number q (0) battle array:

Step 1.2. upgrades the attitude quaternion matrix:

$q(n+1)=\{\cos \frac{\mathrm{\Δ\φ}}{2}I+\frac{\sin \frac{\mathrm{\Δ\φ}}{2}}{\mathrm{\Δ\φ}}[\mathrm{\Δ\Φ}\left]\right\}q\left(n\right)$

Wherein, n is the n moment, and I is unit quaternion, Δ φ=[Δ φ _XΔ φ _YΔ φ _Z], described Δ φ is the gyro output angle increment, [ΔΦ] is:

$\left[\mathrm{\Δ\Φ}\right]=\left[\begin{array}{cccc}0& -\mathrm{\Δ}{\mathrm{\φ}}_X& {-\mathrm{\Δ\φ}}_Y& {-\mathrm{\Δ\φ}}_Z\\ {\mathrm{\Δ\φ}}_X& 0& {\mathrm{\Δ\φ}}_Z& {-\mathrm{\Δ\φ}}_Y\\ {\mathrm{\Δ\φ}}_Y& {-\mathrm{\Δ\φ}}_Z& 0& {\mathrm{\Δ\φ}}_X\\ {\mathrm{\Δ\φ}}_Z& {\mathrm{\Δ\φ}}_Y& {-\mathrm{\Δ\φ}}_X& 0\end{array}\right];$

Step 1.3. is by q (n+1)=[q ₁q ₂q ₃q ₄] ^T, calculating attitude cosine battle array C is:

$C=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]=\left[\begin{array}{ccc}{q}_4^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_4{q}_3)& 2(q_1{q}_3-q_4{q}_2)\\ 2(q_1{q}_2-q_4{q}_3)& q_4^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_4{q}_1)\\ 2(q_1{q}_3+q_4{q}_2)& 2(q_2{q}_3-q_4{q}_1)& q_4^2-q_1^2-q_2^2+q_3^2\end{array}\right];$

Step 1.4. finds the solution attitude of carrier information by attitude cosine battle array C, and described attitude of carrier information comprises course, pitching and roll three attitude angle information.

3. according to claim 2 based on the integrated attitude determination method by the UPF algorithm of DNA algorithm optimization, it is characterized in that: the course in step 1.4, pitching and roll three attitude angle

Solution formula as follows:

The pitching angle theta value is: θ=sin ^-1(C ₂₃);

Course angle

Being calculated as follows shown in table of value:

Being calculated as follows shown in table of roll angle γ value:

C ₁₃The value judgement C ₃₃The value judgement Roll angle γ value ＝0 ＜0 -π ＞0 ＜0 atan ^-1(-C ₁₃/C ₃₃)-π ＞0 ＝0 -π/2 Arbitrary value ＞0 atan ^-1(-C ₁₃/C ₃₃) ＜0 ＝0 π/2 ＜0 ＜0 atan ^-1(-C ₁₃/C ₃₃)+π

。

4. according to claim 1 based on the integrated attitude determination method by the UPF algorithm of DNA algorithm optimization, it is characterized in that: described step 2 specifically comprises the following steps:

The matrix w of step 2.1. definition 3 * 3, v, B and S, 3 * 1 column vector z, a and scalar σ, 4 * 1 column vector q, $B=\underset{k=1}{\overset{3}{\mathrm{\Σ}}}{a}_i{w}_i{v}_i^T,$ S＝B+B ^T， $z=\underset{i=1}{\overset{3}{\mathrm{\Σ}}}{a}_i(w_i\×v_i),\mathrm{\σ}=\mathrm{tr}\left(B\right);$

Wherein,

W=[w ₁w ₂w ₃], the coordinate vector of starlight in the star sensor coordinate system of three stars that it is observed constantly for k, v=[v ₁v ₂v ₃], it is the k reference vector of starlight in geocentric inertial coordinate system of these three stars constantly, a=[a ₁a ₂a ₃] ^T, it is non-negative weighting coefficient, q=[q ₁q ₂q ₃q ₄] ^T, it is attitude quaternion to be found the solution; Concrete:

$w=\left[w_1{w}_2{w}_3\right]=\left[\begin{array}{ccc}{w}_{1x}& w_{2x}& w_{3x}\\ w_{1y}& w_{2y}& w_{3y}\\ w_{1z}& w_{2z}& w_{3z}\end{array}\right],$ $v=\left[v_1{v}_2{v}_3\right]==\left[\begin{array}{ccc}{v}_{1x}& v_{2x}& v_{3x}\\ v_{1y}& v_{2y}& v_{3y}\\ v_{1z}& v_{2z}& v_{3z}\end{array}\right],$

$B=\left[\begin{array}{ccc}{a}_1{w}_{1x}{v}_{1x}+a_2{w}_{2x}{v}_{2x}+a_3{w}_{3x}{v}_{3x}& a_1{w}_{1x}{v}_{1y}+a_2{w}_{2x}{v}_{2y}+a_3{w}_{3x}{v}_{3y}& a_1{w}_{1x}{v}_{1z}+a_2{w}_{2x}{v}_{2z}+a_3{w}_{3x}{v}_{3z}\\ a_1{w}_{1y}{v}_{1x}+a_2{w}_{2y}{v}_{2x}+a_3{w}_{3y}{v}_{3x}& a_1{w}_{1y}{v}_{1y}+a_2{w}_{2y}{v}_{2y}+a_3{w}_{3y}{v}_{3y}& a_1{w}_{1y}{v}_{1z}+a_2{w}_{2y}{v}_{2z}+a_3{w}_{3y}{v}_{3z}\\ a_1{w}_{1z}{v}_{1x}+a_2{w}_{2z}{v}_{2x}+a_3{w}_{3z}{v}_{3x}& a_1{w}_{1z}{v}_{1y}+a_2{w}_{2z}{v}_{2y}+a_3{w}_{3z}{v}_{3y}& a_1{w}_{1z}{v}_{1z}+a_2{w}_{2z}{v}_{2z}+a_3{w}_{3z}{v}_{3z}\end{array}\right]$

$S=2\×\left[\begin{array}{ccc}{a}_1{w}_{1x}{v}_{1x}+a_2{w}_{2x}{v}_{2x}+a_3{w}_{3x}{v}_{3x}& a_1{w}_{1x}{v}_{1y}+a_2{w}_{2x}{v}_{2y}+a_3{w}_{3x}{v}_{3y}& a_1{w}_{1x}{v}_{1z}+a_2{w}_{2x}{v}_{2z}+a_3{w}_{3x}{v}_{3z}\\ a_1{w}_{1y}{v}_{1x}+a_2{w}_{2y}{v}_{2x}+a_3{w}_{3y}{v}_{3x}& a_1{w}_{1y}{v}_{1y}+a_2{w}_{2y}{v}_{2y}+a_3{w}_{3y}{v}_{3y}& a_1{w}_{1y}{v}_{1z}+a_2{w}_{2y}{v}_{2z}+a_3{w}_{3y}{v}_{3z}\\ a_1{w}_{1z}{v}_{1x}+a_2{w}_{2z}{v}_{2x}+a_3{w}_{3z}{v}_{3x}& a_1{w}_{1z}{v}_{1y}+a_2{w}_{2z}{v}_{2y}+a_3{w}_{3z}{v}_{3y}& a_1{w}_{1z}{v}_{1z}+a_2{w}_{2z}{v}_{2z}+a_3{w}_{3z}{v}_{3z}\end{array}\right]$

$z=\underset{i=1}{\overset{3}{\mathrm{\Σ}}}{a}_i(w_i\×v_i)={[B_{23}-B_{32}{B}_{31}-B_{13}{B}_{12}-B_{21}]}^T$

Definition attitude matrix K battle array is as follows:

$K=\left[\begin{array}{cc}S-\mathrm{\σI}& z\\ z^T& \mathrm{\σ}\end{array}\right]$

The corresponding eigenvector of eigenvalue of maximum of attitude matrix K battle array is the optimal estimation under the Minimum Mean Square Error meaning, and expression formula is: Kq=λ _maxQ, λ _maxBe the eigenvalue of maximum of attitude matrix K, q is the corresponding eigenvector of eigenvalue of maximum, namely finds the solution the gained attitude quaternion;

Step 2.2. is by q=[q ₁q ₂q ₃q ₄] ^T, calculating attitude cosine battle array C ' is:

$C^{\′}=\left[\begin{array}{ccc}{C}_{11}^{\′}& C_{12}^{\′}& C_{13}^{\′}\\ C_{21}^{\′}& C_{22}^{\′}& C_{23}^{\′}\\ C_{31}^{\′}& C_{32}^{\′}& C_{33}^{\′}\end{array}\right]=\left[\begin{array}{ccc}{q}_4^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_4{q}_3)& 2(q_1{q}_3-q_4{q}_2)\\ 2(q_1{q}_2-q_4{q}_3)& q_4^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_4{q}_1)\\ 2(q_1{q}_3+q_4{q}_2)& 2(q_2{q}_3-q_4{q}_1)& q_4^2-q_1^2-q_2^2+q_3^2\end{array}\right];$

Step 2.3. draws high precision astronomical attitude information, i.e. course, pitching and roll three attitude angle information by attitude cosine battle array C '.

5. according to claim 4 based on the integrated attitude determination method by the UPF algorithm of DNA algorithm optimization, it is characterized in that: the described course in step 2.3, pitching and roll three attitude angle

Solution formula as follows:

Pitching angle theta ' be worth is: θ '=sin ^-1(C ' ₂₃);

Course angle

Being calculated as follows shown in table of value:

Being calculated as follows shown in table of roll angle γ ' value:

C′ ₁₃The value judgement C′ ₃₃The value judgement Roll angle γ ' value ＝0 ＜0 -π ＞0 ＜0 atan ^-1(-C′ ₁₃/C′ ₃₃)-π ＞0 ＝0 -π/2 Arbitrary value ＞0 atan ^-1(-C′ ₁₃/C′ ₃₃) ＜0 ＝0 π/2 ＜0 ＜0 atan ^-1(-C′ ₁₃/C′ ₃₃)+π

。

Images