CN102495827B - Euler angle Hermite approximate output method based on angular speed - Google Patents

Abstract

The invention discloses an Euler angle Hermite approximate output method based on angular speed, which is used for solving the technical problem of low Euler angle output precision of maneuver of the conventional aircraft. According to the technical scheme, the method comprises the following steps of: introducing multiple parameters and expanding the angular speeds of rolling, pitching and yawing according to the Hermite orthogonal polynomial; and sequentially solving a pitching angle, a rolling angle and a yawing angle and directly performing high-order approximation integral on the expression of an Euler angle so that the Euler angle is solved in a super-linear approximation way, the precision of time update iterative computation for determining the Euler angle is ensured and the accuracy of the output flight attitude of inertia equipment is improved.

Description

Eulerian angle Emmett approximation output method based on angular velocity

Technical field

The present invention relates to a kind of aircraft maneuvering flight and determine method, particularly relate to a kind of Eulerian angle Emmett approximation output method based on angular velocity.

Background technology

Inertial equipment has vital role in movable body navigation with in controlling; The acceleration of rigid motion, angular velocity and attitude etc. all depend on inertial equipment output conventionally, and the output accuracy that therefore improves 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, the attitude accuracy of rigid body requires when very high to adopt attitude gyro to measure as flight test etc., but has the measurements such as angular velocity directly to resolve output in a lot of applications; Main cause is because dynamically attitude sensor is expensive, volume is large, cause a lot of aircraft to adopt angular rate gyroscopes etc. to resolve three Eulerian angle, make the attitude time upgrade output and become the core contents such as navigation, also become and affect one of principal element of inertial navigation system precision, therefore design and adopt the rational attitude time to upgrade the hot subject that output intent just becomes research; From the document of publishing, attitude output is mainly adopted the direct method of approximation of Eulerian equation based on angular velocity or adopts approximate Runge Kutta method to resolve (Sun Li, Qin Yongyuan, attitude algorithms of SINS comparison, 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 in Eulerian equation are coupled mutually, belong to nonlinear differential equation, different with the error range under different flight state in different starting condition, be difficult to guarantee Practical Project permissible accuracy.

Summary of the invention

The poor problem of Eulerian angle output accuracy, the invention provides a kind of Eulerian angle Emmett approximation output method based on angular velocity when overcoming existing aircraft maneuvering flight.The method by introduce a plurality of parameters and by rolling, pitching, yaw rate according to Emmett orthogonal polynomial expansion, by according to solving successively the angle of pitch, roll angle, crab angle, directly the expression formula of Eulerian angle is carried out to high-order approaches integration, solving according to ultralinear of Eulerian angle approached, thereby can guarantee to determine the time renewal iterative computation precision of Eulerian angle and the output accuracy of inertance element.

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

1, (a) is according to Eulerian equation:

In formula: ψ refers to respectively rolling, pitching, crab angle; P, q, r is respectively rolling, pitching, yaw rate; Parameter-definition is identical in full; The calculating of these three Eulerian angle is carried out according to the step that solves successively the angle of pitch, roll angle, crab angle; Rolling, pitching, yaw rate p, q, the expansion of 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)=2t\\ {\mathrm{\ξ}}_2\left(t\right)={4t}^2-2\\ {\mathrm{\ξ}}_3\left(t\right)={8t}^3-12t\\ {\mathrm{\ξ}}_4\left(t\right)={16t}^4-{48t}^2+12\\ {\mathrm{\ξ}}_5\left(t\right)={32t}^5-160t^3+120t\\ {\mathrm{\ξ}}_6\left(t\right)={64t}^6-{480t}^4+720t-120\\ M\\ {\mathrm{\ξ}}_{i+1}\left(t\right)={2\mathrm{t\ξ}}_i\left(t\right)-2i{\mathrm{\ξ}}_{i-1}\left(t\right)\end{array}\right\}$ i＝2，3，L，n-1

Recursive form for Emmett orthogonal polynomial;

(b) time of the angle of pitch upgrades and to solve 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{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\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}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+0.5\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}{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$

T is the sampling period,

$H={\left\{h_{\mathrm{ij}}\right\}}_{(n+1)(n+2)}=\left[\begin{array}{ccccccc}0& 0.5& 0& 0& L& 0& 0\\ 0.5& 0& 0.25& 0& L& 0& 0\\ 0& 0& 0& \frac{1}{6}& L& 0& 0\\ -1.5& 0& 0& 0& L& 0& 0\\ M& M& M& M& O& M& M\\ h_{n1}& 0& 0& 0& L& h_{n(n+1)}& 0\\ h_{(n+1)1}& 0& 0& 0& L& 0& h_{(n+1)(n+2)}\end{array}\right]$

h ₁₂＝0.5，h ₂₃＝0.25， $h_{i(i+1)}=\frac{h_{(i-1)i}}{1+2h_{(i-1)i}},L$

h ₂₁＝0.5，h ₄₁＝-1.5， $h_{\left(2i\right)1}=\frac{{4\mathrm{ih}}_{(2i-2)1}}{1+2h_{\left(2i\right)(2i+1)}},L$

All the other h _ij=0;

2, (a) in the situation that of the known angle of pitch, the time of roll angle upgrades and to solve formula and be:

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\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}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-0.5\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}{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 introduce a plurality of parameters and by rolling, pitching, yaw rate according to Emmett orthogonal polynomial expansion, by according to solving successively the angle of pitch, roll angle, crab angle, directly the expression formula of Eulerian angle is carried out to high-order approaches integration, solving according to ultralinear of Eulerian angle approached, thereby guaranteed the time renewal iterative computation precision of definite Eulerian angle and the output accuracy of inertance element.

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

Embodiment

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

In formula: ψ refers to respectively rolling, pitching, crab angle; P, q, r is respectively rolling, pitching, yaw rate; Parameter-definition is identical in full; The calculating of these three Eulerian angle is carried out according to the step that solves successively the angle of pitch, roll angle, crab angle; Rolling, pitching, yaw rate p, q, the expansion of 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)=2t\\ {\mathrm{\ξ}}_2\left(t\right)={4t}^2-2\\ {\mathrm{\ξ}}_3\left(t\right)={8t}^3-12t\\ {\mathrm{\ξ}}_4\left(t\right)={16t}^4-{48t}^2+12\\ {\mathrm{\ξ}}_5\left(t\right)={32t}^5-160t^3+120t\\ {\mathrm{\ξ}}_6\left(t\right)={64t}^6-{480t}^4+720t-120\\ M\\ {\mathrm{\ξ}}_{i+1}\left(t\right)={2\mathrm{t\ξ}}_i\left(t\right)-2i{\mathrm{\ξ}}_{i-1}\left(t\right)\end{array}\right\}$ i＝2，3，L，n-1

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

(b) time of the angle of pitch upgrades and to solve 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{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\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}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+0.5\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}{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\{h_{\mathrm{ij}}\right\}}_{(n+1)(n+2)}=\left[\begin{array}{ccccccc}0& 0.5& 0& 0& L& 0& 0\\ 0.5& 0& 0.25& 0& L& 0& 0\\ 0& 0& 0& \frac{1}{6}& L& 0& 0\\ -1.5& 0& 0& 0& L& 0& 0\\ M& M& M& M& O& M& M\\ h_{n1}& 0& 0& 0& L& h_{n(n+1)}& 0\\ h_{(n+1)1}& 0& 0& 0& L& 0& h_{(n+1)(n+2)}\end{array}\right]$

h ₁₂＝0.5，h ₂₃＝0.25， $h_{i(i+1)}=\frac{h_{(i-1)i}}{1+2h_{(i-1)i}},L$

h ₂₁＝0.5，h ₄₁＝-1.5， $h_{\left(2i\right)1}=\frac{{4\mathrm{ih}}_{(2i-2)1}}{1+2h_{\left(2i\right)(2i+1)}},L$

All the other h _ij=0;

2, (a) in the situation that of the known angle of pitch, the time of roll angle upgrades and to solve formula and be:

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\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}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-0.5\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}{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:

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

Claims (1)

1. the aircraft Eulerian angle Emmett approximation output method based on angular velocity, is characterized in that comprising the following steps:

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

In formula: θ, ψ refers to respectively rolling, pitching, crab angle; P, q, r is respectively rolling, pitching, yaw rate; The calculating of these three Eulerian angle is carried out according to the step that solves successively the angle of pitch, roll angle, crab angle; Rolling, pitching, yaw rate p, q, the expansion of r is respectively

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

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

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

Wherein

$\left.\begin{array}{c}{\mathrm{\ξ}}_0\left(t\right)=1\\ {\mathrm{\ξ}}_1\left(t\right)=2t\\ {\mathrm{\ξ}}_2\left(t\right)=4t^2-2\\ {\mathrm{\ξ}}_3\left(t\right)=8t^3-12t\\ {\mathrm{\ξ}}_4\left(t\right)=16t^4-48t^2+12\\ {\mathrm{\ξ}}_5\left(t\right)=32t^5-160t^3+120t\\ {\mathrm{\ξ}}_6\left(t\right)=64t^6-480t^4+720t-120\\ .\\ .\\ .\\ {\mathrm{\ξ}}_{i+1}\left(t\right)=2t{\mathrm{\ξ}}_i\left(t\right)-2i{\mathrm{\ξ}}_{i-1}\left(t\right)\end{array}\right\}i=\mathrm{2,3},...,n-1$

Recursive form for Emmett orthogonal polynomial;

(b) time of the angle of pitch upgrades and to solve formula and be:

In formula:

$\begin{array}{c}{a}_1=1+\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}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ +\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}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ +\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}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]}^T\\ -\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}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ -\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}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ -\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}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]}^T\\ +0.25{\left\{\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}\right\}}^2\\ +0.25{\left\{\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}\right\}}^2\\ -0.25{\left\{\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}\right\}}^2\end{array}$

$\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}\left(t\right)\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}{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}\left(t\right)\end{array}\right]}^T\\ +0.5\begin{array}{}\end{array}\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}$

T is the sampling period,

$h_{12}=0.5,h_{23}=0.25,h_{i(i+1)}=\frac{h_{(i-1)i}}{1+{2h}_{(i-1)i}},...$

$h_{21}=0.5,h_{41}=-1.5,h_{\left(2i\right)1}=\frac{{4\mathrm{ih}}_{(2i-2)1}}{1+{2h}_{\left(2i\right)(2i+1)}},...$

All the other h _ij=0;

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

Wherein

$\begin{array}{c}{a}_4=1+\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}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ +\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}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ +\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}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]}^T\\ -\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}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ -\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}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ -\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}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]}^T\\ +0.25{\left\{\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}\right\}}^2\\ -0.25{\left\{\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}\right\}}^2\\ -0.25{\left\{\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}\right\}}^2\end{array}$

$\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}\left(t\right)\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}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ -0.5\begin{array}{}\end{array}\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: