CN102495829B

CN102495829B - Quaternion Walsh approximate output method based on angular velocities for aircraft during extreme flight

Info

Publication number: CN102495829B
Application number: CN201110366547.XA
Authority: CN
Inventors: 史忠科
Original assignee: Northwestern Polytechnical University
Current assignee: Northwestern Polytechnical University
Priority date: 2011-11-17
Filing date: 2011-11-17
Publication date: 2015-02-25
Anticipated expiration: 2031-11-17
Also published as: CN102495829A

Abstract

本发明公开了一种基于角速度的飞行器极限飞行时四元数沃尔什近似输出方法，用于解决现有的飞行器极限飞行时惯性设备输出四元数精度差的技术问题。技术方案是采用采用沃尔什函数多项式对滚转、俯仰、偏航角速度p，q，r进行近似逼近描述，直接得到了四元数状态转移矩阵，保证了确定四元数的迭代计算精度；本发明根据工程精度的要求，确定对滚转、俯仰、偏航角速度p，q，r的沃尔什近似展开的阶次，实现了对四元数状态方程转移矩阵Φ_e[(k+1)T，kT]的超线性逼近，保证了确定四元数的迭代计算精度，从而提高了飞行器极限飞行时惯性设备输出精度。The invention discloses an angular velocity-based quaternion Walsh approximate output method for an aircraft's extreme flight, which is used to solve the existing technical problem of poor precision of the quaternion output by an inertial device during an aircraft's extreme flight. The technical solution is to use the Walsh function polynomial to approximate the roll, pitch, and yaw angular velocities p, q, and r, and directly obtain the quaternion state transition matrix, which ensures the iterative calculation accuracy of determining the quaternion; According to the requirements of engineering precision, the present invention determines the order of the Walsh approximate expansion of roll, pitch and yaw angular velocity p, q, r, and realizes the transfer matrix Φ _e [(k+1 )T, kT] superlinear approximation ensures the iterative calculation accuracy of determining the quaternion, thereby improving the output accuracy of the inertial equipment during the extreme flight of the aircraft.

Description

Quaternion Walsh 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 Walsh approximate output method based on an angular velocity when the aircraft is in extreme flight.

背景技术 Background technique

通常，刚体运动的加速度、角速度和姿态等都依赖于惯性设备输出，因此提高惯性设备的输出精度具有明确的实际意义。飞行器、鱼雷、航天器等空间运动在大多数情况下都采用刚体运动微分方程；而刻画刚体姿态的微分方程又是其中的核心，通常以三个欧拉角即俯仰、滚转和偏航角来描述，通常都由机载惯性设备中俯仰、滚转和偏航角速度解算后输出。当刚体当俯仰角为±90°时，滚转角和偏航角无法定值，同时临近该奇点的区域求解误差过大，导致工程上不可容忍的误差而不能使用；为了避免这一问题，人们采用限制俯仰角取值范围的方法，这使得方程式退化，不能全姿态工作，因而难以广泛用于工程实践。为此，人们基于机载惯性设备中的俯仰、滚转和偏航角速度直接测量值，并采用了方向余弦法、等效转动矢量法、四元数法等输出飞行姿态。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.

方向余弦法避免了欧拉法的“奇异’现象，用方向余弦法计算姿态矩阵没有方程退化问题，可以全姿态工作，但需要求解九个微分方程，计算量较大，实时性较差，无法满足工程实践要求。等效转动矢量法如单子样递推、双子样转动矢量、三子样转动矢量和四子样旋转矢量法以及在此基础上的各种修正算法和递推算法等。文献中研究旋转矢量时，都是基于速率陀螺输出为角增量的算法。然而在实际工程中，一些陀螺的输出是角速率信号，如光纤陀螺、动力调谐陀螺等。当速率陀螺输出为角速率信号时，旋转矢量法的算法误差明显增大。四元数方法是最为广泛使用的方法，该方法是定义四个欧拉角的函数来计算航姿，能够有效弥补欧拉法的奇异性，只要解四个一阶微分方程式组即可，比方向余弦姿态矩阵微分方程式计算量有明显的减少，能满足工程实践中对实时性的要求。其常用的计算方法有毕卡逼近法、二阶、四阶龙格-库塔法和三阶泰勒展开法等(Paul G.Savage.A Unified MathematicalFramework for Strapdown Algorithm Design[J].Journal of guidance，control，anddynamics，2006，29(2)：237-248)。毕卡逼近法实质是单子样算法，对有限转动引起的不可交换误差没有补偿，在高动态情况下姿态解算中的算法漂移会十分严重。采用四阶龙格-库塔法求解四元数微分方程时，随着积分误差的不断积累，会出现三角函数取值超出±1的现象，从而导致计算发散。泰勒展开法也因计算精度的不足而受到制约，特别是对于飞行器机动飞行，姿态方位角速率通常都较大，而且对姿态的估计精度提出了更高要求，而四元数等参数确定带来的误差使得上述方法大多数情况下不能满足工程精度。The direction cosine method avoids the "singularity" phenomenon of the Euler method. There is no equation degradation problem when calculating the attitude matrix with the direction cosine method, 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. Literature When studying the rotation vector, it is based on the algorithm that the output of the rate gyroscope is an angular increment. However, in actual engineering, the output of some gyroscopes is an angular rate signal, such as a fiber optic gyroscope, a power tuning gyroscope, etc. When the output of a rate gyroscope is an angular rate signal signal, the algorithm error of the rotation vector method increases significantly. 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 is only necessary to solve four first-order differential equations, which is significantly less than the calculation amount of the direction cosine attitude matrix differential equation, and 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, etc. (Paul G.Savage.A Unified Mathematical Framework for Strapdown Algorithm Design[J].Journal of guidance, control, and dynamics, 2006, 29(2): 237- 248). The essence of the Picard approximation method is a single sample algorithm, which does not compensate for the non-exchangeable error caused by finite rotation. In the case of high dynamics, the algorithm drift in the attitude solution will be very serious. The fourth-order Runge-Kutta method is used to solve In the case of quaternion differential equations, with the continuous accumulation of integral errors, the value of the trigonometric function will exceed ±1, which will cause the calculation to diverge. The Taylor expansion method is also restricted by the lack of calculation accuracy, especially for aircraft maneuvering In flight, the attitude and azimuth angle rates are usually relatively large, and higher requirements are placed on the estimation accuracy of the attitude. However, the errors caused by the determination of parameters such as quaternions make the above methods unable to meet the engineering accuracy in most cases.

发明内容 Contents of the invention

为了克服现有四元数输出误差大的问题，本发明提供一种基于角速度的飞行器极限飞行时四元数沃尔什近似输出方法，该方法采用采用沃尔什函数多项式对滚转、俯仰、偏航角速度p，q，r进行近似逼近描述，直接得到了四元数状态转移矩阵，可以保证确定四元数的迭代计算精度，从而提高飞行器极限飞行时惯性设备输出四元数精度。In order to overcome the problem that the existing quaternion output error is large, the present invention provides a quaternion Walsh approximate output method based on the angular velocity of the aircraft's limit flight, the method adopts the Walsh function polynomial to roll, pitch, The yaw angular velocity p, q, r is approximated and described, and the quaternion state transition matrix is directly obtained, which can ensure the iterative calculation accuracy of the quaternion, thereby improving the precision of the quaternion output by the inertial equipment during the extreme flight of the aircraft.

本发明解决其技术问题采用的技术方案是，一种基于角速度的飞行器极限飞行时四元数沃尔什近似输出方法，其特点是包括以下步骤：The technical solution that the present invention solves its technical problem adopts is, a kind of quaternion Walsh approximate output method when the aircraft limit flight based on angular velocity, it is characterized in that comprising the following steps:

根据四元数连续状态方程According to the quaternion continuous state equation

$\overset{\cdot \cdot}{e e} = = {A A}_{e e} e e$

和离散状态方程and the discrete equation of state

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

其中e＝[e₁，e₂，e₃，e₄]^T $A_{e} = \frac{1}{2} [\begin{matrix} 0 & - p & - q & - r \\ p & 0 & r & - q \\ q & - r & 0 & p \\ r & q & - p & 0 \end{matrix}]$ where e=[e ₁ , e ₂ , e ₃ , e ₄ ] ^T $A_{e} = \frac{1}{2} [\begin{matrix} 0 & - p & - q & - r \\ p & 0 & r & - q \\ q & - r & 0 & p \\ r & q & - p & 0 \end{matrix}]$

Φ_e[(k+1)T，kT]为A_e的状态转移矩阵，T为采样周期，全文符号定义相同；Φ _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分别为滚转、俯仰、偏航角速度；欧拉角ψ分别指滚转、俯仰、偏航角；p, q, r are roll, pitch, yaw angular velocity respectively; Euler angle ψ refers to roll, pitch and yaw angle respectively;

状态转移矩阵按照逼近式The state transition matrix according to the approximation

${Φ Φ}_{e e} [[((k k + + 11)) T T,, kT kT]] \approx \approx I I + + ΠHξ ΠHξ ((t t)) {| |}_{kT kT}^{((k k + + 11)) T T} + + Π Π {&Integral; &Integral;}_{kT kT}^{((k k + + 11)) T T} [[ξ ξ ((t t)) {ξ ξ}^{T T} ((t t))]] dt dt {H h}^{T T} {Π Π}_{11} - - ΠHξ ΠHξ ((t t)) {| |}_{kT kT}^{((k k + + 11)) T T} ΠHξ ΠHξ ((kT kT))$

及e(k+1)＝Φ_e[(k+1)T，kT]e(k)得到四元数的时间更新值；And e(k+1)= _Φe [(k+1)T, kT]e(k) obtains the time update value of the quaternion;

其中 $I = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}],$ ξ(t)＝[ξ₀(t) ξ₁(t)…ξ_n-1(t)ξ_n(t)]^T in $I = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}],$ ξ(t)=[ξ ₀ (t) ξ ₁ (t)…ξ _n-1 (t)ξ _n (t)] ^T

$ξ_{k} (t) = Π_{j = 0}^{ρ - 1} sgn {\cos [k_{j} 2^{j} πt / (NT)]}$ (0≤t≤NT，k＝0，1，2，…)为沃尔什函数(WalshFunction)；k_j为0或1-k的二进制表示式的二进制数值，ρ为二进制值位数，sgn表示符号函数；T为采样周期，全文符号定义相同；滚转、俯仰、偏航角速度p，q，r的展开式分别为 $ξ_{k} (t) = Π_{j = 0}^{ρ - 1} sgn {\cos [k_{j} 2^{j} πt / (NT)]}$ (0≤t≤NT, k=0, 1, 2, ...) is a Walsh function (WalshFunction); k _j is the binary value of the binary expression of 0 or 1-k, ρ is the number of digits of the binary value, sgn represents the sign function; T is the sampling period, and the full-text symbol definition is the same; roll, pitch, yaw angular velocity p, q, The expansion of r is

p(t)＝[p₀ p₁…p_n-1 p_n][ξ₀(t)ξ₁(t)…ξ_n-1(t)ξ_n(t)]^T 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 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 r(t)=[r ₀ r ₁ …r _n-1 r _n ][ξ ₀ (t)ξ ₁ (t)…ξ _n-1 (t)ξ _n (t)] ^T

$Π Π = = \frac{11}{22} {{[\begin{matrix} 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \\ 00 & 00 & 00 & 11 \\ 00 & 00 & - - 11 & 00 \end{matrix}] [\begin{matrix} {p p}_{00} & {p p}_{11} & \cdot &Center Dot; \cdot \cdot \cdot &Center Dot; & {p p}_{n no - - 11} & {p p}_{n no} \end{matrix}]$

$+ + [\begin{matrix} 00 & 00 & - - 11 & 00 \\ 00 & 00 & 00 & - - 11 \\ 11 & 00 & 00 & 00 \\ 00 & 11 & 00 & 00 \end{matrix}] [\begin{matrix} {q q}_{00} & {q q}_{11} & \cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot; & {q q}_{n no - - 11} & {q q}_{n no} \end{matrix}] + + [\begin{matrix} 00 & 00 & 00 & - - 11 \\ 00 & 00 & 11 & 00 \\ 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \end{matrix}] [\begin{matrix} {r r}_{00} & {r r}_{11} & \cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot; & {r r}_{n no - - 11} & {r r}_{n no} \end{matrix}]}}$

${Π Π}_{11} = = \frac{11}{22} {{{[\begin{matrix} {p p}_{00} & {p p}_{11} & \cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot; & {p p}_{n no - - 11} & {p p}_{n no} \end{matrix}]}^{T T} [\begin{matrix} 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \\ 00 & 00 & 00 & 11 \\ 00 & 00 & - - 11 & 00 \end{matrix}]$

$+ + {[\begin{matrix} {q q}_{00} & {q q}_{11} & \cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot; & {q q}_{n no - - 11} & {q q}_{n no} \end{matrix}]}^{T T} [\begin{matrix} 00 & 00 & - - 11 & 00 \\ 00 & 00 & 00 & - - 11 \\ 11 & 00 & 00 & 00 \\ 00 & 11 & 00 & 00 \end{matrix}] + + {[\begin{matrix} {r r}_{00} & {r r}_{11} & \cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot; & {r r}_{n no - - 11} & {r r}_{n no} \end{matrix}]}^{T T} [\begin{matrix} 00 & 00 & 00 & - - 11 \\ 00 & 00 & 11 & 00 \\ 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \end{matrix}]}}$

本发明的有益效果是：由于采用沃尔什函数多项式对滚转、俯仰、偏航角速度p，q，r进行近似逼近描述，直接得到了四元数状态转移矩阵，保证了确定四元数的迭代计算精度，从而提高了飞行器极限飞行时惯性设备输出四元数精度。The beneficial effect of the present invention is: owing to adopting Walsh function polynomial to roll, pitch, yaw angular velocity p, q, r carry out approximate approximation description, directly obtained quaternion state transition matrix, guaranteed the determination of quaternion Iterative calculation accuracy, thus improving the quaternion accuracy output by the inertial device during the extreme flight of the aircraft.

下面结合实施例对本发明作详细说明。The present invention is described in detail below in conjunction with embodiment.

具体实施方式 Detailed ways

$\overset{\cdot &Center Dot;}{e e} = = {A A}_{e e} e e$

和离散状态方程and the discrete equation of state

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

其中 $I = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}],$ ξ(t)＝[ξ₀(t)ξ₁(t)…ξ_n-1(t)ξ_n(t)]^T in $I = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}],$ ξ(t)=[ξ ₀ (t)ξ ₁ (t)…ξ _n-1 (t)ξ _n (t)] ^T

其中， $ξ_{k} (t) = Π_{j = 0}^{ρ - 1} sgn {\cos [k_{j} 2^{j} πt / (NT)]}$ (0≤t≤NT，k＝0，1，2，…)为沃尔什函数(WalshFunction)；k_j为0或1-k的二进制表示式的二进制数值，ρ为二进制值位数，sgn表示符号函数；滚转、俯仰、偏航角速度p，q，r的展开式分别为in, $ξ_{k} (t) = Π_{j = 0}^{ρ - 1} sgn {\cos [k_{j} 2^{j} πt / (NT)]}$ (0≤t≤NT, k=0, 1, 2, ...) is a Walsh function (WalshFunction); k _j is the binary value of the binary expression of 0 or 1-k, ρ is the number of digits of the binary value, sgn represents the sign function; the expansion expressions of the roll, pitch, and yaw angular velocities p, q, and r are respectively

$p p ((t t)) = = [\begin{matrix} {p p}_{00} & {p p}_{11} & \cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot; & {p p}_{n no - - 11} & {p p}_{n no} \end{matrix}] {[\begin{matrix} {ξ ξ}_{00} ((t t)) & {ξ ξ}_{11} ((t t)) & \cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot; & {ξ ξ}_{n no - - 11} ((t t)) & {ξ ξ}_{n no} ((t t)) \end{matrix}]}^{T T}$

$q q ((t t)) = = [\begin{matrix} {q q}_{00} & {q q}_{11} & \cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot; & {q q}_{n no - - 11} & {q q}_{n no} \end{matrix}] {[\begin{matrix} {ξ ξ}_{00} ((t t)) & {ξ ξ}_{11} ((t t)) & \cdot \cdot \cdot \cdot \cdot &Center Dot; & {ξ ξ}_{n no - - 11} ((t t)) & {ξ ξ}_{n no} ((t t)) \end{matrix}]}^{T T}$

$r r ((t t)) = = [\begin{matrix} {r r}_{00} & {r r}_{11} & \cdot \cdot \cdot \cdot \cdot \cdot & {r r}_{n no - - 11} & {r r}_{n no} \end{matrix}] {[\begin{matrix} {ξ ξ}_{00} ((t t)) & {ξ ξ}_{11} ((t t)) & \cdot \cdot \cdot \cdot \cdot \cdot & {ξ ξ}_{n no - - 11} ((t t)) & {ξ ξ}_{n no} ((t t)) \end{matrix}]}^{T T}$

$Π Π = = \frac{11}{22} {{[\begin{matrix} 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \\ 00 & 00 & 00 & 11 \\ 00 & 00 & - - 11 & 00 \end{matrix}] [\begin{matrix} {p p}_{00} & {p p}_{11} & \cdot \cdot \cdot \cdot \cdot \cdot & {p p}_{n no - - 11} & {p p}_{n no} \end{matrix}]$

$+ + [\begin{matrix} 00 & 00 & - - 11 & 00 \\ 00 & 00 & 00 & - - 11 \\ 11 & 00 & 00 & 00 \\ 00 & 11 & 00 & 00 \end{matrix}] [\begin{matrix} {q q}_{00} & {q q}_{11} & \cdot \cdot \cdot \cdot \cdot \cdot & {q q}_{n no - - 11} & {q q}_{n no} \end{matrix}] + + [\begin{matrix} 00 & 00 & 00 & - - 11 \\ 00 & 00 & 11 & 00 \\ 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \end{matrix}] [\begin{matrix} {r r}_{00} & {r r}_{11} & \cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot; & {r r}_{n no - - 11} & {r r}_{n no} \end{matrix}]}}$

当对惯性设备直接输出滚转、俯仰、偏航角速度p，q，r采用三阶逼近描述时，所得结果也接近O(T³)，相比毕卡逼近等方法的O(T²)精度要高。When the inertial equipment directly outputs the roll, pitch, and yaw angular velocities p, q, and r using the third-order approximation description, the obtained result is also close to O(T ³ ), compared with the O(T ² ) accuracy of the Picard approximation and other methods to be tall.

Claims

1. a kind of quaternion Walsh approximate output method when the aircraft limit flight based on angular velocity, it is characterized in that comprising the following steps:

According to the quaternion continuous state equation

\overset{\cdot &Center Dot;}{e e} = = {A A}_{e e} e e

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} [\begin{matrix} 0 & - p & - q & - r \\ p & 0 & r & - q \\ q & - r & 0 & p \\ r & q & - p & 0 \end{matrix}]

Φ _e [(k+1)T, kT] is the state transition matrix of A _e , and 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;

Establishing Approximation Formula of State Transition Matrix

{Φ Φ}_{e e} [[((k k + + 11)) T T,, kT kT]] \approx \approx I I + + ΠHξ ΠHξ ((t t)) {| |}_{kT kT}^{((k k + + 11)) T T} + + Π Π {&Integral; &Integral;}_{kT kT}^{((k k + + 11)) T T} [[ξ ξ ((t t)) {ξ ξ}^{T T} ((t t))]] dt dt {H h}^{T T} {Π Π}_{11} - - ΠHξ ΠHξ ((t t)) {| |}_{kT kT}^{((k k + + 11)) T T} ΠHξ ΠHξ ((kT kT))

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

in

I = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}],

ξ(t)=[ξ ₀ (t) ξ ₁ (t) … ξ _n-1 (t) ξ _n (t)] ^T

(0≤t≤NT, k=0, 1, 2, ...) is a Walsh function (WalshFunction); k _j is the binary value of the binary expression of 0 or 1-k, ρ is the number of digits of the binary value, sgn represents the sign function; T is the sampling period; the expansion expressions of roll, pitch and yaw angular velocity p, q, r are respectively for

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{matrix} Π Π = = \frac{11}{22} {{[\begin{matrix} 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \\ 00 & 00 & 00 & 11 \\ 00 & 00 & - - 11 & 00 \end{matrix}] [\begin{matrix} {p p}_{00} & {p p}_{11} & . . . . . . & {p p}_{n no - - 11} & {p p}_{n no} \end{matrix}] \\ + + [\begin{matrix} 00 & 00 & - - 11 & 00 \\ 00 & 00 & 00 & - - 11 \\ 11 & 00 & 00 & 00 \\ 00 & 11 & 00 & 00 \end{matrix}] [\begin{matrix} {q q}_{00} & {q q}_{11} & . . . . . . & {q q}_{n no - - 11} & {q q}_{n no} \end{matrix}] + + [\begin{matrix} 00 & 00 & 00 & - - 11 \\ 00 & 00 & 11 & 00 \\ 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \end{matrix}] [\begin{matrix} {r r}_{00} & {r r}_{11} & . . . . . . & {r r}_{n no - - 11} & {r r}_{n no} \end{matrix}]}} \end{matrix}

\begin{matrix} {Π Π}_{11} = = \frac{11}{22} {{{[\begin{matrix} {p p}_{00} & {p p}_{11} & . . . . . . & {p p}_{n no - - 11} & {p p}_{n no} \end{matrix}]}^{T T} [\begin{matrix} 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \\ 00 & 00 & 00 & 11 \\ 00 & 00 & - - 11 & 00 \end{matrix}] \\ + + {[\begin{matrix} {q q}_{00} & {q q}_{11} & . . . . . . & {q q}_{n no - - 11} & {q q}_{n no} \end{matrix}]}^{T T} [\begin{matrix} 00 & 00 & - - 11 & 00 \\ 00 & 00 & 00 & - - 11 \\ 11 & 00 & 00 & 00 \\ 00 & 11 & 00 & 00 \end{matrix}] + + {[\begin{matrix} {r r}_{00} & {r r}_{11} & . . . . . . & {r r}_{n no - - 11} & {r r}_{n no} \end{matrix}]}^{T T} [\begin{matrix} 00 & 00 & 00 & - - 11 \\ 00 & 00 & 11 & 00 \\ 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \end{matrix}]}} \end{matrix}