CN102494689A - 基于角速度的欧拉角多项式类近似输出方法 - Google Patents

Abstract

本发明公开了一种基于角速度的欧拉角多项式类近似输出方法，用于解决现有的飞行器机动飞行时欧拉角输出精度差的技术问题。技术方案是通过引入多个参数并采用可以描述任意有限的统一多项式对滚转、俯仰、偏航角速度p，q，r进行近似逼近描述，可以根据工程精度的要求，可以事先定义常数矩阵M，减少对滚转、俯仰、偏航角速度p，q，r多项式描述的阶次，按照依次求解俯仰角、滚转角、偏航角，直接对欧拉角的表达式进行高阶逼近积分，使得欧拉角的求解按照超线性逼近，保证了确定欧拉角的时间更新迭代计算精度，从而提高了惯性设备输出飞行姿态的准确性。

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)；由于欧拉方程中三个欧拉角互相耦合，属于非线性微分方程，在不同初始条件和不同飞行状态下的误差范围不同，难以保证实际工程要求的精度。

发明内容

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

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

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

式中：

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

p(t)＝[p₀ p₁ L p_n-1 p_n]M[1 t L t^n-1 tⁿ]^T

q(t)＝[q₀ q₁ L q_n-1 q_n]M[1 t L t^n-1 tⁿ]^T

r(t)＝[r₀ r₁ L r_n-1 r_n]M[1 t L t^n-1 tⁿ]^T

其中：M为事先定义的常数矩阵；定义T为采样周期，全文符号定义相同；

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

式中：

$a_1=1+\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\mathrm{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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]\mathrm{MH}{\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{M}^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{MH}{\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{M}^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{MH}{\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{M}^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]\mathrm{MH}{\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]\mathrm{MH}{\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]\mathrm{MH}{\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]\mathrm{MH}{\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}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{M\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{MH}{\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{M}^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]\mathrm{MH}{\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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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]\mathrm{MH}{\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{M}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$H=\mathrm{diag}\{1,\frac{1}{2},\frac{1}{3},L,\frac{1}{n},\frac{1}{n+1}\};$

$\mathrm{\Ω}\left(t\right)=\left[\begin{array}{ccccc}\frac{1}{2}{t}^2& \frac{1}{3}{t}^3& L& \frac{1}{n+1}{t}^{n+1}& \frac{1}{n+2}{t}^{n+2}\\ \frac{1}{3}{t}^3& \frac{1}{4}{t}^4& L& \frac{1}{n+2}{t}^{n+2}& \frac{1}{n+3}{t}^{n+3}\\ M& M& O& M& M\\ \frac{1}{n+1}{t}^{n+1}& \frac{1}{n+2}{t}^{n+2}& L& \frac{1}{2n}{t}^{2n}& \frac{1}{2n+1}{t}^{2n+1}\\ \frac{1}{n+2}{t}^{n+2}& \frac{1}{n+3}{t}^{n+3}& L& \frac{1}{2n+1}{t}^{2n+1}& \frac{1}{2n+2}{t}^{2n+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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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]\mathrm{MH}{\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{M}^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{MH}{\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{M}^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{MH}{\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{M}^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]\mathrm{MH}{\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]\mathrm{MH}{\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]\mathrm{MH}{\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]\mathrm{MH}{\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}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]{\mathrm{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{MH}{\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{M}^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]\mathrm{MH}{\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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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]\mathrm{MH}{\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{M}^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进行近似逼近描述，通过按照依次求解俯仰角、滚转角、偏航角，直接对欧拉角的表达式进行高阶逼近积分，使得欧拉角的求解按照超线性逼近，从而保证了确定欧拉角的时间更新迭代计算精度和惯性单元的输出精度。

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

具体实施方式

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

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

p(t)＝[p₀ p₁ L p_n-1 p_n]M[1 t L t^n-1 tⁿ]^T

q(t)＝[q₀ q₁ L q_n-1 q_n]M[1 t L t^n-1 tⁿ]^T

r(t)＝[r₀ r₁ L r_n-1 r_n]M[1 t L t^n-1 tⁿ]^T

其中：M为事先定义的常数矩阵，定义T为采样周期；

对于Chebyshev(切比雪夫)正交多项式：

$\left.\begin{array}{c}{\mathrm{\ξ}}_0\left(t\right)=1\\ {\mathrm{\ξ}}_1\left(t\right)=1-2t/b\\ {\mathrm{\ξ}}_2\left(t\right)=8{(t/b)}^2-8(t/b)+1\\ M\\ {\mathrm{\ξ}}_{i+1}\left(t\right)=2{\mathrm{\ξ}}_1\left(t\right){\mathrm{\ξ}}_i\left(t\right)-{\mathrm{\ξ}}_{i-1}\left(t\right)\end{array}\right\}$ i＝2，3，L，n-1，0≤t≤NT，b＝NT

则常数矩阵

$M=\left\{m(i,j)\right\}=\left[\begin{array}{c}{m}_1\\ m_2\\ m_3\\ M\\ m_N\end{array}\right]=\left[\begin{array}{ccccc}1& 0& 0& L& 0\\ 1& -\frac{2}{b}& 0& L& 0\\ 1& -\frac{8}{b}& \frac{8}{b^2}& L& 0\\ M& M& M& O& M\\ & & & & \end{array}\right]$

$m(i,j)=2m(i-1,j)-m(i-2,j)-\frac{4}{b}m(i-1,j-1),$ (i＝3，4，L，N；j＝1，2，L，i)

m(i，j)＝0，(j＞i)

m(i，0)＝0，(j＝1，2，L，N)

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

式中：

$a_1=1+\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\mathrm{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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]\mathrm{MH}{\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{M}^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{MH}{\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{M}^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{MH}{\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{M}^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]\mathrm{MH}{\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]\mathrm{MH}{\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]\mathrm{MH}{\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]\mathrm{MH}{\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}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]{\mathrm{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{MH}{\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{M}^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]\mathrm{MH}{\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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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]\mathrm{MH}{\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{M}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$H=\mathrm{diag}\{1,\frac{1}{2},\frac{1}{3},L,\frac{1}{n},\frac{1}{n+1}\};$

$\mathrm{\Ω}\left(t\right)=\left[\begin{array}{ccccc}\frac{1}{2}{t}^2& \frac{1}{3}{t}^3& L& \frac{1}{n+1}{t}^{n+1}& \frac{1}{n+2}{t}^{n+2}\\ \frac{1}{3}{t}^3& \frac{1}{4}{t}^4& L& \frac{1}{n+2}{t}^{n+2}& \frac{1}{n+3}{t}^{n+3}\\ M& M& O& M& M\\ \frac{1}{n+1}{t}^{n+1}& \frac{1}{n+2}{t}^{n+2}& L& \frac{1}{2n}{t}^{2n}& \frac{1}{2n+1}{t}^{2n+1}\\ \frac{1}{n+2}{t}^{n+2}& \frac{1}{n+3}{t}^{n+3}& L& \frac{1}{2n+1}{t}^{2n+1}& \frac{1}{2n+2}{t}^{2n+2}\end{array}\right]$

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

其中

$a_4=1+\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\mathrm{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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]\mathrm{MH}{\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{M}^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{MH}{\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{M}^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{MH}{\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{M}^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]\mathrm{MH}{\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]\mathrm{MH}{\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]\mathrm{MH}{\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]\mathrm{MH}{\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}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]{\mathrm{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{MH}{\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{M}^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]\mathrm{MH}{\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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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]\mathrm{MH}{\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{M}^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(t)＝[p₀ p₁ L p_n-1 p_n]M[1 t L t^n-1 tⁿ]^T

q(t)＝[q₀ q₁ L q_n-1 q_n]M[1 t L t^n-1 tⁿ]^T

r(t)＝[r₀ r₁ L r_n-1 r_n]M[1 t L t^n-1 tⁿ]^T

其中：M为事先定义的常数矩阵；定义T为采样周期，全文符号定义相同；

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

式中：

$a_1=1+\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]{\mathrm{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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]\mathrm{MH}{\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{M}^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{MH}{\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{M}^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{MH}{\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{M}^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]\mathrm{MH}{\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]\mathrm{MH}{\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]\mathrm{MH}{\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]\mathrm{MH}{\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}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{M\Ω}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{MH}{\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{M}^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]\mathrm{MH}{\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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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]\mathrm{MH}{\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{M}^T{\left[\begin{array}{ccccc}{p}_0& p_1& L& p_{n-1}& p_n\end{array}\right]}^T$

$H=\mathrm{diag}\{1,\frac{1}{2},\frac{1}{3},L,\frac{1}{n},\frac{1}{n+1}\};$

$\mathrm{\Ω}\left(t\right)=\left[\begin{array}{ccccc}\frac{1}{2}{t}^2& \frac{1}{3}{t}^3& L& \frac{1}{n+1}{t}^{n+1}& \frac{1}{n+2}{t}^{n+2}\\ \frac{1}{3}{t}^3& \frac{1}{4}{t}^4& L& \frac{1}{n+2}{t}^{n+2}& \frac{1}{n+3}{t}^{n+3}\\ M& M& O& M& M\\ \frac{1}{n+1}{t}^{n+1}& \frac{1}{n+2}{t}^{n+2}& L& \frac{1}{2n}{t}^{2n}& \frac{1}{2n+1}{t}^{2n+1}\\ \frac{1}{n+2}{t}^{n+2}& \frac{1}{n+3}{t}^{n+3}& L& \frac{1}{2n+1}{t}^{2n+1}& \frac{1}{2n+2}{t}^{2n+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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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]\mathrm{MH}{\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{M}^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{MH}{\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{M}^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{MH}{\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{M}^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]\mathrm{MH}{\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]\mathrm{MH}{\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]\mathrm{MH}{\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]\mathrm{MH}{\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}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]{\mathrm{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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}{r}_0& r_1& L& r_{n-1}& r_n\end{array}\right]\mathrm{MH}{\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{M}^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]\mathrm{MH}{\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{M\Ω}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{H}^T{M}^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]\mathrm{MH}{\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{M}^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}$

式中：