CN102564423B - 基于角速度的欧拉角沃尔什近似输出方法 - Google Patents

Abstract

本发明公开了一种基于角速度的欧拉角沃尔什近似输出方法，用于解决现有的飞行器机动飞行时欧拉角输出精度差的技术问题。技术方案是通过引入多个参数并将滚转、俯仰、偏航角速度按照沃尔什函数多项式展开，按照依次求解俯仰角、滚转角、偏航角，直接对欧拉角的表达式进行高阶逼近积分，使得欧拉角的求解按照超线性逼近，保证了确定欧拉角的时间更新迭代计算精度，从而提高了惯性设备输出飞行姿态的准确性。

Description

基于角速度的欧拉角沃尔什近似输出方法

技术领域

本发明涉及一种飞行器机动飞行姿态确定方法，特别是涉及一种基于角速度的欧拉角沃尔什近似输出方法。

背景技术

惯性设备在运动体导航和控制中具有重要作用；刚体运动的加速度、角速度和姿态等通常都依赖于惯性设备输出，因此提高惯性设备的输出精度具有明确的实际意义；在惯性设备中，加速度采用加速度计、角速度采用角速率陀螺直接测量方式，刚体的姿态精度要求很高时如飞行试验等采用姿态陀螺测量，但在很多应用领域都有角速度等测量直接解算输出；主要原因是由于动态姿态传感器价格昂贵、体积大，导致很多飞行器采用角速率陀螺等解算三个欧拉角，使得姿态时间更新输出成为导航等核心内容，也使其成为影响惯导系统精度的主要因素之一，因此设计和采用合理的姿态时间更新输出方法就成为研究的热点课题；从公开发表的文献中对姿态输出主要基于角速度采用欧拉方程直接近似法或采用近似龙格库塔方法解算(孙丽、秦永元，捷联惯导系统姿态算法比较，中国惯性技术学报，2006，Vol.14(3)：6-10；Pu Li，Wang TianMiao，Liang JianHong，Wang Song，An Attitude Estimate Approach using MEMS Sensors forSmall UAVs，2006，IEEE International Conference on Industrial Informatics，1113-1117)；由于欧拉方程中三个欧拉角互相耦合，属于非线性微分方程，在不同初始条件和不同飞行状态下的误差范围不同，难以保证实际工程要求的精度。

发明内容

为了克服现有的飞行器机动飞行时欧拉角输出精度差的问题，本发明提供一种基于角速度的欧拉角沃尔什近似输出方法。该方法通过引入多个参数并将滚转、俯仰、偏航角速度按照沃尔什函数多项式展开，通过按照依次求解俯仰角、滚转角、偏航角，直接对欧拉角的表达式进行高阶逼近积分，使得欧拉角的求解按照超线性逼近，从而可以保证确定欧拉角的时间更新迭代计算精度和惯性单元的输出精度。

本发明解决其技术问题所采用的技术方案是：一种基于角速度的欧拉角沃尔什近似输出方法，其特点是包括以下步骤：

1、(a)根据欧拉方程：

式中：分别指滚转、俯仰、偏航角；p，q，r分别为滚转、俯仰、偏航角速度；全文参数定义相同；这三个欧拉角的计算按照依次求解俯仰角、滚转角、偏航角的步骤进行；滚转、俯仰、偏航角速度p，q，r的展开式分别为

p(t)＝[p₀ p₁ L p_n-1 p_n][ξ₀(t) ξ₁(t) L ξ_n-1(t) ξ_n(t)]^T

q(t)＝[q₀ q₁ L q_n-1 q_n][ξ₀(t) ξ₁(t) L ξ_n-1(t) ξ_n(t)]^T

r(t)＝[r₀ r₁ L r_n-1 r_n][ξ₀(t) ξ₁(t) L ξ_n-1(t) ξ_n(t)]^T

其中， ${\mathrm{\ξ}}_k\left(t\right)=\underset{j=0}{\overset{\mathrm{\ρ}-1}{\mathrm{\Π}}}\mathrm{sgn}\left\{\cos \right[k_j{2}^j\mathrm{\πt}/\left(\mathrm{NT}\right)\left]\right\}$ (0≤t≤NT，k＝0，1，2，L)为沃尔什函数(WalshFunction)；k_j为0或1-k的二进制表示式的二进制数值，ρ为二进制值位数，sgn表示符号函数；T为采样周期，全文符号定义相同；

(b)俯仰角的时间更新求解式为：

式中：

$a_1=1+\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$+0.25{\left\{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$+0.25{\left\{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$-0.25{\left\{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$a_2=\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$-0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]{\mathrm{dtH}}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$a_3=\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T$

$+0.5\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$-0.5\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$H=\left[\begin{array}{ccccc}\frac{1}{2}& & -\frac{2}{n}{I}_{\frac{n}{8}}& & \\ & O& & -\frac{1}{n}{I}_{\frac{n}{4}}& \\ \frac{2}{n}{I}_{\frac{n}{8}}& & 0_{\frac{n}{8}}& & -\frac{1}{2n}{I}_{\frac{n}{2}}\\ & \frac{1}{n}{I}_{\frac{n}{4}}& & 0_{\frac{n}{4}}& \\ & & \frac{1}{2n}{I}_{\frac{n}{2}}& & 0_{\frac{n}{2}}\end{array}\right]$

2、(a)在已知俯仰角的情况下，滚转角的时间更新求解式为：

其中

$a_4=1+\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$+0.25{\left\{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$-0.25{\left\{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$-0.25{\left\{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$a_5=\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$+0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]{\mathrm{dtH}}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$a_6=\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$-0.5\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$+0.5\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

(b)在俯仰角、滚转角已知情况下，偏航角的求解式为：

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^t[b_1\left(t\right)+b_2\left(t\right)]\mathrm{dt}$

式中：

本发明的有益效果是：由于通过引入多个参数并将滚转、俯仰、偏航角速度按照沃尔什函数多项式展开，通过按照依次求解俯仰角、滚转角、偏航角，直接对欧拉角的表达式进行高阶逼近积分，使得欧拉角的求解按照超线性逼近，从而保证了确定欧拉角的时间更新迭代计算精度和惯性单元的输出精度。

下面结合具体实施方式对本发明作详细说明。

具体实施方式

1、(a)根据刚体姿态方程(欧拉方程)：

其中：分别指滚转、俯仰、偏航角；p，q，r分别为滚转、俯仰、偏航角速度；全文参数定义相同；这三个欧拉角的计算按照依次求解俯仰角、滚转角、偏航角的步骤进行；滚转、俯仰、偏航角速度p，q，r的展开式分别为

p(t)＝[p₀ p₁ L p_n-1 p_n][ξ₀(t) ξ₁(t) L ξ_n-1(t) ξ_n(t)]^T

q(t)＝[q₀ q₁ L q_n-1 q_n][ξ₀(t) ξ₁(t) L ξ_n-1(t) ξ_n(t)]^T

r(t)＝[r₀ r₁ L r_n-1 r_n][ξ₀(t) ξ₁(t) L ξ_n-1(t) ξ_n(t)]^T

其中， ${\mathrm{\ξ}}_k\left(t\right)=\underset{j=0}{\overset{\mathrm{\ρ}-1}{\mathrm{\Π}}}\mathrm{sgn}\left\{\cos \right[k_j{2}^j\mathrm{\πt}/\left(\mathrm{NT}\right)\left]\right\}$ (0≤t≤NT，k＝0，1，2，L)为沃尔什函数(WalshFunction)；k_j为0或1-k的二进制表示式的二进制数值，ρ为二进制值位数，sgn表示符号函数；T为采样周期，全文符号定义相同；

(b)俯仰角的时间更新求解式为：

式中：

$a_1=1+\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$+0.25{\left\{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$+0.25{\left\{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$-0.25{\left\{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$a_2=\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$-0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]{\mathrm{dtH}}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$a_3=\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T$

$+0.5\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$-0.5\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$H=\left[\begin{array}{ccccc}\frac{1}{2}& & -\frac{2}{n}{I}_{\frac{n}{8}}& & \\ & O& & -\frac{1}{n}{I}_{\frac{n}{4}}& \\ \frac{2}{n}{I}_{\frac{n}{8}}& & 0_{\frac{n}{8}}& & -\frac{1}{2n}{I}_{\frac{n}{2}}\\ & \frac{1}{n}{I}_{\frac{n}{4}}& & 0_{\frac{n}{4}}& \\ & & \frac{1}{2n}{I}_{\frac{n}{2}}& & 0_{\frac{n}{2}}\end{array}\right]$

2、(a)在已知俯仰角的情况下，滚转角的时间更新求解式为：

其中

$a_4=1+\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$+\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}.$

${\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]}^T$

$+0.25{\left\{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$-0.25{\left\{\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$-0.25{\left\{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2$

$a_5=\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$+0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]{\mathrm{dtH}}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$-0.5\begin{array}{c}\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\end{array}H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$a_6=\left[\begin{array}{ccccc}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$-0.5\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

$+0.5\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}$

$\·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& L& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& L& q_{n-1}& q_n\end{array}\right]}^T$

(b)在俯仰角、滚转角已知情况下，偏航角的求解式为：

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^t[b_1\left(t\right)+b_2\left(t\right)]\mathrm{dt}$

式中：

当对惯性设备直接输出滚转、俯仰、偏航角速度p，q，r采用三阶逼近描述时，所得结果也接近O(T³)，相比欧拉方程直接近似法或采用近似龙格库塔方法解算等方法的O(T²)精度要高。

Claims (1)

1.一种基于角速度的飞行器机动飞行姿态确定方法，其特征在于包括以下步骤：

步骤1、（a）根据欧拉方程:

式中：分别指滚转、俯仰、偏航角；p,q,r分别为滚转、俯仰、偏航角速度；这三个欧拉角的计算按照依次求解俯仰角、滚转角、偏航角的步骤进行；滚转、俯仰、偏航角速度p,q,r的展开式分别为

$p\left(t\right)=\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\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$

$q\left(t\right)=\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\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$

$r\left(t\right)=\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\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$

其中，(0≤t≤NT,k=0,1,2,…)为沃尔什函数（WalshFunction）；k_j为0或1-k的二进制表示式的二进制数值，ρ为二进制值位数，sgn表示符号函数；T为采样周期；

（b）俯仰角的时间更新求解式为：

式中：

$\begin{array}{c}{a}_2=\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ -0.5\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ +0.5\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ \·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\begin{array}{c}\end{array}\end{array}$

$\begin{array}{c}{a}_3=\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T\\ +0.5\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ -0.5\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ \·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\begin{array}{c}\end{array}\end{array}$

步骤2、（a）在已知俯仰角的情况下，滚转角的时间更新求解式为：

其中

$\begin{array}{c}{a}_5=\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ +0.5\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ -0.5\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ \·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\begin{array}{c}\end{array}\end{array}$

$\begin{array}{c}{a}_6=\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ -0.5\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]{\∫}_{\mathrm{kT}}^{(k+1)T}\left[\mathrm{\ξ}\left(t\right){\mathrm{\ξ}}^T\left(t\right)\right]\mathrm{dt}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ +0.5\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ \·{\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_n\left(t\right)& {\mathrm{\ξ}}_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\begin{array}{c}\end{array}\end{array}$

（b）在俯仰角、滚转角已知情况下，偏航角的求解式为：

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^t[b_1\left(t\right)+b_2\left(t\right)]\mathrm{dt}$

式中：