CN102506873B - Euler angle Laguerre approximate output method based on angle velocity - Google Patents

Abstract

The invention discloses an Euler angle Laguerre approximate output method based on angle velocity, which is used for solving the technical problem that output precision of the Euler angles of existing aircrafts during maneuver flight is poor. The technical scheme includes: introducing multiple parameters, expanding the rolling angle velocity, the pitch angle velocity and the yaw angle velocity according to the Laguerre orthogonal polynomials, solving the rolling angle, the pitch angle and the yaw angle sequentially, and performing high-order approximate integration to the expressions of the Euler angles directly so that the solutions of the Euler angles approximate according to the superlinearity. Accordingly, time iterative calculation precision for determining the Euler angle is guaranteed and accuracy of output flight attitude of inertia equipment is improved.

Description

Based on the Eulerian angle Laguerre approximate output method of angular velocity

Technical field

The present invention relates to a kind of aircraft maneuvering flight defining method, particularly relate to a kind of Eulerian angle Laguerre approximate output method based on angular velocity.

Background technology

Inertial equipment is in movable body navigation and have vital role in controlling; The acceleration of rigid motion, angular velocity and attitude etc. usually all depend on inertial equipment and export, and the output accuracy therefore improving inertial equipment has clear and definite practical significance; In inertial equipment, acceleration adopts accelerometer, angular velocity to adopt the direct metering system of angular rate gyroscope, as flight test etc. adopts attitude gyro to measure when the attitude accuracy of rigid body requires very high, but angular velocity etc. is had to measure directly calculation output in a lot of application; Main cause is because dynamic attitude sensor is expensive, volume is large, a lot of aircraft is caused to adopt angular rate gyroscope etc. to resolve three Eulerian angle, the attitude time is upgraded and is output into the core contents such as navigation, also become one of principal element affecting inertial navigation system precision, therefore design and adopt renewal of rational attitude time output intent just to become the hot subject of research; From the document published, attitude is exported and mainly adopt the direct method of approximation of Eulerian equation based on angular velocity or adopt approximate Runge Kutta method to resolve (Sun Li, Qin Yongyuan, attitude algorithms of SINS compares, China's inertial technology journal, 2006, Vol.14 (3): 6-10; Pu Li, Wang TianMiao, Liang JianHong, Wang Song, An Attitude Estimate Approach using MEMS Sensors for Small UAVs, 2006, IEEE International Conference on Industrial Informatics, 1113-1117); Because three Eulerian angle are coupled mutually in Eulerian equation, belong to nonlinear differential equation, the error range under different starting condition with different flight state is different, is difficult to ensure Practical Project permissible accuracy.

Summary of the invention

During in order to overcome existing aircraft maneuvering flight, the problem of eulerian angle output precision difference, the invention provides a kind of Eulerian angle Laguerre approximate output method based on angular velocity.The method is by the multiple parameter of introducing and by rolling, pitching, yaw rate according to Laguerre orthogonal polynomial expansion, by according to solving the angle of pitch, roll angle, crab angle successively, directly high order approximation integration is carried out to the expression formula of Eulerian angle, solving of Eulerian angle is approached according to ultralinear, thus can ensure to determine that the time of Eulerian angle upgrades the output accuracy of iterative computation precision and inertance element.

The technical solution adopted for the present invention to solve the technical problems is: a kind of Eulerian angle Laguerre approximate output method based on angular velocity, is characterized in comprising the following steps:

1, (a) is according to Eulerian equation:

In formula: refer to rolling, pitching, crab angle respectively; P, q, r are respectively rolling, pitching, yaw rate; Parameter definition is identical in full; The calculating of these three Eulerian angle according to solve successively the angle of pitch, roll angle, crab angle step carry out; The expansion of rolling, pitching, yaw rate p, q, r is respectively

p(t)＝[p ₀?p ₁?L?p _n-1?p _n][ξ ₀(t)?ξ ₁(t)?L?ξ _n-1(t)?ξ _n(t)] ^T

q(t)＝[q ₀?q ₁?L?q _n-1?q _n][ξ ₀(t)?ξ ₁(t)?L?ξ _n-1(t)?ξ _n(t)] ^T

r(t)＝[r ₀?r ₁?L?r _n-1?r _n][ξ ₀(t)?ξ ₁(t)?L?ξ _n-1(t)?ξ _n(t)] ^T

Wherein

$\left.\begin{array}{c}{\mathrm{\ξ}}_0\left(t\right)=1\\ {\mathrm{\ξ}}_1\left(t\right)=1-t\\ {\mathrm{\ξ}}_2\left(t\right)=1-2t+0.5t^2\\ M\\ (i+1){\mathrm{\ξ}}_{i+1}\left(t\right)=(1+2i-t){\mathrm{\ξ}}_i\left(t\right)-i{\mathrm{\ξ}}_{i-1}\left(t\right)\end{array}\right\},i=\mathrm{2,3},L,n-1$

For the recursive form of Laguerre orthogonal polynomial, T is the sampling period, and symbol is identical in full;

B the time renewal of () angle of pitch solves formula and is:

In formula:

$a_1=1+\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$+0.25{\left\{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$+0.25{\left\{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$-0.25{\left\{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$a_2=\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$-0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]{\mathrm{dtH}}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$a_3=\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T$

$+0.5\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$-0.5\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$H=\left[\begin{array}{ccccccc}-1& 0& 0& L& 0& 0& 0\\ -1& -1& 0& L& 0& 0& 0\\ 0& 0& 1& L& 0& 0& 0\\ 0& 0& 0& L& 0& 0& 0\\ M& M& M& O& M& M& M\\ 0& 0& 0& L& 0& 1& -1\\ 0& 0& 0& L& 0& 0& 1\end{array}\right]$

Work as p, when the most high-order term n of expansion of q, r is odd number, m=4,6, K, n+1, m=5 when high-order term n is even number, 7, K, n+1;

2, (a) is when the known angle of pitch, and the time renewal of roll angle solves formula and is:

Wherein

$a_4=1+\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$+0.25{\left\{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$-0.25{\left\{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$-0.25{\left\{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$a_5=\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$+0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]{\mathrm{dtH}}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$a_6=\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$-0.5\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$+0.5\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

B (), under the angle of pitch, roll angle known case, the formula that solves of crab angle is:

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^t[b_1\left(t\right)+b_2\left(t\right)]\mathrm{dt}$

In formula:

The invention has the beneficial effects as follows: due to by introducing multiple parameter by rolling, pitching, yaw rate according to Laguerre orthogonal polynomial expansion, by according to solving the angle of pitch, roll angle, crab angle successively, directly high order approximation integration is carried out to the expression formula of Eulerian angle, solving of Eulerian angle is approached according to ultralinear, thus ensure that the output accuracy of time renewal iterative computation precision and the inertance element determining Eulerian angle.

Below in conjunction with embodiment, the present invention is elaborated.

Embodiment

1, (a) is according to rigid-body attitude equation (Eulerian equation):

refer to rolling, pitching, crab angle respectively; P, q, r are respectively rolling, pitching, yaw rate; Parameter definition is identical in full; The calculating of these three Eulerian angle according to solve successively the angle of pitch, roll angle, crab angle step carry out; The expansion of rolling, pitching, yaw rate p, q, r is respectively

p(t)＝[p ₀?p ₁?L?p _n-1?p _n]M[1?t?L?t ^n-1?t ⁿ] ^T

q(t)＝[q ₀?q ₁?L?q _n-1?q _n]M[1?t?L?t ^n-1?t ⁿ] ^T

r(t)＝[r ₀?r ₁?L?r _n-1?r _n]M[1?t?L?t ^n-1?t ⁿ] ^T

Wherein: M is the constant matrices of predefined, definition of T is the sampling period;

For Chebyshev (Chebyshev) orthogonal polynomial:

$\left.\begin{array}{c}{\mathrm{\ξ}}_0\left(t\right)=1\\ {\mathrm{\ξ}}_1\left(t\right)=1-2t/b\\ {\mathrm{\ξ}}_2\left(t\right)=8{(t/b)}^2-8(t/b)+1\\ M\\ {\mathrm{\ξ}}_{i+1}\left(t\right)=2{\mathrm{\ξ}}_1\left(t\right){\mathrm{\ξ}}_i\left(t\right)-{\mathrm{\ξ}}_{i-1}\left(t\right)\end{array}\right\},i=\mathrm{2,3},L,n-\mathrm{1,0}\≤t\≤\mathrm{NT},b=\mathrm{NT}$

Then constant matrices

$M=\left\{m(i,j)\right\}=\left[\begin{array}{c}{m}_1\\ m_2\\ m_3\\ M\\ m_N\end{array}\right]=\left[\begin{array}{ccccc}1& 0& 0& L& 0\\ 1& -\frac{2}{b}& 0& L& 0\\ 1& -\frac{8}{b}& \frac{8}{b^2}& L& 0\\ M& M& M& M& M\\ & & & & \end{array}\right]$

$m(i,j)=2m(i-1,j)-m(i-2,j)-\frac{4}{b}m(i-1,j-1),(i=\mathrm{3,4},L,N;j=\mathrm{1,2},L,i)$

m(i，j)＝0，(j＞i)

m(i，0)＝0，(j＝1，2，L，N)

B) time of the angle of pitch upgrades and solves formula and be:

In formula:

$a_1=1+\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]\mathrm{M\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]\mathrm{M\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{M\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{M}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{M}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{M}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$+0.25{\left\{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$+0.25{\left\{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$-0.25{\left\{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$a_2=\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$-0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}\mathrm{M\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{M}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$a_3=\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T$

$+0.5\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]{\mathrm{M\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$-0.5\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{M^T\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$H=\mathrm{diag}\{1,\frac{1}{2},\frac{1}{3},L,\frac{1}{n},\frac{1}{n+1}\};$

2a) when the known angle of pitch, the time renewal of roll angle solves formula and is:

Wherein

$a_4=1+\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\mathrm{M\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]{\mathrm{M\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]{\mathrm{M\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{M}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{M}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{M}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$+0.25{\left\{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$-0.25{\left\{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$-0.25{\left\{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$a_5=\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$+0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}\mathrm{M\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{M}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$a_6=\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$-0.5\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\mathrm{M\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^T\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$+0.5\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]\mathrm{MH}{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{M}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

B) under the angle of pitch, roll angle known case, the formula that solves of crab angle is:

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^t[b_1\left(t\right)+b_2\left(t\right)]\mathrm{dt}$

In formula:

When directly exporting rolling to inertial equipment, pitching, yaw rate p, q, r be when adopting three rank to approach description, and acquired results is also close to O (T ³), the O (T of the method such as compare the direct method of approximation of Eulerian equation or adopt approximate Runge Kutta method to resolve ²) precision will height.

Claims (1)

1., based on an Eulerian angle Laguerre approximate output method for angular velocity, it is characterized in that comprising the following steps:

Step 1, (a) are according to Eulerian equation:

In formula: refer to rolling, pitching, crab angle respectively; P (t), q (t), r (t) is respectively rolling, pitching, yaw rate; The calculating of these three Eulerian angle according to solve successively the angle of pitch, roll angle, crab angle step carry out; Rolling, pitching, yaw rate p (t), q (t), the expansion of r (t) is respectively

$p\left(t\right)=\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_{n-1}\left(t\right)& {\mathrm{\ξ}}_n\left(t\right)\end{array}\right]}^T$

$q\left(t\right)=\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_{n-1}\left(t\right)& {\mathrm{\ξ}}_n\left(t\right)\end{array}\right]}^T$

$r\left(t\right)=\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_{n-1}\left(t\right)& {\mathrm{\ξ}}_n\left(t\right)\end{array}\right]}^T$

Wherein

$\left.\begin{array}{c}{\mathrm{\ξ}}_0\\ {\mathrm{\ξ}}_1\\ {\mathrm{\ξ}}_2\left(t\right)=1-2t+{0.5t}^2\\ .\\ .\\ .\\ (i+1){\mathrm{\ξ}}_{i+1}\left(t\right)=(1+2i-t){\mathrm{\ξ}}_i\left(t\right)-{\mathrm{i\ξ}}_{i-1}\left(t\right)\end{array}\right\}i=\mathrm{2,3},...,n-1$

For the recursive form of Laguerre orthogonal polynomial, T is the sampling period;

B the time renewal of () angle of pitch solves formula and is:

In formula:

$\begin{array}{c}{a}_2=\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\end{array}\right]}^T{|}_{\mathrm{kT}}^T\\ -0.5\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ +0.5\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ \·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\end{array}$

$\begin{array}{c}{a}_3=\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\end{array}\right]}^T\\ +0.5\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ -0.5\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ \·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\end{array}$

Step 2, (a) are when the known angle of pitch, and the time renewal of roll angle solves formula and is:

Wherein

$\begin{array}{c}{a}_5=\left[\begin{array}{ccccc}{p}_0& p_1& ....& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ +0.5\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ -0.5\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ \·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\end{array}$

$\begin{array}{c}{a}_6=\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ -0.5\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ +0.5\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ \·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\end{array}$

B (), under the angle of pitch, roll angle known case, the formula that solves of crab angle is:

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^t[b_1\left(t\right)+b_2\left(t\right)]\mathrm{dt}$

In formula: