CN102359789A - Arbitrary order output method for rigid body space motion state - Google Patents

Abstract

The present invention discloses an arbitrary order output method for a rigid body space motion state. According to the method, three velocity components of a machine shaft system and a three-ary number form a linear differential equation by defining the three-ary number; a state transition matrix can be solved according to a manner of an arbitrary order keeper so as to acquire an expression of a rigid body motion discrete state equation, such that the singularity problem of the attitude equation is avoided so as to acquire the main motion state of the rigid body. According to the present invention, the three-ary number is introduced, such that the state transition matrix is the block upper triangular manner, and can be solved by reducing the order so as to substantially simplify the calculation complexity, and be easily used by the engineering.

Description

A kind of any rank 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 any rank output intent of rigid space motion state, and this method makes that through definition ternary number axis is that three speed components and ternary number constitute the linear differential equation group; 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 any rank 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(t,t_0)=I+\left[\begin{array}{ccc}0& {\∫}_{t_0}^t\mathrm{rdt}& -{\∫}_{t_0}^t\mathrm{qdt}\\ -{\∫}_{t_0}^t\mathrm{rdt}& 0& {\∫}_{t_0}^t\mathrm{pdt}\\ {\∫}_{t_0}^t\mathrm{qdt}& -{\∫}_{t_0}^t\mathrm{pdt}& 0\end{array}\right]+$

$\left[\begin{array}{ccc}-{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2-{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2& {\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2& {\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2\\ {\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2& -{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2-{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2& {\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2\\ {\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2& {\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2& -{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2-{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2\end{array}\right]+\·\·\·$

${\mathrm{\Φ}}_s(t,t_0)=I+\left[\begin{array}{ccc}0& -{\∫}_{t_0}^t\mathrm{rdt}& {\∫}_{t_0}^t\mathrm{qdt}\\ {\∫}_{t_0}^t\mathrm{rdt}& 0& {\∫}_{t_0}^t\mathrm{pdt}\\ -{\∫}_{t_0}^t\mathrm{qdt}& -{\∫}_{t_0}^t\mathrm{pdt}& 0\end{array}\right]+$

$\left[\begin{array}{ccc}-{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2-{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2& -{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2& -{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2\\ -{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2& -{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2-{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2& {\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2\\ -{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2& {\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2& -{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2-{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2\end{array}\right]+\·\·\·$

$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$ More than desirable t in two formulas ₀=kT, t=(k+1) T;

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

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(t,t_0)=I+\left[\begin{array}{ccc}0& {\∫}_{t_0}^t\mathrm{rdt}& -{\∫}_{t_0}^t\mathrm{qdt}\\ -{\∫}_{t_0}^t\mathrm{rdt}& 0& {\∫}_{t_0}^t\mathrm{pdt}\\ {\∫}_{t_0}^t\mathrm{qdt}& -{\∫}_{t_0}^t\mathrm{pdt}& 0\end{array}\right]+$

$\left[\begin{array}{ccc}-{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2-{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2& {\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2& {\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2\\ {\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2& -{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2-{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2& {\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2\\ {\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2& {\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2& -{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2-{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2\end{array}\right]+\·\·\·$

${\mathrm{\Φ}}_s(t,t_0)=I+\left[\begin{array}{ccc}0& -{\∫}_{t_0}^t\mathrm{rdt}& {\∫}_{t_0}^t\mathrm{qdt}\\ {\∫}_{t_0}^t\mathrm{rdt}& 0& {\∫}_{t_0}^t\mathrm{pdt}\\ -{\∫}_{t_0}^t\mathrm{qdt}& -{\∫}_{t_0}^t\mathrm{pdt}& 0\end{array}\right]+$

$\left[\begin{array}{ccc}-{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2-{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2& -{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2& -{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2\\ -{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2& -{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2-{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2& {\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2\\ -{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2& {\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2& -{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2-{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2\end{array}\right]+\·\·\·$

More than desirable t in two formulas ₀=kT, t=(k+1) T;

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

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. any rank 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(t,t_0)=I+\left[\begin{array}{ccc}0& {\∫}_{t_0}^t\mathrm{rdt}& -{\∫}_{t_0}^t\mathrm{qdt}\\ -{\∫}_{t_0}^t\mathrm{rdt}& 0& {\∫}_{t_0}^t\mathrm{pdt}\\ {\∫}_{t_0}^t\mathrm{qdt}& -{\∫}_{t_0}^t\mathrm{pdt}& 0\end{array}\right]+$

$\left[\begin{array}{ccc}-{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2-{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2& {\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2& {\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2\\ {\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2& -{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2-{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2& {\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2\\ {\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2& {\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2& -{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2-{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2\end{array}\right]+\·\·\·$

${\mathrm{\Φ}}_s(t,t_0)=I+\left[\begin{array}{ccc}0& -{\∫}_{t_0}^t\mathrm{rdt}& {\∫}_{t_0}^t\mathrm{qdt}\\ {\∫}_{t_0}^t\mathrm{rdt}& 0& {\∫}_{t_0}^t\mathrm{pdt}\\ -{\∫}_{t_0}^t\mathrm{qdt}& -{\∫}_{t_0}^t\mathrm{pdt}& 0\end{array}\right]+$

$\left[\begin{array}{ccc}-{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2-{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2& -{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2& -{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2\\ -{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2& -{\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2-{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2& {\∫}_{t_0}^{t}r{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2\\ -{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2& {\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{rdtdt}}_2& -{\∫}_{t_0}^{t}q{\∫}_{t_0}^{t_2}{\mathrm{qdtdt}}_2-{\∫}_{t_0}^{t}p{\∫}_{t_0}^{t_2}{\mathrm{pdtdt}}_2\end{array}\right]+\·\·\·$

$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$ More than desirable t in two formulas ₀=kT, t=(k+1) T;

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

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}}.$