CN102323991B - Walsh output method for spatial motion state of rigid body - Google Patents

Abstract

The invention discloses a Walsh output method for a spatial motion state of a rigid body. In the Walsh output method, by defining a ternary number, three speed components of a machine body axis system and the ternary number constitute a linear simultaneous differential equation, a Walsh function polynomial is used for performing approximate description on roll rate p, pitch rate q and yaw rate r, a state-transition matrix of a system can be solved according to a random-order keeper manner so as to further obtain an expression of a motion discrete state equation of the rigid body, and thus, the problem of a singular gesture equation is avoided and the main motion state of the rigid body is obtained. In the invention, by introducing the ternary number to make the state-transition matrix be of a blocked upper triangular form, the state-transition matrix can be solved in a reduced order manner, the computation complexity is greatly simplified, and engineering purposes are facilitated.

Description

A kind of modeling method of rigid space motion state Walsh output model

Technical field

The present invention relates to spatial movement rigid model, particularly the modeling method of the large maneuvering flight State-output of aircraft model.

Background technology

Axis is that the rigid motion differential equation is the fundamental equation of describing the spatial movements such as aircraft, torpedo, spacecraft.Conventionally, in the application such as data processing, the state variable of axon system mainly comprises the X of 3 speed components, three Eulerian angle and earth axes _e, Y _e, Z _edeng, due to Z _ebe defined as vertical ground and point to ground ball center, so Z _ereality is negative flying height; X _e, Y _econventionally main GPS, GNSS, the Big Dipper etc. of relying on directly provide; Eulerian angle represent rigid space motion attitude, and the differential equation of portraying rigid body attitude is core wherein, is that pitching, rolling and crab angle are described conventionally with three Eulerian angle.When the angle of pitch of rigid body is ± 90 °, roll angle and crab angle cannot definite values, and it is excessive that the region of simultaneously closing on this singular point solves error, causes intolerable error in engineering and can not use; For fear of this problem, first people adopt the method for restriction angle of pitch span, and this degenerates equation, attitude work entirely, thereby be difficult to be widely used in engineering practice.Along with the research to aircraft extreme flight, people have adopted again direction cosine method, Rotation Vector, Quaternion Method etc. to calculate rigid motion attitude in succession.

Direction cosine method has been avoided " unusual " phenomenon of Eulerian angle describing methods, and with direction cosine method, calculating attitude matrix does not have equation degenerate problem, attitude work entirely, but need to solve 9 differential equations, calculated amount is larger, and real-time is poor, cannot meet engineering practice requirement.Rotation Vector is as list sample recursion, Shuangzi sample gyration vector, three increment gyration vectors and four increment rotating vector methods and various correction algorithms on this basis and recursive algorithm etc.While studying rotating vector in document, be all based on rate gyro, to be output as the algorithm of angle increment.Yet in Practical Project, the output of some gyros is angle rate signals, as optical fibre gyro, dynamic tuned gyroscope etc.When rate gyro is output as angle rate signal, the Algorithm Error of rotating vector method obviously increases.Quaternion Method is that the function of 4 Eulerian angle of definition calculates boat appearance, can effectively make up the singularity of Eulerian angle describing method, as long as separate 4 differential equation of first order formula groups, than direction cosine attitude matrix differential equation calculated amount, there is obvious minimizing, can meet the requirement to real-time in engineering practice.Its conventional computing method have the card of finishing approximatioss, second order, fourth-order Runge-Kutta method and three rank Taylor expansions etc.Finishing card approximatioss essence is list sample algorithm, and what limited rotation was caused can not compensate by exchange error, and the algorithm drift under high current intelligence in attitude algorithm can be very serious.While adopting fourth-order Runge-Kutta method to solve quaternion differential equation, along with the continuous accumulation of integral error, there will be exceed ± 1 phenomenon of trigonometric function value, thereby cause calculating, disperse; Taylor expansion is also because the deficiency of computational accuracy is restricted.When rigid body is large when motor-driven, angular speed causes more greatly the error of said method larger; Moreover, the error that attitude is estimated usually can cause the error of 4 components of speed, highly output sharply to increase.

Summary of the invention

In order to overcome the large problem of existing rigid motion model output model modeling error, the invention provides a kind of modeling method of rigid space motion state Walsh output model, the method is by definition Three-ary Number, making axis is that three speed components and Three-ary Number form linear differential equation group, and adopt walsh function polynomial expression to rolling, pitching, yaw rate p, q, r carries out close approximation description, can be according to the state-transition matrix of the mode solving system of arbitrary order retainer, and then obtain the expression formula of rigid motion discrete state equations, avoided attitude equation singular problem, thereby obtain rigid body main movement state.

The present invention solves the technical scheme that its technical matters adopts, a kind of modeling method of rigid space motion state Walsh output model, and its feature comprises the following steps:

1, the axis of rigid space motion is that three speed components are output as:

$\begin{array}{c}{\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\τ}\end{array}$

Wherein: u, v, it is x that w is respectively along rigid body axis, y, the rigid space motion speed component of z axle, n _x, n _y, n _zbe respectively along x, y, the overload of z axle, g is acceleration of gravity, s ₁, s ₂, s ₃for the Three-ary 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{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{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{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{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 is respectively angular velocity in roll, rate of pitch and the yaw rate of rigid space motion, and T is the sampling period;

$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],\mathrm{\ξ}\left(t\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,$

${\mathrm{\ξ}}_{\mathrm{\λ}}\left(t\right)=\underset{j=0}{\overset{\mathrm{\ρ}-1}{\mathrm{\Π}}}\mathrm{sgn}\left\{\cos \right[{\mathrm{\λ}}_j{2}^j\mathrm{\πt}/\left(\mathrm{NT}\right)\left]\right\},(0\≤t\≤\mathrm{NT},\mathrm{\λ}=\mathrm{0,1},...,n)$ For walsh function; $\mathrm{\λ}=\underset{j=0}{\overset{\mathrm{\ρ}-1}{\mathrm{\Σ}}}{\mathrm{\λ}}_j{2}^j,$ λ _jbe 0 or the binary numeral of the binary representation formula of 1-λ, ρ is binary value figure place, and sgn represents sign function;

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

$\begin{array}{c}{\mathrm{\Π}}_v=\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]\end{array}$

$\begin{array}{c}{\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]\end{array}$

2, the height of rigid space motion is 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: the height that h is rigid space motion;

3, the attitude angle of rigid space motion is output as:

$\mathrm{\θ}\left(t\right)=0.5\left\{{\sin }^{-1}\right[s_1\left(t\right)]+{\cos }^{-1}\sqrt{s_2^2\left(t\right)+s_3^2\left(t\right)}\}$

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

Wherein: the roll angle, the angle of pitch and the crab angle that represent respectively rigid space motion,

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

The invention has the beneficial effects as follows: by introducing Three-ary Number, to make state-transition matrix be triangular form on piecemeal, can depression of order solving state transition matrix, greatly simplified computation complexity, be convenient to engineering and use.

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

Embodiment

1, the axis of rigid space motion is that three speed components are output as:

$\begin{array}{c}{\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\τ}\end{array}$

Wherein: u, v, it is x that w is respectively along rigid body axis, y, the rigid space motion speed component of z axle, n _x, n _y, n _zbe respectively along x, y, the overload of z axle, g is acceleration of gravity, s ₁, s ₂, s ₃for the Three-ary 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{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{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{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{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 is respectively angular velocity in roll, rate of pitch and the yaw rate of rigid space motion, and T is the sampling period;

$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],\mathrm{\ξ}\left(t\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,$

${\mathrm{\ξ}}_{\mathrm{\λ}}\left(t\right)=\underset{j=0}{\overset{\mathrm{\ρ}-1}{\mathrm{\Π}}}\mathrm{sgn}\left\{\cos \right[{\mathrm{\λ}}_j{2}^j\mathrm{\πt}/\left(\mathrm{NT}\right)\left]\right\},(0\≤t\≤\mathrm{NT},\mathrm{\λ}=\mathrm{0,1},...,n)$ For walsh function; $\mathrm{\λ}=\underset{j=0}{\overset{\mathrm{\ρ}-1}{\mathrm{\Σ}}}{\mathrm{\λ}}_j{2}^j,$ λ _jbe 0 or the binary numeral of the binary representation formula of 1-λ, ρ is binary value figure place, and sgn represents sign function;

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

$\begin{array}{c}{\mathrm{\Π}}_v=\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]\end{array}$

$\begin{array}{c}{\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]\end{array}$

2, the height of rigid space motion is 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: the height that h is rigid space motion;

3, the attitude angle of rigid space motion is output as:.

$\mathrm{\θ}\left(\mathrm{kT}\right)=0.5\left\{{\sin }^{-1}\right[s_1\left(\mathrm{kT}\right)]+{\cos }^{-1}\sqrt{s_2^2\left(\mathrm{kT}\right)+s_3^2\left(\mathrm{kT}\right)}\}$

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

Wherein: the roll angle, the angle of pitch and the crab angle that represent respectively rigid space motion,

${\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. a modeling method for rigid space motion state Walsh output model, described rigid body is aircraft, its feature comprises the following steps:

A) axis of rigid space motion is that three speed components are output as:

$\begin{array}{c}{\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\τ}\end{array}$

Wherein: u, v, it is x that w is respectively along rigid body axis, y, the rigid space motion speed component of z axle, n _x, n _y, n _zbe respectively along x, y, the overload of z axle, g is acceleration of gravity, s ₁, s ₂, s ₃for the Three-ary 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{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{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{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{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 is respectively angular velocity in roll, rate of pitch and the yaw rate of rigid space motion, and T is the sampling period;

$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],\mathrm{\ξ}\left(t\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,$

${\mathrm{\ξ}}_{\mathrm{\λ}}\left(t\right)=\underset{j=0}{\overset{\mathrm{\ρ}-1}{\mathrm{\Π}}}\mathrm{sgn}\left\{\cos \right[{\mathrm{\λ}}_j{2}^j\mathrm{\πt}/\left(\mathrm{NT}\right)\left]\right\}$ (0≤t≤NT, λ=0,1 ..., n) be walsh function; $\mathrm{\λ}=\underset{j=0}{\overset{\mathrm{\ρ}-1}{\mathrm{\Σ}}}{\mathrm{\λ}}_j{2}^j,$ λ _jbe 0 or the binary numeral of the binary representation formula of 1-λ, ρ is binary value figure place, and sgn represents sign function;

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

$\begin{array}{c}{\mathrm{\Π}}_v=\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]\end{array}$

$\begin{array}{c}{\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]\end{array}$

B) height of rigid space motion is 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: the height that h is rigid space motion;

C) attitude angle of rigid space motion is output as:

$\mathrm{\θ}\left(t\right)=0.5\left\{{\sin }^{-1}\right[s_1\left(t\right)]+{\cos }^{-1}\sqrt{s_2^2\left(t\right)+s_3^2\left(t\right)}\}$

$\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: the roll angle, the angle of pitch and the crab angle that represent respectively rigid space motion,

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