CN108225337B - 基于sr-ukf滤波的星敏感器和陀螺组合定姿方法 - Google Patents

Abstract

本发明公开了一种基于SR‑UKF滤波的星敏感器和陀螺组合定姿方法，属于测绘卫星或其他航天器的高精度组合定姿技术领域。目的是提出一种基于SR‑UKF滤波的星敏感器/陀螺组合定姿方法，将SR‑UKF滤波算法用于星敏和陀螺组合定姿，对于现如今传统的EKF滤波方法有较大提升。所述方法具体包括：步骤1仿真出星敏感器四元数和陀螺的角速度；步骤2以误差四元数及陀螺随机漂移误差为状态变量，利用SR‑UKF算法融合处理星敏感器和陀螺的姿态信息进行滤波处理，并进行反馈，通过迭代滤波处理尽量消除星敏感器和陀螺的误差影响，求解高精度的姿态信息。

Description

基于SR-UKF滤波的星敏感器和陀螺组合定姿方法

技术领域

本发明涉及一种卫星的星敏感器和陀螺组合定姿方法，特别是一种基于SR-UKF滤波的星敏感器和陀螺组合定姿方法，可用于测绘卫星或其他航天器的高精度组合定姿。

背景技术

高精度立体测绘卫星为了获取地相机任意摄影时刻的姿态信息，通常采用星敏和陀螺构成姿态测量系统。陀螺仪的优点是获取的角速度信息在一定时间范围内精度高、具有优良的实时性，但是其缺点是随着时间增长会产生一定测量误差，星敏可以获得高精度的姿态信息，但是其频率较低。星敏和陀螺组合应用可以克服各自的缺点，能够获得高精度高更新率的姿态信息。

由于描述卫星姿态的系统模型一般是非线性的,所以姿态确定问题的实质是一个非线性滤波问题，滤波过程中，线性化误差、测量噪声统计特性的不确定性和敏感器相对安装误差对滤波精度都有小幅度影响，但随着应用背景对性能指标提出高精度要求，这些因素造成的影响都呈现出来,它们在不同程度上影响姿态的确定精度和有效载荷的指向精度。

平方根无迹卡尔曼滤波(SR-UKF)是一种新颖的非线性系统的滤波方法，与普通的无迹卡尔曼滤波(UKF)不同，它不必每次时间更新都计算状态协方差矩阵的平方根，而是利用矩阵QR分解、矩阵 Cholesky分解因数更新以及最小二乘等强有力的线性代数技术，以Cholesky分解因数的形式直接传播和更新状态协方差矩阵的平方根，因而可以降低计算复杂度，获得更高的效率。针对组合定姿系统的非线性问题，在SR-UKF中用协方差平方根阵代替协方差阵参加迭代运算，有效地避免了滤波器的发散，提高了滤波算法的收敛速度和稳定性，更好地满足了组合定姿系统的机动特性。

在卫星组合定姿技术方面，以往都采用扩展卡尔曼滤波 EKF(Extended KalmanFilter)方法，EKF是传统非线性估计中的代表，其基本思想是将非线性状态函数和量测函数进行局部线性化，即进行一阶Taylor多项式展开，然后应用线性系统Kalman滤波公式，尽管EKF得到了广泛的应用，但它依然存在自身无法克服的理论局限性:①要求非线性系统状态函数和量测函数必须是连续可微的，这限制了EKF的应用范围；②对非线性函数的一阶线性化近似精度偏低，特别地，当系统具有强非线性时，EKF估计精度严重下降，甚至发散；③需要计算非线性函数的雅克比矩阵，容易造成EKF数值稳定性差和出现计算发散。

为突破上面三个缺点，近几年来人们基于近似一个高斯分布比近似一个任意的非线性函数容易的观点，提出了一些相关的算法，与 EKF中采用泰勒展开忽略高阶项进行线性化不同，这些算法利用统计线性化方法，通过特定的采样方法确定一系列采样点(Sigmapoints) 来获取系统的相关统计参量，将非线性映射直接作用于各sigma点，根据映射后的点集重建统计参量，然后根据新的统计参量重新选择 Sigma点集并重复上述过程。这种方法可以在不必对非线性映射近似的情况下，使一个随机变量的分布按非线性映射递推传播。这种利用sigma点重建统计参量的方法统称为sigma点卡尔曼滤波，包括了无迹卡尔曼和中心差分卡尔曼滤波。

与标准的无迹卡尔曼滤波相比，平方根形式的无迹卡尔曼滤波 (SR-UKF)又有两个明显的优点：(1)Sigma点集的构成和不确定性的传播都只需要协方差矩阵的平方根，而不再需要完整的协方差矩阵，只存储和运算平方根因数可以降低计算负担获得更高的效率；(2) 为使协方差矩阵有意义，必须保证它是一个非负定矩阵，而对于非线性系统方程来说，不能完全确保其非负定性，但是利用平方根的平方得到的协方差矩阵一定是非负定的，因此，方根形式可以获得更稳定可靠的结果。有鉴于此，在实际应用中应优先考虑平方根形式的UKF。

发明内容

因此，针对现有技术的上述问题，本发明为克服现有技术的不足，提出一种基于SR-UKF滤波的星敏感器/陀螺组合定姿方法，将 SR-UKF滤波算法用于星敏和陀螺组合定姿，对于现如今传统的EKF 滤波方法有较大提升。

具体的，所述方法具体包括：

步骤1仿真出星敏感器四元数和陀螺的角速度；

步骤2以误差四元数及陀螺随机漂移误差为状态变量，利用 SR-UKF算法融合处理星敏感器和陀螺的姿态信息进行滤波处理，并进行反馈，通过迭代滤波处理消除星敏感器和陀螺的误差影响，求解高精度的姿态信息。

进一步的，所述方法中步骤1中仿真的输入输出具体包括：

输入：

长半径；偏心率；轨道倾角；升交点赤经；近地点角距；过近地点时刻；地球引力常数；采样间隔；星敏感器噪声误差；第一星敏感器在卫星本体坐标系中的方向角；第二星敏感器在卫星本体坐标系中的方向角；

输出：

无误差姿态四元数；有误差姿态四元数；无误差陀螺角速度；有误差陀螺角速度。

进一步的，所述方法中步骤1具体包括：

步骤1a参数设定中主要设定卫星轨道参数、两个星敏感器的安装参数，采样频率等；

步骤1b卫星轨道瞬时位置计算中主要是利用卫星的轨道参数计算卫星的位置，主要过程包括地心系与轨道坐标系之间的关系求解、卫星地心系坐标解算、卫星轨道坐标系解算等；

步骤1c利用两个星敏感器的安装参数以及瞬时卫星位置信息，构建矢量信息，通过定姿计算得到无误差的卫星姿态信息；

步骤1d利用两个星敏感器的安装参数获取主光轴信息，添加随机噪声后，联合卫星的瞬时位置信息，构建矢量信息，采用QUEST定姿方法解算得到带误差的卫星姿态信息；

步骤1e利用解算得到的无误差的姿态信息，通过角速度的变换量计算，得到无误差的陀螺数据；

步骤1f利用得到的无误差陀螺数据，在三轴添加随机噪声，得到带误差的陀螺数据。

进一步的，所述方法中步骤2具体包括：

步骤2a参数设置

设置陀螺误差因数，将陀螺误差因数设置为从0.1到2.5，每隔0.2 取一次；读入数据，包括带误差和不带误差的四元数姿态数据以及角速度姿态数据；状态变量为误差四元数和陀螺随机漂移误差，状态初值：

X₀＝[Δq_ini3×3 Δd_ini3×3]＝[0 0 0 0 0 0]

系统噪声矩阵

测量噪声协方差矩阵H＝[I_3×3 0_3×3]，其中q_rms为星敏测量中误差；

陀螺随机漂移的中误差d_rms＝10^-6[1 1 1]

初始

步骤2b设置状态方程与量测方程，状态方程如下：

x_k+1＝f(x_k)+w_k

量测方程：z_k＝h(x_k)+v_k，量测量是指由预测四元数和星敏感器得到的带误差姿态四元数二者之间的差值，q_predic为星敏感器输出的姿态四元数，q_g为陀螺数据转换的四元数；

步骤2c设置SR-UKF滤波输入的参量；

步骤2d计算权重

W_i ^(m)＝W_i ^(c)＝λ/[2(n+λ)]i＝1,…，2n

其中，λ＝α²(n+κ)-n,

为刻度参数

步骤2e创建状态值x的sigma点

由时刻k-1的

和k时刻的S_k来计算的Sigma点集

设状态向量为n维,

为时刻k-1的状态向量估计值,P_k-1为该时刻状态向量的协方差矩阵,2n+1维的Sigma点集可以表示为:

每个sigma点可分为以下两部分；

步骤2f对状态方程进行UT变换

将步骤2e得到的sigma点代入非线性状态函数中，然后进行加权处理得到一步预测状态值再利用QR分解及Cholesky因式得到预测 Sk；

χ_k|k-1＝f(χ_k-1)

其中，预测sigma点

步骤2g对量测方程进行UT变换

将sigma点代入量测函数中，然后进行加权处理得到一步预测量测值再利用QR分解及Cholesky因式得到预测Syk；

将步骤2f得到的预测sigma点带入非线性量测方程；

步骤2h计算互协方差矩阵值

步骤2i更新状态值及协方差矩阵值

步骤2j四元数修正

四元数修正是通过对预测四元数和状态更新后误差四元数做叉乘实现；姿态四元数修正方程：

为k时刻误差四元数估计值，

为k时刻的四元数预测值。

本发明的技术效果为，本发明提出的独特的星敏与陀螺数据仿真算法，用QUEST定姿算法将卫星上配置的两个星敏数据进行处理得到独立的四元数姿态数据与三轴角位移数据。解决系统非线性和噪声非高斯问题，以快速获得高精度的姿态数据，并能够准确地估计陀螺随机漂移，实现星敏感器和陀螺的长时间、高精度的组合定姿。

附图说明：

图1为星敏感器和陀螺数据数据仿真流程图；

图2为步骤2的流程图；

图3为星敏(误差1.0”频率4hz)陀螺(频率8hz)SR-UKF联合定姿的三轴中误差示意图；

图4为星敏(1.0",4Hz)和不同采样率陀螺数据SR-UKF组合定姿的俯仰精度对比图；

图5为星敏(1.0",4Hz)和不同采样率陀螺数据SR-UKF组合定姿的横滚精度对比图；

图6为星敏(1.0",4Hz)和不同采样率陀螺数据SR-UKF组合定姿的偏转精度对比图；

图7为星敏(误差1.0”频率4hz)陀螺(误差0.5”频率8hz)SR-UKF 联合定姿的姿态角误差示意图；

图8为星敏(误差1.0”频率4hz)陀螺(误差0.5”频率8hz)SR-UKF 联合定姿的随机漂移误差示意图；

图9为星敏(1.0”,4hz)陀螺(0.5”,8hz)联合定姿EKF和 SR-UKF的姿态角误差对比示意图；

图10为星敏(1.0”,4hz)陀螺(0.5”,8hz)联合定姿EKF和 SR-UKF的随机漂移误差对比示意图。

具体实施方式

下面对本发明的具体实施方式进行说明：

本实施例中，提供一种基于SR-UKF滤波的星敏感器/陀螺组合定姿方法，其实现主要概括为两个主要步骤：

1、由于星敏感器和陀螺的真实数据难获得，需要仿真出星敏感器四元数和陀螺的角速度，主要目的是为后续的联合定姿算法研究提供数据支持。

2、以误差四元数及陀螺随机漂移误差为状态变量，利用SR-UKF 算法融合处理星敏感器和陀螺的姿态信息进行滤波处理，并进行反馈，通过迭代滤波处理尽量消除星敏感器和陀螺的误差影响，求解高精度的姿态信息。

针对第1步：

利用MATLAB 2013b对星敏感器和陀螺数据进行仿真，仿真流程图如图1所示。

仿真的输入输出如下：

输入：

长半径；偏心率；轨道倾角；升交点赤经；近地点角距；过近地点时刻；地球引力常数；采样间隔(可根据实际需要修改)；星敏感器噪声误差(可根据实际需要修改)；星敏感器1在卫星本体坐标系中的方向角；星敏感器2在卫星本体坐标系中的方向角。

输出：

无误差姿态四元数；有误差姿态四元数；无误差陀螺角速度；有误差陀螺角速度。

具体步骤如下：

(1)参数设定中主要设定卫星轨道参数、两个星敏感器的安装参数，采样频率等；

(2)卫星轨道瞬时位置计算中主要是利用卫星的轨道参数计算卫星的位置，主要过程包括地心系与轨道坐标系之间的关系求解、卫星地心系坐标解算、卫星轨道坐标系解算等；

(3)利用两个星敏感器的安装参数以及瞬时卫星位置信息，构建矢量信息，通过定姿计算得到无误差的卫星姿态信息。

(4)利用两个星敏感器的安装参数获取主光轴信息，添加随机噪声后，联合卫星的瞬时位置信息，构建矢量信息，采用QUEST定姿方法解算得到带误差的卫星姿态信息。

(5)利用解算得到的无误差的姿态信息，通过角速度的变换量计算，得到无误差的陀螺数据；

(6)利用得到的无误差陀螺数据，在三轴添加随机噪声，得到带误差的陀螺数据。

针对第二步：如图2所示，具体包括：

1)设置状态方程与量测方程，状态方程如下：

x_k+1＝f(x_k)+w_k

量测方程：z_k＝h(x_k)+v_k，量测量是指由预测四元数和星敏感器得到的带误差姿态四元数二者之间的差值，其中q_predic为星敏感器输出的姿态四元数，q_g为陀螺数据转换的四元数。

2)设置UKF滤波输入的参量

(状态方程、量测方程、状态初值、量测量、P0、Q、R)

系统噪声矩阵

初始协方差矩阵

状态变量为误差四元数和陀螺随机漂移误差，状态初值

X₀＝[Δq_ini3×3 Δd_ini3×3]＝[0 0 0 0 0 0]

3)计算权重

W_i ^(m)＝W_i ^(c)＝λ/[2(n+λ)]i＝1,…，2n

其中，λ＝α²(n+κ)-n

4)创建状态值x的sigma点

由时刻k-1的

和

来计算的Sigma点集

设状态向量为n维,

为时刻k-1的状态向量估计值,P_k-1为该时刻状态向量的协方差矩阵,2n+1维的Sigma点集可以表示为:

每个sigma点可分为以下两部分：

5)对状态方程进行UT变换

将上一步得到的sigma点代入非线性状态函数中，然后进行加权处理得到一步预测状态值和预测协方差矩阵值。

(此步得到的预测sigma点

)

6)对量测方程进行UT变换

(将第5步得到的预测sigma点带入非线性量测方程。)

7)计算互协方差矩阵值

8)更新状态值及协方差矩阵值

9)四元数修正

四元数修正是通过对预测四元数和状态更新后误差四元数做叉乘实现。姿态四元数修正方程：

为k时刻误差四元数估计值，

为k时刻的四元数预测值。

实例及精度评价

1)参数设置

设置陀螺误差因数，将陀螺误差因数设置为从0.1到2.5，每隔0.2 取一次，以控制后面加误差的大小，便于最后分析陀螺误差对定姿精度的影响。读入数据，包括带误差和不带误差的四元数姿态数据以及角速度姿态数据；状态变量为误差四元数和陀螺随机漂移误差，状态初值：

X₀＝[Δq_ini3×3 Δd_ini3×3]＝[0 0 0 0 0 0]

系统噪声矩阵

测量噪声协方差矩阵H＝[I_3×3 0_3×3]，其中q_rms为星敏测量中误差。

陀螺随机漂移的中误差d_rms＝10^-6[1 1 1]

初始

1)设置状态方程与量测方程，状态方程如下：

x_k+1＝f(x_k)+w_k

量测方程：z_k＝h(x_k)+v_k，量测量是指由预测四元数和星敏感器得到的带误差姿态四元数二者之间的差值，其中q_predic为星敏感器输出的姿态四元数，q_g为陀螺数据转换的四元数。

2)设置SR-UKF滤波输入的参量

(状态方程、量测方程、状态初值、量测量、S0、Q、R)

3)计算权重

W_i ^(m)＝W_i ^(c)＝λ/[2(n+λ)]i＝1,…，2n

其中，λ＝α²(n+κ)-n,

为刻度参数

4)创建状态值x的sigma点

由时刻k-1的

和k时刻的S_k来计算的Sigma点集

设状态向量为n维,

为时刻k-1的状态向量估计值,P_k-1为该时刻状态向量的协方差矩阵,2n+1维的Sigma点集可以表示为:

每个sigma点可分为以下两部分

5)对状态方程进行UT变换

将上一步得到的sigma点代入非线性状态函数中，然后进行加权处理得到一步预测状态值再利用QR分解及Cholesky因式得到预测 Sk。

χ_k|k-1＝f(χ_k-1)(此步得到的预测sigma点

)

6)对量测方程进行UT变换

将sigma点代入量测函数中，然后进行加权处理得到一步预测量测值再利用QR分解及Cholesky因式得到预测Syk。

(将第5步得到的预测sigma点带入非线性量测方程。)

7)计算互协方差矩阵值

8)更新状态值及协方差矩阵值

9)四元数修正

四元数修正是通过对预测四元数和状态更新后误差四元数做叉乘实现。姿态四元数修正方程：

为k时刻误差四元数估计值，

为k时刻的四元数预测值。

参数选取：

卫星轨道参数为：

表1为卫星轨道及星敏安装数据；

表1

表2为实验数据准备；

表2

星敏数据(采样频率4hz)	陀螺数据
		无误差数据	无误差数据
误差为0.6”的星敏数据	采样频率为8hz的陀螺数据
		误差为0.8”的星敏数据	采样频率为12hz的陀螺数据
误差为1.0”的星敏数据	采样频率为16hz的陀螺数据

选取固定频率和精度的星敏(4hz，1.0”)和不同采样频率的陀螺数据进行联合定姿。

表3为两台1.0”测量精度星敏与陀螺SR-UKF组合定姿精度仿真分析统计表。

表3

根据表3和图3可以直观看出陀螺测量误差对联合定姿精度有直接影响，随着陀螺测量误差的增大，定姿中误差值也在增大，在用 SR-UKF算法滤波时，在星敏频率4hz，陀螺频率8hz的条件下，陀螺误差2.1”即可达到1.0”的定姿精度，陀螺误差1.7”可达到0.8”的定姿精度。

图4、图5、和图6分别是用SR-UKF算法时三轴姿态角在不同陀螺频率下的联合定姿精度，在相同的星敏精度和陀螺测量精度下，随着陀螺采样率从8Hz到16Hz不断提高，其组合定姿精度也相应地提高，可见较高的陀螺测量频率能明显改善组合定姿的精度。要实现陀螺与两台星敏组合定姿三轴均达到0.7”精度，星敏在1.0"测量精度时，当陀螺采用率在8Hz条件下，其测量精度要达到1.5"/s，当陀螺采用率在12Hz条件下，其测量精度要达到1.7"/s，当陀螺采用率在16Hz条件下，只需其测量精度要达到2.3"/s即可；

从图7、图8可以看出，EKF和SR-UKF的影响在姿态角误差及随机漂移误差方面，没有显著的量级上的差异。

表4为星敏(误差1.0”频率4hz)陀螺(误差0.5”频率12hz)联合定姿的随机漂移误差；

表4

表5为星敏(误差1.0”频率4hz)陀螺(误差0.5”频率16hz)联合定姿的随机漂移误差；

表5

从表4和表5的结果来看，当陀螺测量误差较低时，EKF与SR-UKF 的精度相当，SR-UKF精度略高于EKF，而当陀螺测量误差较大时， SR-UKF的定姿精度明显高于EKF，其原因在于陀螺测量误差增大，导致系统的非线性程度增大，所以，对于线性化误差较大的姿态确定系统，SR-UKF方法具有有更广的适用性。

对于表4来讲，当星敏频率4hz陀螺频率12hz时，要实现陀螺与两台星敏组合定姿三轴均达到0.8”精度，使用EKF滤波定姿算法时，要求陀螺测量误差小于1.9”/s，而使用SR-UKF滤波定姿算法时，需要陀螺测量误差小于2.1”/s，可见当要求相同定姿精度时，SR-UKF滤波算法对陀螺的测量误差要求较低，算法本身的定姿精度较高。

当固定星敏误差频率(1.0”,4hz)及固定陀螺误差频率(0.5”,8hz) 的前提下，分别使用EKF和SR-UKF进行实验，取前250组实验结果进行作图对比分析，由图9、图10可以看出EKF算法的姿态角误差波动较大，而SR-UKF算法滤波较为稳定，分析其原因，其一，EKF 算法对系统近似线性化的处理一定程度上破坏了系统高斯分布的假设，从而对滤波精度和稳定性带来了一定影响；其二，EKF算法忽略的高阶截断误差在大角度较强非线性情况下对滤波精度和稳定性都带来了较大的影响。

以上是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (2)

1.一种基于SR-UKF滤波的星敏感器和陀螺组合定姿方法，其特征在于，所述方法具体包括：

步骤1仿真出星敏感器四元数和陀螺的角速度；

步骤2以误差四元数及陀螺随机漂移误差为状态变量，利用SR-UKF算法融合处理星敏感器和陀螺的姿态信息进行滤波处理，并进行反馈，通过迭代滤波处理消除星敏感器和陀螺的误差影响，求解高精度的姿态信息；

步骤1中仿真的输入输出具体包括：

输入：

长半径；偏心率；轨道倾角；升交点赤经；近地点角距；过近地点时刻；地球引力常数；采样间隔；星敏感器噪声误差；第一星敏感器在卫星本体坐标系中的方向角；第二星敏感器在卫星本体坐标系中的方向角；

输出：

无误差姿态四元数；有误差姿态四元数；无误差陀螺角速度；有误差陀螺角速度；

步骤1具体包括：

步骤1a参数设定中设定卫星轨道参数、两个星敏感器的安装参数，采样频率；

步骤1b卫星轨道瞬时位置计算中是利用卫星的轨道参数计算卫星的位置，包括地心系与轨道坐标系之间的关系求解、卫星地心系坐标解算、卫星轨道坐标系解算；

步骤1c利用两个星敏感器的安装参数以及瞬时卫星位置信息，构建矢量信息，通过定姿计算得到无误差的卫星姿态信息；

步骤1d利用两个星敏感器的安装参数获取主光轴信息，添加随机噪声后，联合卫星的瞬时位置信息，构建矢量信息，采用QUEST定姿方法解算得到带误差的卫星姿态信息；

步骤1e利用解算得到的无误差的姿态信息，通过角速度的变换量计算，得到无误差的陀螺数据；

步骤1f利用得到的无误差陀螺数据，在三轴添加随机噪声，得到带误差的陀螺数据。

2.如权利要求1所述的基于SR-UKF滤波的星敏感器和陀螺组合定姿方法，其特征在于，所述方法中步骤2具体包括：

步骤2a参数设置

设置陀螺误差因数，将陀螺误差因数设置为从0.1到2.5，每隔0.2取一次；读入数据，包括带误差和不带误差的四元数姿态数据以及角速度姿态数据；状态变量为误差四元数和陀螺随机漂移误差，状态初值：

X₀＝[Δq_ini3×3 Δd_ini3×3]＝[0 0 0 0 0 0]

系统噪声矩阵

测量噪声协方差矩阵H＝[I_3×3 0_3×3]，其中q_rms为星敏测量中误差；

陀螺随机漂移的中误差d_rms＝10^-6[1 1 1]

初始

步骤2b设置状态方程与量测方程，状态方程如下：

x_k+1＝f(x_k)+w_k

量测方程：z_k＝h(x_k)+v_k，量测量是指由预测四元数和星敏感器得到的带误差姿态四元数二者之间的差值，q_predic为星敏感器输出的姿态四元数，q_g为陀螺数据转换的四元数；

步骤2c设置SR-UKF滤波输入的参量；

步骤2d计算权重

其中，λ＝α²(n+κ)-n,

为刻度参数

步骤2e创建状态值x的sigma点

由时刻k-1的

和k时刻的S_k来计算的Sigma点集

设状态向量为n维,

为时刻k-1的状态向量估计值,P_k-1为该时刻状态向量的协方差矩阵,2n+1维的Sigma点集可以表示为:

每个sigma点可分为以下两部分；

步骤2f对状态方程进行UT变换

将步骤2e得到的sigma点代入非线性状态函数中，然后进行加权处理得到一步预测状态值再利用QR分解及Cholesky因式得到预测Sk；

χ_k|k-1＝f(χ_k-1)

其中，预测sigma点

步骤2g对量测方程进行UT变换

将sigma点代入量测函数中，然后进行加权处理得到一步预测量测值再利用QR分解及Cholesky因式得到预测Syk；

将步骤2f得到的预测sigma点带入非线性量测方程；

步骤2h计算互协方差矩阵值

步骤2i更新状态值及协方差矩阵值

步骤2j四元数修正

四元数修正是通过对预测四元数和状态更新后误差四元数做叉乘实现；姿态四元数修正方程：

为k时刻误差四元数估计值，

为k时刻的四元数预测值。

Images