CN102494688B - Quaternion Laguerre Approximation Output Method Based on Angular Velocity for Aircraft Limit Flight - Google Patents

Abstract

The invention discloses a quaternion Laguerre approximate output method based on an angular speed used during extreme flight of a flying vehicle, which is used for solving the technical problem of poor quaternion output accuracy of inertia equipment during extreme flight of the conventional flying vehicle. In a technical scheme, approximate approach description is performed on a rolling angular speed, a pitching angular speed and a yawing angular speed p, q and r by adopting a Shifted Lagnuerre orthogonal polynomial to directly obtain a quaternion state transfer matrix, so that the iterative computation accuracy of a determined quaternion is ensured. Shifted Lagnuerre orthogonal polynomial orders for the rolling angular speed, the pitching angular speed and the yawing angular speed p, q and r can be determined according to engineering accuracy, so that ultra-linear approximation for a quaternion state equation transfer matrix phie[(k+1)T, kT] is realized, the iterative computation accuracy of the determined quaternion is ensured, and the output accuracy of the inertia equipment during extreme flight of the flying vehicle is increased.

Description

Quaternion Laguerre Approximation Output Method Based on Angular Velocity for Aircraft Limit Flight

technical field

The invention relates to an attitude output method of an airborne inertial device of an aircraft, in particular to a quaternion Laguerre approximate output method based on an angular velocity when the aircraft is in extreme flight.

Background technique

Usually, the acceleration, angular velocity, and attitude of rigid body motion depend on the output of inertial equipment, so improving the output accuracy of inertial equipment has clear practical significance. Space motions such as aircraft, torpedoes, and spacecraft use differential equations of rigid body motion in most cases; and the differential equations describing the attitude of rigid bodies are the core of them, usually with three Euler angles, namely pitch, roll and yaw angles To describe, usually the pitch, roll and yaw angular velocities in the airborne inertial equipment are calculated and output. When the pitch angle of the rigid body is ±90°, the roll angle and yaw angle cannot be fixed. At the same time, the solution error of the area near the singularity is too large, resulting in an intolerable error in engineering and cannot be used; in order to avoid this problem, People use the method of limiting the value range of the pitch angle, which degenerates the equation and cannot work at all attitudes, so it is difficult to be widely used in engineering practice. For this reason, based on the direct measurements of the pitch, roll and yaw angular velocities in the airborne inertial equipment, the direction cosine method, the equivalent rotation vector method, and the quaternion method are used to output the flight attitude.

The direction cosine method avoids the "singularity" phenomenon of the Euler method. The calculation of the attitude matrix by the direction cosine method has no problem of equation degradation, and it can work in all attitudes, but it needs to solve nine differential equations, which requires a large amount of calculation and poor real-time performance. Meet the requirements of engineering practice. Equivalent rotation vector methods, such as single-sample recursion, double-sample rotation vector, three-sample rotation vector, and four-sample rotation vector methods, as well as various correction algorithms and recursive algorithms based on this. When studying the rotation vector in the literature, it is based on the algorithm that the output of the rate gyro is an angular increment. However, in actual engineering, the output of some gyroscopes is an angular rate signal, such as fiber optic gyroscopes, dynamic tuning gyroscopes, etc. When the output of the rate gyro is an angular rate signal, the algorithm error of the rotation vector method increases obviously. The quaternion method is the most widely used method. This method is to define the function of four Euler angles to calculate the attitude, which can effectively compensate for the singularity of the Euler method. It only needs to solve four first-order differential equations. The amount of calculation of the direction cosine attitude matrix differential equation is significantly reduced, which can meet the real-time requirements in engineering practice. The commonly used calculation methods include Picard approximation method, second-order, fourth-order Runge-Kutta method and third-order Taylor expansion method (Paul G.Savage.A Unified Mathematical Framework for Strapdown Algorithm Design[J].Journal of guidance, control, anddynamics, 2006, 29(2): 237-248). The essence of the Picasat approximation method is a single sample algorithm, which does not compensate for the non-exchangeable error caused by the finite rotation, and the algorithm drift in the attitude calculation will be very serious under high dynamic conditions. When using the fourth-order Runge-Kutta method to solve quaternion differential equations, with the continuous accumulation of integral errors, the value of trigonometric functions will exceed ±1, which will lead to calculation divergence. The Taylor expansion method is also constrained by the lack of calculation accuracy, especially for aircraft maneuvering, the attitude and azimuth angle rates are usually relatively large, and higher requirements are placed on the estimation accuracy of the attitude, while the determination of parameters such as quaternions brings The error makes the above method can not meet the engineering accuracy in most cases.

Contents of the invention

In order to overcome the problem that the existing quaternion output error is large, the present invention provides a quaternion Laguerre approximate output method based on the angular velocity of the aircraft's limit flight, the method uses a shifted Laguerre (Shifted Laguerre) orthogonal polynomial pair The rolling, pitching, and yaw angular velocities p, q, and r are approximated and described, and the quaternion state transition matrix is obtained directly, so that the iterative calculation accuracy of determining the quaternion can be guaranteed.

The technical scheme that the present invention solves its technical problem adopts is, a kind of quaternion Laguerre approximate output method based on the aircraft limit flight of angular velocity, it is characterized in that comprising the following steps:

According to the quaternion continuous state equation

$\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; A</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\mathrm{<span\; class="notranslate">\; e</span>}$

and the discrete equation of state

e(k+1)=Φ _e [(k+1)T, kT]e(k)

where e=[e ₁ , e ₂ , e ₃ , e ₄ ] ^T $A_e=\frac{1}{2}\left[\begin{array}{cccc}0& -p& -q& -r\\ p& 0& r& -q\\ q& -r& 0& p\\ r& q& -p& 0\end{array}\right]$

Φ _e [(k+1)T, kT] is the state transition matrix of A _e , T is the sampling period, and the full-text symbols are defined the same;

θ，ψ分别指滚转、俯仰、偏航角；p, q, r are roll, pitch, yaw angular velocity respectively; Euler angle

θ, ψ refer to roll, pitch and yaw angle respectively;

The state transition matrix according to the approximation

${\mathrm{<span\; class="notranslate">\; \&Phi;</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; [</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; T</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; kT</span>}<span\; class="notranslate">\; ]</span><span\; class="notranslate">\; \&ap;</span>\mathrm{<span\; class="notranslate">\; I</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&Pi;H\&xi;</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>{<span\; class="notranslate">\; |</span>}_{\mathrm{<span\; class="notranslate">\; kT</span>}}^{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&Pi;</span>}{<span\; class="notranslate">\; \&Integral;</span>}_{\mathrm{<span\; class="notranslate">\; kT</span>}}^{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; [</span>\mathrm{<span\; class="notranslate">\; \&xi;</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; \&xi;</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ]</span>\mathrm{<span\; class="notranslate">\; dt</span>}{\mathrm{<span\; class="notranslate">\; h</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; \&Pi;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; \&Pi;H\&xi;</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>{<span\; class="notranslate">\; |</span>}_{\mathrm{<span\; class="notranslate">\; kT</span>}}^{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; T</span>}}\mathrm{<span\; class="notranslate">\; \&Pi;H\&xi;</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; kT</span>}<span\; class="notranslate">\; )</span>$

And e(k+1)= _Φe [(k+1)T, kT]e(k) obtains the time update value of the quaternion;

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

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

is the recursive form of the Laguerre orthogonal polynomial, and the expansion expressions of the roll, pitch, and yaw angular velocities p, q, and r are 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{<span\; class="notranslate">\; \&Pi;</span>}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; \{</span>\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]\left[\begin{array}{ccccccc}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&CenterDot;</span>& <span\; class="notranslate">\; \&CenterDot;</span>& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]$

$<span\; class="notranslate">\; +</span>\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]\left[\begin{array}{ccccccc}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&CenterDot;</span>& <span\; class="notranslate">\; \&CenterDot;</span>& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]\left[\begin{array}{ccccccc}{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&CenterDot;</span>& <span\; class="notranslate">\; \&CenterDot;</span>& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]<span\; class="notranslate">\; \}</span>$

${\mathrm{<span\; class="notranslate">\; \&Pi;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; \{</span>{\left[\begin{array}{ccccccc}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&CenterDot;</span>& <span\; class="notranslate">\; \&CenterDot;</span>& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]}^{\mathrm{<span\; class="notranslate">\; T</span>}}\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]$

$<span\; class="notranslate">\; +</span>{\left[\begin{array}{ccccccc}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&Center\; Dot;</span>& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]}^{\mathrm{<span\; class="notranslate">\; T</span>}}\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]<span\; class="notranslate">\; +</span>{\left[\begin{array}{ccccccc}{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& <span\; class="notranslate">\; \&CenterDot;</span>& <span\; class="notranslate">\; \&CenterDot;</span>& <span\; class="notranslate">\; \&CenterDot;</span>& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]}^{\mathrm{<span\; class="notranslate">\; T</span>}}\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]<span\; class="notranslate">\; \}</span>$

When p, q, the highest order item n of the expanded formula of r is an odd number, m=4, 6,..., n+1, and when the high order item n is an even number, m=5, 7,..., n+ 1.

The beneficial effects of the present invention are: due to the use of shifted Laguerre (Shifted Laguerre) orthogonal polynomials to roll, pitch, yaw angular velocity p, q, r is carried out approximate approximation description, directly obtained quaternion state transition matrix, thereby The accuracy of the iterative calculation for determining the quaternion is guaranteed.

The present invention is described in detail below in conjunction with embodiment.

Detailed ways

According to the quaternion continuous state equation

$\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; A</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\mathrm{<span\; class="notranslate">\; e</span>}$

and the discrete equation of state

e(k+1)=Φ _e [(k+1)T, kT]e(k)

where e=[e ₁ , e ₂ , e ₃ , e ₄ ] ^T $A_e=\frac{1}{2}\left[\begin{array}{cccc}0& -p& -q& -r\\ p& 0& r& -q\\ q& -r& 0& p\\ r& q& -p& 0\end{array}\right]$

Φ _e [(k+1)T, kT] is the state transition matrix of A _e , T is the sampling period,

θ，ψ分别指滚转、俯仰、偏航角；p, q, r are roll, pitch, yaw angular velocity respectively; Euler angle

θ, ψ refer to roll, pitch and yaw angle respectively;

The state transition matrix according to the approximation

${\mathrm{<span\; class="notranslate">\; \&Phi;</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; [</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; T</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; kT</span>}<span\; class="notranslate">\; ]</span><span\; class="notranslate">\; \&ap;</span>\mathrm{<span\; class="notranslate">\; I</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&Pi;H\&xi;</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>{<span\; class="notranslate">\; |</span>}_{\mathrm{<span\; class="notranslate">\; kT</span>}}^{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&Pi;</span>}{<span\; class="notranslate">\; \&Integral;</span>}_{\mathrm{<span\; class="notranslate">\; kT</span>}}^{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; [</span>\mathrm{<span\; class="notranslate">\; \&xi;</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; \&xi;</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ]</span>\mathrm{<span\; class="notranslate">\; dt</span>}{\mathrm{<span\; class="notranslate">\; h</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; \&Pi;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; \&Pi;H\&xi;</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>{<span\; class="notranslate">\; |</span>}_{\mathrm{<span\; class="notranslate">\; kT</span>}}^{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; T</span>}}\mathrm{<span\; class="notranslate">\; \&Pi;H\&xi;</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; kT</span>}<span\; class="notranslate">\; )</span>$

And e(k+1)= _Φe [(k+1)T, kT]e(k) obtains the time update value of the quaternion;

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

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

is the recursive form of the Laguerre (Laguerre) orthogonal polynomial, and the expansion expressions of the roll, pitch, and yaw angular velocities p, q, and r are 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{<span\; class="notranslate">\; \&Pi;</span>}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; \{</span>\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]\left[\begin{array}{ccccccc}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&Center\; Dot;</span>& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]$

$<span\; class="notranslate">\; +</span>\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]\left[\begin{array}{ccccccc}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&CenterDot;</span>& <span\; class="notranslate">\; \&CenterDot;</span>& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]\left[\begin{array}{ccccccc}{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&Center\; Dot;</span>& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]<span\; class="notranslate">\; \}</span>$

${\mathrm{<span\; class="notranslate">\; \&Pi;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; \{</span>{\left[\begin{array}{ccccccc}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&Center\; Dot;</span>& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]}^{\mathrm{<span\; class="notranslate">\; T</span>}}\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]$

$<span\; class="notranslate">\; +</span>{\left[\begin{array}{ccccccc}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&Center\; Dot;</span>& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]}^{\mathrm{<span\; class="notranslate">\; T</span>}}\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]<span\; class="notranslate">\; +</span>{\left[\begin{array}{ccccccc}{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&Center\; Dot;</span>& <span\; class="notranslate">\; \&Center\; Dot;</span>& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]}^{\mathrm{<span\; class="notranslate">\; T</span>}}\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]<span\; class="notranslate">\; \}</span>$

When p, q, the highest order item n of the expanded formula of r is an odd number, m=4, 6,..., n+1, and when the high order item n is an even number, m=5, 7,..., n+ 1.

Claims (1)

1. the hypercomplex number Laguerre approximation output method during aircraft extreme flight based on angular velocity, is characterized in that comprising the following steps:

According to hypercomplex number continuous state equation

$\stackrel{\&CenterDot;}{e}=A_{e}e$

And discrete state equations

e(k+1)＝Φ _e[(k+1)T，kT]e(k)

E=[e wherein ₁, e ₂, e ₃, e ₄] ^t $A_e=\frac{1}{2}\left[\begin{array}{cccc}0& -p& -q& -r\\ p& 0& r& -q\\ q& -r& 0& p\\ r& q& -p& 0\end{array}\right]$

Φ _e[(k+1) T, kT] is A _estate-transition matrix, T is the sampling period;

P, q, r is respectively rolling, pitching, yaw rate; Eulerian angle

θ, Ψ refers to respectively rolling, pitching, crab angle;

State-transition matrix is according to approximant

${\mathrm{\&Phi;}}_e[(k+1)T,\mathrm{kT}]\&ap;I+\mathrm{\&Pi;H\&xi;}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}+\mathrm{\&Pi;}{\&Integral;}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\&xi;}\left(t\right){\mathrm{\&xi;}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\mathrm{\&Pi;}}_1-\mathrm{\&Pi;H\&xi;}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}\mathrm{\&Pi;H\&xi;}\left(\mathrm{kT}\right)$

And e (k+1)=Φ _e[(k+1) T, kT] e (k) obtains the time renewal value of hypercomplex number;

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

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

For the recursive form of Laguerre orthogonal polynomial, 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

$\mathrm{\&Pi;}=\frac{1}{2}\{\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& -1& 0\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& \&CenterDot;\&CenterDot;\&CenterDot;& p_{n-1}& p_n\end{array}\right]$

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

${\mathrm{\&Pi;}}_1=\frac{1}{2}\{{\left[\begin{array}{ccccc}{p}_0& p_1& \&CenterDot;\&CenterDot;\&CenterDot;& p_{n-1}& p_n\end{array}\right]}^T\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& -1& 0\end{array}\right]$

$+{\left[\begin{array}{ccccc}{q}_0& q_1& \&CenterDot;\&CenterDot;\&CenterDot;& q_{n-1}& q_n\end{array}\right]}^T\left[\begin{array}{cccc}0& 0& -1& 0\\ 0& 0& 0& -1\\ 1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]+{\left[\begin{array}{ccccc}{r}_0& r_1& \&CenterDot;\&CenterDot;\&CenterDot;& r_{n-1}& r_n\end{array}\right]}^T\left[\begin{array}{cccc}0& 0& 0& -1\\ 0& 0& 1& 0\\ 0& -1& 0& 0\\ 1& 0& 0& 0\end{array}\right]\}$