CN104121907A - 一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法 - Google Patents

Abstract

本发明涉及一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法。本发明包括在计算时间更新时，设置初始值及罗德里格斯参数；计算容积点值；计算四元数误差点；计算四元数容积点；计算迭代四元数及迭代四元数误差；计算迭代容积点；估计一步预测状态；估计平方根因子预测误差协方差阵；计算测量更新中的容积点；计算迭代容积点值；计算预测测量估计；计算平方根因子新息协方差矩阵估计；计算互协方差阵；卡尔曼增益；状态更新估计；计算估计误差协方差阵及相应的平方根因子；四元数更新；计算相应的估计误差欧拉角。本发明能够提高对飞行器姿态的精确性与稳定性。

Description

一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法

技术领域

本发明属于非线性系统的飞行器姿态估计问题，特别涉及一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法。

背景技术

非线性滤波是信号处理、目标跟踪和控制领域方面的热门话题之一，特别是，在卡尔曼滤波框架下的非线性滤波研究仍是一个很火的问题，在近年来受到越来越多的关注和研究。

众所周知，姿态估计的本质是典型的非线性滤波问题。传统的非线性滤波器不能直接用来处理大多情况下的飞行器姿态确定。这是因为可用的姿态估计器需要非线性滤波器能够很好地结合实际的姿态估计背景。那就是，姿态估计原则必须要整合到非线性滤波器的设计过程中。传统的非线性滤波方法包括扩展卡尔曼滤波器(EKF)和无迹卡尔曼滤波器(UKF)。EKF是现在在非线性系统中应用最为广泛的方法。通过近似状态分布为高斯随机变量，然后对其进行一阶泰勒展开来近似非线性函数。但不幸的是，这种线性化方法只能得到低的估计精度，甚至会造成估计器的不稳定性。同时，EKF需要计算雅克比矩阵，且计算量会很大。为了克服EKF的线性化，无迹卡尔曼滤波器(UKF)是一个处理非线性滤波时更为实用的方法。因为UKF能够直接使用非线性系统的模型从而取代使用EKF时的线性化，同时也避免了对雅克比矩阵的计算。UKF是通过非线性无迹变换(UT)和最小采样即sigma点。这种策略使得UKF能够明显改善EKF的性能，尽管UKF已经有很多的应用，但它还是有很多缺点，尤其是在高维情况下。同时UKF计算过程中对于协方差矩阵并不能总是保证是正定的。因此，UKF的使用仍然受到限制。

发明内容

本发明针对现有技术的不足，提供了一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法。

本发明的方法具体是：

1).设置初始值参数，假设飞行器姿态带有平方根因子估计误差协方差S_0|0和飞行器姿态状态估计

2).设置姿态迭代其中k表示时间。

3).计算第i次容积点参量其中表示i个数，i＝1，2，...，m，S_k-1|k-1表示k-1时刻飞行器姿态带有平方根因子估计误差协方差阵，ξ_i表示列向量序列，表示k-1时刻飞行器姿态状态估计。

4).计算飞行器姿态参数误差点 $\mathrm{\δ}{\hat{q}}_{i,k-1|k-1}={\left[\begin{array}{cc}\mathrm{\δ}{\hat{q}}_{13,i,k-1|k-1}^T& \mathrm{\δ}{\hat{q}}_{4,i,k-1|k-1}\end{array}\right]}^T;$ 其中代表三个欧拉角，即飞行器姿态方位角，表示角度的余弦值。

5).将计算得到的飞行器姿态参数误差点带入第i次四元数容积点 ${\hat{q}}_{i,k-1|k-1}=\mathrm{\δ}{\hat{q}}_{i,k-1|k-1}\⊗{\hat{q}}_{k-1|k-1},$ 其中，为飞行器姿态参数估计。

6).迭代四元数预测估计及迭代四元数预测误差 $\mathrm{\δ}{\hat{q}}_{i,k|k-1}={\hat{q}}_{i,k|k-1}\⊗{\left[{\hat{q}}_{0,k|k-1}\right]}^{-1},$ Ω为飞行器角度函数。

7).迭代容积点其中，f为标量因子，a∈[0，1]。

8).计算一步状态预测估计带有平方根因子的预测误差协方差 $S_{k|k-1}=\mathrm{Tria}\left(\begin{array}{c}\end{array}\left[\begin{array}{cc}{X}_{k|k-1}^{*}& S_{\stackrel{\‾}{Q},k-1}\end{array}\right]\right),$ 为状态估计的均方差，为的平方根因子。

9).将带有平方根因子的预测误差协方差及飞行器姿态k-1时刻的状态预测估计值带入测量更新容积点方程

10).测量更新中的迭代容积点Z_i，k|k-1＝h[X_i，k|k-1，k]。

11).计算测量预测估计平方根因子新息协方差阵S_zz，k|k-1＝Tria([Z_k|k-1 S_R，k])、互协方差阵卡尔曼增益其中，S_R，k表示为测量噪声方差R_k的平方根因子。

12).从上面计算中得到状态更新估计平方根因子S_k|k、估计误差协方差阵P_k|k＝[X_k|k-1-K_kZ_k|k-1 K_kS_R，k][X_k|k-1-K_kZ_k|k-1 K_kS_R，k]^T。

13).计算更新四元数估计四元数估计误差

14).判断是k否小于迭代次数N，如果k小于N，则跳转到步骤2，k：＝k+1，否则，执行步骤15。

15).计算中得到三个欧拉角度(飞行器的滚动角，俯仰角和偏航角)的一致估计误差值

16).输出结果飞行器姿态方位角实现对飞行器姿态的估计。

本发明的有益效果：使用平方根容积卡尔曼滤波器的飞行器姿态估计方法，不仅提高了飞行器姿态估计的精确度和稳定性，而且使用(正交三角形)QR分解，避免在求解平方根操作中，出现误差协方差阵的对称性和正定性性质的破坏。

附图说明

图1为本发明方法流程图。

具体实施方式

下面首先给出姿态运动学模型的回顾，其次给出带有陀螺偏置的陀螺仪模型，再次给出广义罗格里格斯参数描述，再次对测量方程的建立，最后平方根容积卡尔曼滤波器的飞行器姿态估计的滤波公式，下面详细介绍本发明的实施过程。

1.姿态运动学模型

给出四元数表达式：

$q=\left[\begin{array}{c}{q}_{13}\\ q_4\end{array}\right]=\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]---\left(1\right)$

其中，

$q_{13}=\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\end{array}\right]\hat{e}\sin (\mathrm{\θ}/2)---\left(2\right)$

q₄＝cos(θ/2)          (3)

为对应于旋转轴的单位矢量和θ是旋转角度。从公式(2)和(3)中，我们可以得出结论四元数满足标准化约束

四元数相关的姿态矩阵为：

A(q)＝Ξ^T(q)ψ(q)            (4)

其中，

$\mathrm{\Ξ}\left(q\right)=\left[\begin{array}{c}{q}_4{I}_{3\×3}+[q_{13}\×]\\ -q_{13}^T\end{array}\right]---\left(5\right)$

$\mathrm{\ψ}\left(q\right)=\left[\begin{array}{c}{q}_4{I}_{3\×3}-[q_{13}\×]\\ {-q}_{13}^T\end{array}\right]---\left(6\right)$

其中，[q₁₃×]是差乘矩阵，它定义为：

$[q_{13}\×]=\left[\begin{array}{ccc}0& {-q}_3& q_2\\ q_3& 0& {-q}_1\\ {-q}_2& q_1& 0\end{array}\right]---\left(7\right)$

四元数的逆定义为：

$q^{-1}=\frac{q^{*}}{\left|\right|q\left|\right|}---\left(8\right)$

这里，

$q^{*}=\left[\begin{array}{c}{-q}_{13}\\ q_4\end{array}\right]---\left(9\right)$

因为四元数满足标准化约束，因此四元数的逆也可以表示为：

$q^{-1}=\left[\begin{array}{c}{-q}_{13}\\ q_4\end{array}\right]---\left(10\right)$

四元数的直乘定义为：

$q\⊗q^{\′}=\left[\begin{array}{cc}\mathrm{\ψ}\left(q\right)& q\end{array}\right]q^{\′}=\left[\begin{array}{cc}\mathrm{\Ξ}\left(q^{\′}\right)& q^{\′}\end{array}\right]q---\left(11\right)$

四元数姿态运动学方程表示为：

$\stackrel{\·}{q}=\frac{1}{2}\mathrm{\Ξ}\left(q\right)\mathrm{\ω}=\frac{1}{2}\mathrm{\Ω}\left(\mathrm{\ω}\right)q---\left(12\right)$

这里，ω是飞行器的角速率，且有

$\mathrm{\Ω}\left(\mathrm{\ω}\right)=\left[\begin{array}{cc}-[\mathrm{\ω}\×]& \mathrm{\ω}\\ {-\mathrm{\ω}}^T& 0\end{array}\right]---\left(13\right)$

式(12)的离散时间描述为：

q_k+1＝Ω(ω_k)q_k   (14)

其中，

${\mathrm{\Φ}}_k=\sin \left(0.5\right|\left|{\mathrm{\ω}}_k\right|\left|\mathrm{\Δt}\right){\mathrm{\ω}}_k/\left|\right|{\mathrm{\ω}}_k\left|\right|---\left(17\right)$

这里，Δt是陀螺仪的采样周期。

为了对比，介绍修正的罗德里格斯参数姿态运动学方程：

$\stackrel{\·}{p}=\frac{1}{4}[(1-p^{T}p)I_{3\×3}+2[p\×]+{2\mathrm{pp}}^T]\mathrm{\ω}---\left(18\right)$

其中，修正的罗德里格斯参数p＝[p_x p_v p_z]^T可以定义为：

$p=f\frac{q_{13}}{a+q_4}---\left(19\right)$

这里，a∈[0，1]并且f是以标量因子。

比较式(14)和式(18)，我们可以看出式(18)的表达式更为复杂。而式(14)比较简单且容易计算并更新。

2.陀螺仪模型

陀螺仪模型描述

$\widehat{\mathrm{\ω}}=\mathrm{\ω}+\mathrm{\β}+{\mathrm{\η}}_v---\left(20\right)$

其中，是陀螺仪的测量角度速率，ω是陀螺仪真实的角度速率，η_v是零均值高斯白噪声

$E\left\{{\mathrm{\η}}_v\left(t\right){\mathrm{\η}}_v^T\left(\mathrm{\τ}\right)\right\}={\mathrm{\σ}}_v^2\mathrm{\δ}(t-\mathrm{\τ})I_{3\×3}---\left(21\right)$

其中，δ(t-τ)是狄利克雷脉冲函数。

另外，陀螺仪偏置β模型可以表示为

β＝η_u  (22)

其中，为均值为零的高斯白噪声

$E\left\{{\mathrm{\η}}_u\left(t\right){\mathrm{\η}}_u^T\left(\mathrm{\τ}\right)\right\}={\mathrm{\σ}}_u^2\mathrm{\δ}(t-\mathrm{\τ})I_{3\×3}---\left(23\right)$

3.广义罗德里格斯参数

广义罗德里格斯参数定义为：

$\mathrm{\δp}=f\frac{\mathrm{\δ}{q}_{13}}{a+\mathrm{\δ}{q}_4}---\left(24\right)$

其中， $\mathrm{\δq}={\left[\begin{array}{cc}\mathrm{\δ}{q}_{13}^T& \mathrm{\δ}{q}_4\end{array}\right]}^T$ 是误差四元数，有a的参数范围为0到1。f是标量因子。如果a＝0和f＝1称为吉布斯矢量。但当a＝1和f＝1时，变成了标准修正罗德里格斯参数。在这里，选取a＝1和f＝2(a+1)。

GRP的逆传递函数的误差四元数定义为：

$\mathrm{\δ}{q}_4=\frac{-a{\left|\right|\mathrm{\δp}\left|\right|}^2+f\sqrt{f^2+(1-a^2){\left|\right|\mathrm{\δp}\left|\right|}^2}}{f^2+{\left|\right|\mathrm{\δp}\left|\right|}^2}---\left(25\right)$

δq₁₃＝f^-1(a+δq₄)δp  (26)

4.测量方程

传感器的测量方程为：

b_k＝A(q_k)r_k+v_k  (27)

其中，b_k和r_k是传感器在t_k时刻的单位矢量分别代表主体框架和惯性框架。

A(q_k)是姿态矩阵，v_k是均值为零的高斯白噪声且满足

$E\left\{v_k{v}_j^T\right\}={\mathrm{\σ}}^2{\mathrm{\δ}}_{\mathrm{kj}}{I}_{3\×3}---\left(28\right)$

测量方程为

$z_k=\left[\begin{array}{c}{b}_{1,k}\\ b_{2,k}\end{array}\right]=\left[\begin{array}{c}A\left(q_k\right)r_{1,k}\\ A\left(q_k\right)r_{2,k}\end{array}\right]+\left[\begin{array}{c}{v}_{1,k}\\ v_{2,k}\end{array}\right]---\left(29\right)$

从式(27)和式(28)，可以得出

$R_k=\mathrm{diag}\left[\begin{array}{cc}{\mathrm{\σ}}_1^2{I}_{3\×3}& {\mathrm{\σ}}_2^2{I}_{3\×3}\end{array}\right]---\left(30\right)$

其中，R_k是式(28)中的测量噪声协方差

5.飞行器平方根容积卡尔曼滤波器的姿态估计，参见图1

状态矢量为：

$x_k=\left[\begin{array}{c}{\mathrm{\δp}}_k\\ {\mathrm{\β}}_k\end{array}\right]---\left(31\right)$

5.1时间更新

1)设置迭代姿态为零矢量

2)容积点值其中m＝2n_x，n_x为状态矢量的维数。

其中[1]_i为第i^th行的点设置为[1]。

且有

$X_{i,k-1|k-1}=\left[\begin{array}{c}{X}_{i,k-1|k-1}^{\mathrm{\δp}}\\ X_{i,k-1|k-1}^{\mathrm{\β}}\end{array}\right]---\left(32\right)$

3)计算相关的误差四元数

$\mathrm{\δ}{\hat{q}}_{4,i,k-1|k-1}=\frac{-a{\left|\right|X_{i,k-1|k-1}^{\mathrm{\δp}}\left|\right|}^2+f\sqrt{f^2+(1-a^2){\left|\right|X_{i,k-1|k-1}^{\mathrm{\δp}}\left|\right|}^2}}{f^2+{\left|\right|X_{i,k-1|k-1}^{\mathrm{\δp}}\left|\right|}^2}---\left(33\right)$

$\mathrm{\δ}{\hat{q}}_{13,i,k-1|k-1}=f^{-1}[a+{\mathrm{\δ}\hat{q}}_{4,i,k-1|k-1}]X_{i,k-1|k-1}^{\mathrm{\δp}}---\left(34\right)$

4)计算四元数容积点

${\hat{q}}_{i,k-1|k-1}=\mathrm{\δ}{\hat{q}}_{i,k-1|k-1}\⊗{\hat{q}}_{k-1|k-1}---\left(35\right)$

其中，

$\mathrm{\δ}{\hat{q}}_{i,k-1|k-1}={\left[\begin{array}{cc}\mathrm{\δ}{\hat{q}}_{13,i,k-1|k-1}^T& \mathrm{\δ}{\hat{q}}_{4,i,k-1|k-1}\end{array}\right]}^T---\left(36\right)$

5)计算迭代四元数的估计值

${\hat{q}}_{i,k|k-1}=\mathrm{\Ω}\left[{\widehat{\mathrm{\ω}}}_{i,k-1|k-1}\right]{\hat{q}}_{i,k-1|k-1}---\left(37\right)$

其中，

${\widehat{\mathrm{\ω}}}_{i,k-1|k-1}={\widetilde{\mathrm{\ω}}}_{k-1}-X_{i,k-1|k-1}^{\mathrm{\β}},i=\mathrm{1,2},...,m---\left(38\right)$

${\widehat{\mathrm{\ω}}}_{0,k-1|k-1}={\widetilde{\mathrm{\ω}}}_{k-1}-{\hat{x}}_{k-1|k-1}^{\mathrm{\β}}---\left(39\right)$

${\hat{q}}_{0,k-1|k-1}={\hat{q}}_{k-1|k-1}---\left(40\right)$

6)计算迭代误差四元数估计值

$\mathrm{\δ}{\hat{q}}_{i,k|k-1}={\hat{q}}_{i,k|k-1}\⊗{\left[{\hat{q}}_{0,k|k-1}\right]}^{-1}---\left(41\right)$

7)计算迭代容积点

$X_{i,k|k-1}^{*\mathrm{\δp}}=f\frac{\mathrm{\δ}{\hat{q}}_{13,i,k|k-1}}{a+\mathrm{\δ}{\hat{q}}_{4,i,k|k-1}}---\left(42\right)$

$X_{i,k|k-1}^{*\mathrm{\β}}=X_{i,k-1|k-1}^{\mathrm{\β}}---\left(43\right)$

8)状态预测估计

${\hat{x}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}---\left(44\right)$

9)预测估计误差协方差的平方根因子

$S_{k|k-1}=\mathrm{Tria}(\left[\begin{array}{cc}{X}_{k|k-1}^{*}& S_{\stackrel{\‾}{Q},k-1}\end{array}\right])---\left(45\right)$

其中，的平方根因子为即 ${\stackrel{\‾}{Q}}_{k-1}=S_{{\stackrel{\‾}{Q}}_{k-1}}{S}_{{\stackrel{\‾}{Q}}_{k-1}}^T.$

有

$X_{k|k-1}^{*}=\frac{1}{\sqrt{m}}\left[\begin{array}{ccccc}{X}_{1,k|k-1}^{*}& -{\hat{x}}_{k|k-1}& \·\·\·& X_{m,k|k-1}^{*}& -{\hat{x}}_{k|k-1}\end{array}\right]---\left(46\right)$

${\stackrel{\‾}{Q}}_k=\frac{\mathrm{\Δt}}{2}\left[\begin{array}{cc}({\mathrm{\σ}}_v^2-\frac{1}{6}{\mathrm{\σ}}_u^2\mathrm{\Δ}{t}^2)I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& {\mathrm{\σ}}_u^2{I}_{3\×3}\end{array}\right]---\left(47\right)$

5.2测量更新

1)计算容积点

$X_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1}---\left(48\right)$

2)计算迭代容积点估计值

$Z_{i,k|k-1}=h[X_{i,k|k-1},k]=\left[\begin{array}{c}A\left({\hat{q}}_{i,k|k-1}\right)r_{1,k}\\ A\left({\hat{q}}_{i,k|k-1}\right)r_{2,k}\end{array}\right]---\left(49\right)$

3)测量预测估计

${\hat{z}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{Z}_{i,k|k-1}---\left(50\right)$

4)信息协方差阵的平方根因子估计

S_zz，k|k-1＝Tria([Z_k|k-1 S_R，k])     (51)

其中，S_R，k为测量噪声协方差R_k的平方根，且

$Z_{k|k-1}=\frac{1}{\sqrt{m}}\left[\begin{array}{ccccc}{Z}_{1,k|k-1}& -{\hat{z}}_{k|k-1}& \·\·\·& Z_{m,k|k-1}& -{\hat{z}}_{k|k-1}\end{array}\right]---\left(52\right)$

5)互协方差矩阵

$P_{\mathrm{xz},k|k-1}=X_{k|k-1}{Z}_{k|k-1}^T---\left(53\right)$

其中，

$X_{k|k-1}=\frac{1}{\sqrt{m}}\left[\begin{array}{ccccc}{X}_{1,k|k-1}& -{\hat{z}}_{k|k-1}& \·\·\·& X_{m,k|k-1}& -{\hat{z}}_{k|k-1}\end{array}\right]---\left(54\right)$

6)计算卡尔曼增益

$K_k=(P_{\mathrm{xz},k|k-1}/S_{\mathrm{zz},k|k-1}^T)/S_{\mathrm{zz},k|k-1}---\left(55\right)$

7)状态更新估计值

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})---\left(56\right)$

其中，

${\hat{x}}_{k|k}={\left[\begin{array}{cc}\mathrm{\δ}{\hat{p}}_{k|k}^T& {\widehat{\mathrm{\β}}}_{k|k}^T\end{array}\right]}^T---\left(57\right)$

8)估计误差协方差阵

P_k|k＝[X_k|k-1-K_kZ_k|k-1 K_kS_R，k][X_k|k-1-K_kZ_k|k-1 K_kS_R，k]^T   (58)

9)四元数更新

${\hat{q}}_{k|k}=\mathrm{\δ}{\hat{q}}_{k|k}\⊗{\hat{q}}_{0,k|k-1}---\left(59\right)$

其中， $\mathrm{\δ}{\hat{q}}_{k|k}={\left[\begin{array}{cc}\mathrm{\δ}{\hat{q}}_{13,k|k}^T& \mathrm{\δ}{\hat{q}}_{4,k|k}\end{array}\right]}^T,$ 且有

$\mathrm{\δ}{\hat{q}}_{4,k|k}=\frac{-a{\left|\right|\mathrm{\δ}{\hat{p}}_{k|k}\left|\right|}^2+f\sqrt{f^2+(1-a^2){\left|\right|\mathrm{\δ}{\hat{p}}_{k|k}\left|\right|}^2}}{f^2+{\left|\right|\mathrm{\δ}{\hat{p}}_{k|k}\left|\right|}^2}---\left(60\right)$

$\mathrm{\δ}{\hat{q}}_{13,k|k}=f^{-1}(a+\mathrm{\δ}{\hat{q}}_{4,k|k})\mathrm{\δ}{\hat{p}}_{k|k}---\left(61\right)$

10)计算误差欧拉角相关估计

其中，θ和ψ分别表示为横滚角和偏航角，且有

$\mathrm{\δ}{\hat{q}}_{13,k|k}=\left[\begin{array}{c}\mathrm{\δ}{\hat{q}}_{1,k|k}\\ \mathrm{\δ}{\hat{q}}_{2,k|k}\\ \mathrm{\δ}{\hat{q}}_{3,k|k}\end{array}\right]---\left(63\right)$

本发明提出的新型平方根容积卡尔曼滤波(SCKF)，使用径向规则和球面积分规则来最优化容积点和权重。同时SCKF能够有效地应用在高维的非线性滤波中，具有计算代价低，精确性和稳定性也一定程度上进行了提高。

Claims (1)

1.一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法，其特征在于该方法包括以下步骤：

1).设置初始值参数，假设飞行器姿态带有平方根因子估计误差协方差S_0|0和飞行器姿态状态估计

2).设置姿态迭代其中k表示时间；

3).计算第i次容积点参量其中表示i个数，i＝1，2，...，m，S_k-1|k-1表示k-1时刻飞行器姿态带有平方根因子估计误差协方差阵，ξ_i表示列向量序列，表示k-1时刻飞行器姿态状态估计；

4).计算飞行器姿态参数误差点 $\mathrm{\δ}{\hat{q}}_{i,k-1|k-1}={\left[\begin{array}{cc}\mathrm{\δ}{\hat{q}}_{13,i,k-1|k-1}^T& \mathrm{\δ}{\hat{q}}_{4,i,k-1|k-1}\end{array}\right]}^T;$ 其中代表三个欧拉角，即飞行器姿态方位角，表示角度的余弦值；

5).将计算得到的飞行器姿态参数误差点带入第i次四元数容积点 ${\hat{q}}_{i,k-1|k-1}=\mathrm{\δ}{\hat{q}}_{i,k-1|k-1}\⊗{\hat{q}}_{k-1|k-1},$ 其中，为飞行器姿态参数估计；

6).迭代四元数预测估计及迭代四元数预测误差 $\mathrm{\δ}{\hat{q}}_{i,k|k-1}={\hat{q}}_{i,k|k-1}\⊗{\left[{\hat{q}}_{0,k|k-1}\right]}^{-1},$ Ω为飞行器角度函数；

7).迭代容积点其中，f为标量因子，a∈[0，1]；

8).计算一步状态预测估计带有平方根因子的预测误差协方差为状态估计的均方差，为的平方根因子；

9).将带有平方根因子的预测误差协方差及飞行器姿态k-1时刻的状态预测估计值带入测量更新容积点方程

10).测量更新中的迭代容积点Z_i，k|k-1＝h[X_i，k|k-1，k]；

11).计算测量预测估计平方根因子新息协方差阵S_zz,k|k-1＝Tria([Z_k|k-1 S_R，k])、互协方差阵卡尔曼增益其中，S_R，k表示为测量噪声方差R_k的平方根因子；

12).从上面计算中得到状态更新估计平方根因子S_k|k、估计误差协方差阵P_k|k＝[X_k|k-1-K_kZ_k|k-1 K_kS_R，k][X_k|k-1-K_kZ_k|k-1 K_kS_R，k]^T；

13).计算更新四元数估计四元数估计误差

14).判断是k否小于迭代次数N，如果k小于N，则跳转到步骤2，k：＝k+1，否则，执行步骤15；

15).计算中得到三个欧拉角度(飞行器的滚动角，俯仰角和偏航角)的一致估计误差值

16).输出结果飞行器姿态方位角实现对飞行器姿态的估计。