CN102384746A - Chebyshev output method for space motion state of rigid body - Google Patents

Abstract

The invention discloses a Chebyshev output method for the space motion state of a rigid body. The method comprises the steps of: defining a ternary number so that three speed components of the set of axes of the body and the ternary number form a system of linear differential equations; and performing approximation description on rolling, pitching and yawing angular speeds p, q and r by using Shifted Chebyshev orthogonal polynomials, wherein the state transfer matrix of the system can be solved in the manner of any order holder, thus obtaining the expression of the discrete state equation of the motion of the rigid body; and the problem of singularity of the attitude equation is avoided; therefore, the main motion state of the rigid body is obtained. The Chebyshev output method is characterized in that the ternary number is introduced so that the state transfer matrix is in the form of portioned upper triangles and thereby can be solved through deflation; therefore, the complexity of calculation is greatly simplified; and the Chebyshev output method is convenient for engineering use.

Description

A kind of Chebyshev's output intent of rigid space motion state

Technical field

The present invention relates to the spatial movement rigid model, particularly the big maneuvering flight state output of aircraft problem.

Background technology

Axis is that the rigid motion differential equation is a fundamental equation of describing spatial movements such as aircraft, torpedo, spacecraft.Usually, in data processing etc. was used, the state variable of axon system mainly comprised the X of 3 speed components, three Eulerian angle and earth axes _E, Y _E, Z _EDeng, because Z _EBe defined as vertical ground and point to ground ball center, so Z _EActual flying height for bearing; X _E, Y _EUsually main dependence GPS, GNSS, the Big Dipper etc. directly provide; Eulerian angle are represented the rigid space athletic posture, and the differential equation of portrayal rigid body attitude is a core wherein, is that pitching, lift-over and crab angle are described with three Eulerian angle usually.When the angle of pitch of rigid body was ± 90 °, roll angle and crab angle can't definite values, and it is excessive that error is found the solution in the zone of closing on this singular point simultaneously, caused intolerable error on the engineering and can not use; For fear of this problem, people at first adopt the method for restriction angle of pitch span, and this makes equation degenerate, attitude work entirely, thereby be difficult to be widely used in engineering practice.Along with the research to the aircraft extreme flight, people have adopted direction cosine method, equivalent gyration vector method, hypercomplex number method etc. to calculate the rigid motion attitude again in succession.

Direction cosine method has been avoided " unusual " phenomenon of Eulerian angle describing methods, and calculating attitude matrix with direction cosine method does not have the equation degenerate problem, attitude work entirely; But need find the solution 9 differential equations; Calculated amount is bigger, and real-time is relatively poor, can't satisfy the engineering practice requirement.Equivalence gyration vector method such as list appearance recursion, Shuangzi appearance gyration vector, three increment gyration vectors and four increment rotating vector methods and various correction algorithms on this basis and recursive algorithm etc.When studying rotating vector in the document, all be based on the algorithm that rate gyro is output as angle increment.Yet in actual engineering, the output of some gyros is angle rate signals, like optical fibre gyro, dynamic tuned gyroscope etc.When rate gyro was output as angle rate signal, the Algorithm Error of rotating vector method obviously increased.The hypercomplex number method is that the function of 4 Eulerian angle of definition calculates the boat appearance; Can effectively remedy the singularity of Eulerian angle describing method; As long as separate 4 differential equation of first order formula groups; Analogy has tangible minimizing to cosine attitude matrix differential equation calculated amount, can satisfy in the engineering practice requirement to real-time.Its The common calculation methods has the card of finishing approximatioss, second order, fourth-order Runge-Kutta method and three rank Taylor expansion methods etc.Finishing card approximatioss essence is list appearance algorithm, can not compensate by exchange error what limited rotation caused, and the algorithm drift under high current intelligence in the attitude algorithm can be very serious.When adopting fourth-order Runge-Kutta method to find the solution the hypercomplex number differential equation,, the trigonometric function value can occur to exceed ± 1 phenomenon, disperse thereby cause calculating along with the continuous accumulation of integral error; The Taylor expansion method also is restricted because of the deficiency of computational accuracy.When rigid body is big when motor-driven, angular speed causes the error of said method bigger more greatly; Moreover, the error of attitude estimation usually can cause the error of 4 components of speed, highly output sharply to increase.

Summary of the invention

In order to overcome the existing big problem of rigid motion model output error, the present invention provides a kind of Chebyshev's output intent of rigid space motion state, and this method is through definition ternary number; Make that axis is that three speed components and ternary number constitute the linear differential equation group; And adopting Shifted Chebyshev (change Chebyshev) orthogonal polynomial to lift-over, pitching, yaw rate p, q, r carry out close approximation to be described; Can be according to the state-transition matrix of the mode solving system of any rank retainer; And then obtain the expression formula of rigid motion discrete state equations, avoid attitude equation singular problem, thereby obtained the main motion state of rigid body.

The present invention solves the technical scheme that its technical matters adopts, a kind of Chebyshev's output intent of rigid space motion state, and its characteristic may further comprise the steps:

1, axis is that three speed components are output as:

${\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}+g{\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]{\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}$

$+g{\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Φ}}_v[(k+1)T,\mathrm{\τ}]\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right]\mathrm{d\τ}$

Wherein: u, it is x that v, w are respectively along the rigid body axis, y, the speed component of z axle, n _x, n _y, n _zBe respectively along x, y, the overload of z axle, g is an acceleration of gravity, s ₁, s ₂, s ₃Be the ternary number of definition, and

${\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}$

${{\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_v\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{+{\mathrm{\Π}}_v\{P\⊗[{\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\mathrm{\Π}}_v^T-{\mathrm{\Π}}_v\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Π}}_v\mathrm{H\ξ}\left(\mathrm{kT}\right)$

${{\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_s\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{+{\mathrm{\Π}}_s\{P\⊗[{\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\mathrm{\Π}}_s^T-{\mathrm{\Π}}_s\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Π}}_s\mathrm{H\ξ}\left(\mathrm{kT}\right)$

P, q, r are respectively lift-over, pitching, yaw rate, and T is the sampling period; Parameter-definition is identical in full;

$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$ ξ(t)＝[ξ ₀(t)ξ ₁(t)…ξ _n-1(t)ξ _n(t)] ^T，

$\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\\ \·\\ \·\\ \·\\ {\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},\·\·\·,n-\mathrm{1,0}\≤t\≤\mathrm{NT}$

Be the recursive form of Chebyshev (Chebyshev) orthogonal polynomial, b=NT, lift-over, 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

${\mathrm{\Π}}_v=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& \text{0}\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& \·\·\·& p_{n-1}& p_n\end{array}\right]$

$+\left[\begin{array}{ccc}0& 0& -1\\ 0& 0& 0\\ 1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& \·\·\·& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& \·\·\·& r_{n-1}& r_n\end{array}\right]$

${\mathrm{\Π}}_s=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& \·\·\·& p_{n-1}& p_n\end{array}\right]$

$+\left[\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ -1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& \·\·\·& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& \·\·\·& r_{n-1}& r_n\end{array}\right]$

Work as p, q, when the high-order term n of the expansion of r is odd number, m=4,6 ..., n+1, m=5 when high-order term n is even number, 7 ..., n+1, H _i(i=1,2 ..., n) be H row vector accordingly,

2, highly be output as:

$\stackrel{\·}{h}=\left[\begin{array}{ccc}u& v& w\end{array}\right]\left[\begin{array}{c}{s}_1\\ s_2\\ s_3\end{array}\right]$

Wherein: h is a height;

3, attitude angle is output as:

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^t\frac{{\mathrm{qs}}_2\left(t\right)+{\mathrm{rs}}_3\left(t\right)}{s_2^2\left(t\right)+s_3^2\left(t\right)}\mathrm{dt}$

Wherein:

θ, ψ represent lift-over, pitching, crab angle respectively, $\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]={\mathrm{\Φ}}_s(t,\mathrm{KT}){\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{KT}}.$

The invention has the beneficial effects as follows: make that through introducing the ternary number state-transition matrix is a triangular form on the piecemeal, can find the solution state-transition matrix by depression of order, simplified computation complexity greatly, be convenient to engineering and use.

Below in conjunction with embodiment the present invention is elaborated.

Embodiment

1, axis is that three speed components are output as:

${\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}+g{\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]{\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}$

$+g{\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Φ}}_v[(k+1)T,\mathrm{\τ}]\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right]\mathrm{d\τ}$

Wherein: u, it is x that v, w are respectively along the rigid body axis, y, the speed component of z axle, n _x, n _y, n _zBe respectively along x, y, the overload of z axle, g is an acceleration of gravity, s ₁, s ₂, s ₃Be the ternary number of definition, and

${\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}$

${{\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_v\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{+{\mathrm{\Π}}_v\{P\⊗[{\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\mathrm{\Π}}_v^T-{\mathrm{\Π}}_v\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Π}}_v\mathrm{H\ξ}\left(\mathrm{kT}\right)$

${{\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_s\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{+{\mathrm{\Π}}_s\{P\⊗[{\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\mathrm{\Π}}_s^T-{\mathrm{\Π}}_s\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Π}}_s\mathrm{H\ξ}\left(\mathrm{kT}\right)$

P, q, r are respectively lift-over, pitching, yaw rate, and T is the sampling period; Parameter-definition is identical in full;

$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$ ξ(t)＝[ξ ₀(t)?ξ ₁(t)…ξ _n-1(t)ξ _n(t)] ^T，

$\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\\ \·\\ \·\\ \·\\ {\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},\·\·\·,n-\mathrm{1,0}\≤t\≤\mathrm{NT}$ Be the recursive form of Chebyshev (Chebyshev) orthogonal polynomial, b=NT, lift-over, 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

${\mathrm{\Π}}_v=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& \text{0}\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& \·\·\·& p_{n-1}& p_n\end{array}\right]$

$+\left[\begin{array}{ccc}0& 0& -1\\ 0& 0& 0\\ 1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& \·\·\·& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& \·\·\·& r_{n-1}& r_n\end{array}\right]$

${\mathrm{\Π}}_s=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& \·\·\·& p_{n-1}& p_n\end{array}\right]$

$+\left[\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ -1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& \·\·\·& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& \·\·\·& r_{n-1}& r_n\end{array}\right]$

Work as p, q, when the high-order term n of the expansion of r is odd number, m=4,6 ..., n+1, m=5 when high-order term n is even number, 7 ..., n+1, H _i(i=1,2 ..., n) be H row vector accordingly,

2, highly be output as:

$\stackrel{\·}{h}=\left[\begin{array}{ccc}u& v& w\end{array}\right]\left[\begin{array}{c}{s}_1\\ s_2\\ s_3\end{array}\right]$

Wherein: h is a height;

3, attitude angle is output as:.

$\mathrm{\ψ}\left[(k+1)T\right]=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^{(k+1)T}\frac{{\mathrm{qs}}_2\left(t\right)+{\mathrm{rs}}_3\left(t\right)}{s_2^2\left(t\right)+s_3^2\left(t\right)}\mathrm{dt}$

Wherein:

θ, ψ represent lift-over, pitching, crab angle respectively, ${\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_s[{(k+1)T,\mathrm{KT})\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{KT}}.$

Claims (1)

1. Chebyshev's output intent of a rigid space motion state, its characteristic may further comprise the steps:

Axis is that three speed components are output as:

${\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}+g{\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]{\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}$

$+g{\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Φ}}_v[(k+1)T,\mathrm{\τ}]\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right]\mathrm{d\τ}$

Wherein: u, it is x that v, w are respectively along the rigid body axis, y, the speed component of z axle, n _x, n _y, n _zBe respectively along x, y, the overload of z axle, g is an acceleration of gravity, s ₁, s ₂, s ₃Be the ternary number of definition, and

${\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}$

${{\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_v\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{+{\mathrm{\Π}}_v\{P\⊗[{\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\mathrm{\Π}}_v^T-{\mathrm{\Π}}_v\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Π}}_v\mathrm{H\ξ}\left(\mathrm{kT}\right)$

${{\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_s\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{+{\mathrm{\Π}}_s\{P\⊗[{\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\mathrm{\Π}}_s^T-{\mathrm{\Π}}_s\mathrm{H\ξ}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Π}}_s\mathrm{H\ξ}\left(\mathrm{kT}\right)$

P, q, r are respectively lift-over, pitching, yaw rate, and T is the sampling period; Parameter-definition is identical in full;

$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$ ξ(t)＝[ξ ₀(t)?ξ ₁(t)…ξ _n-1(t)ξ _n(t)] ^T，

$\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\\ \·\\ \·\\ \·\\ {\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},\·\·\·,n-\mathrm{1,0}\≤t\≤\mathrm{NT}$

Be the recursive form of Chebyshev (Chebyshev) orthogonal polynomial, b=NT, lift-over, 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

${\mathrm{\Π}}_v=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& \text{0}\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& \·\·\·& p_{n-1}& p_n\end{array}\right]$

$+\left[\begin{array}{ccc}0& 0& -1\\ 0& 0& 0\\ 1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& \·\·\·& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& \·\·\·& r_{n-1}& r_n\end{array}\right]$

${\mathrm{\Π}}_s=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& \·\·\·& p_{n-1}& p_n\end{array}\right]$

$+\left[\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ -1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& \·\·\·& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& \·\·\·& r_{n-1}& r_n\end{array}\right]$

Work as p, q, when the high-order term n of the expansion of r is odd number, m=4,6 ..., n+1, m=5 when high-order term n is even number, 7 ..., n+1, H _i(i=1,2 ..., n) be H row vector accordingly,

Highly be output as:

$\stackrel{\·}{h}=\left[\begin{array}{ccc}u& v& w\end{array}\right]\left[\begin{array}{c}{s}_1\\ s_2\\ s_3\end{array}\right]$

Wherein: h is a height;

Attitude angle is output as:

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^t\frac{{\mathrm{qs}}_2\left(t\right)+{\mathrm{rs}}_3\left(t\right)}{s_2^2\left(t\right)+s_3^2\left(t\right)}\mathrm{dt}$

Wherein:

θ, ψ represent lift-over, pitching, crab angle respectively, $\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]={\mathrm{\Φ}}_s(t,\mathrm{KT}){\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{KT}}.$