CN108225308A - 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 - Google Patents

Abstract

本发明公开了一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法，包括如下步骤：获取载体固定坐标系下多传感器数据；对加速度计和磁力计采集的数据进行滤波，并对这两个传感器采集的数据做归一化处理；根据四元数微分方程和姿态矩阵构建载体***的状态方程，并确定***的过程噪声方差矩阵；利用快速高斯‑牛顿法构建***观测模型，并确定***量测噪声方差矩阵；根据建立的***状态方程和观测模型建立卡尔曼滤波递推方程；利用递推得到的最佳四元数解算出载体的三个姿态角。本发明能够极大简化计算量，解决了现有参数计算不详的问题。

Description

一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法

技术领域

本发明涉及运动载体导航的多传感器数据融合技术领域，尤其是一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法。

背景技术

在人体运动跟踪、飞行器导航等运动载体导航的多传感器数据融合领域，对载体的姿态进行精准的实时估计有着广泛的应用。低成本的微机电***的迅速发展使得更小更便宜的惯性传感器得到了广泛的应用。但是低成本的惯性测量单元测得的数据容易受到高频噪声和时变的偏置的影响，所以传感器数据融合算法中需要进行平滑处理以及无偏置估计。为了获得更精准的姿态，常常将加速度计和磁力计的数据与陀螺仪输出的角速度数据融合。而最常用的非线性姿态估计方法所使用的技术就是互补滤波和扩展卡尔曼滤波。

互补滤波方法中的互补滤波系数在动态***中需要实时修正，但是如何修正并没有具体的模型方程，常使用的方法是采用自适应调参。黄鹤等人提出的“一种基于自适应互补融合的无人机姿态控制***及方法”提出了使用梯度下降法来获得第二姿态角的方法，但是不同的传感器的使用会导致自适应调参的方法不同，参数的估计会在实际过程中会造成很大的麻烦，甚至导致姿态估计不精确。李国朋等人提出的“一种基于互补卡尔曼滤波算法计算融合姿态角度的方法”提出了互补滤波与卡尔曼滤波结合的方法，但是建立的***模型只适用于单轴***，不适合实际的三轴陀螺仪和三轴加速度计***。陈洋等人提出的“共轭梯度与扩展卡尔曼滤波结合的四旋翼解算方法”提出了使用共轭梯度方法建立扩展卡尔曼滤波观测模型，但是***状态方程建立不够精确，没有给出过程噪声方差矩阵和测量噪声方差矩阵在实际过程中的计算方法。

发明内容

本发明所要解决的技术问题在于，提供一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法，能够极大简化计算量，解决了现有参数计算不详的问题。

为解决上述技术问题，本发明提供一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法，包括如下步骤：

(1)搭建姿态估计***，获取载体固定参考系坐标系(即b系，坐标原点位于惯性测量组件中心的“右-前-上”直角坐标系)下多轴传感器数据，陀螺仪采集的三轴角速度数据，加速度计采集的三轴加速度数据，磁力计采集的三轴磁感应强度数据；

(2)对采集的加速度数据和磁感应强度数据做滤波处理，并对这两个传感器采集的数据做归一化处理；三轴陀螺仪采集的数据为w＝[w_x w_y w_z]^T，其中w_x、w_y、w_z分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴角速度数据；T表示转置；归一化后的三轴加速度计采集的数据为a＝[a_x a_y a_z]^T，其中a_x、a_y、a_z分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴加速度数据；归一化后的三轴磁力计采集的数据为m＝[m_x m_y m_z]^T，其中m_x、m_y、m_z分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴磁感应强度数据；

(3)根据四元数微分方程和姿态矩阵构建载体***的状态方程，并得到***的过程噪声方差矩阵；

(4)利用快速高斯-牛顿法构建***的观测模型，并得到***的测量噪声方差矩阵；

(5)根据建立的***状态方程和观测模型建立扩展卡尔曼滤波递推方程；

(6)利用每次递推得到的最佳四元数解算载体的三个姿态角：航向角为Ψ，俯仰角为θ，横滚角为γ。

优选的，步骤(3)中，根据四元数微分方程和姿态矩阵构建***的状态方程，并得到***过程噪声方差矩阵的具体过程如下：

四元数微分方程如下：

式中，q_k表示k次采样时刻四元数，其中q_k＝[q_k1 q_k2 q_k3 q_k0]^T，[q_k1 q_k2 q_k3]^T是四元数q_k的矢量部分，用表示，q_k0是标量部分；表示q_k的微分；w_k表示k次采样时刻的陀螺仪采集的数据；其中Ω(w_k)的定义如下：

式中，w_k＝[w_kx w_ky w_kz]^T，其中w_kx、w_ky、w_kz分别为k次采样时刻陀螺仪在载体固定坐标系采集的x轴、y轴和z轴方向上的角速度数据；T表示转置；

根据四元数微分方程得到的离散时间模型为：

式中，q_k+1表示k+1次采样时刻四元数；Ω_k是k次采样时刻的Ω(w_k)；q_k为k次采样时刻的四元数，q(0)为初始条件得到的四元数；T_s为采样周期；

根据四元数q_k＝[q_k1 q_k2 q_k3 q_k0]^T与姿态矩阵(n系：“东-北-天”地理坐标系)的关系得到姿态矩阵如下：

以姿态四元数作为***的状态，即：

X_k＝q_k

式中，X_k表示k次采样时刻状态矩阵；q_k表示k次采样时刻四元数；

建立的***的状态方程如下式：

X_k+1＝Φ_kX_k+V_K

式中，X_k+1表示k+1次采样时刻状态矩阵；Φ_k表示k次采样时刻状态转移矩阵；X_k表示k次采样时刻四元数；V_K为k次采样时刻过程噪声阵；

Φ_k＝exp(Ω_kT_s)

式中，Ω_k为k次采样时刻的Ω(w_k)；T_s为采样周期；

式中，为高斯白噪声，其协方差矩阵为Σ_k＝||e_k||I，I为3阶单位矩阵；Ξ_k表示k次采样时刻的误差系数矩阵；

式中，为k次采样时刻实测的加速度a_k＝[a_kx a_ky a_kz]^T与预测的加速度v_k＝[v_kxv_ky v_kz]^T之间的旋转误差，a_kx、a_ky、a_kz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴加速度数据，v_kx、v_ky、v_kz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴加速度数据；为k次采样时刻实测的磁感应强度m_k＝[m_kx m_ky m_kz]^T与预测的磁感应强度h_k＝[h_kx h_ky h_kz]^T之间的旋转误差，m_kx、m_ky、m_kz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴磁感应强度数据，h_kx、h_ky、h_kz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴磁感应强度数据；加速度预测向量v_k＝[v_kx v_ky v_kz]^T和磁感应强度预测向量h_k＝[h_kx h_ky h_kz]^T分别为理想状态下导航坐标系中的地球重力矢量和地磁场强度矢量在载体坐标系中的投影，同时投影可以通过姿态转换得到；

***过程噪声方差矩阵计算公式如下：

Q_k＝(T_s/2)²Ξ_kΣ_kΞ_k ^T。

优选的，步骤(4)中，利用快速高斯-牛顿法构建***的观测模型，并得到***测量噪声方差矩阵的具体过程如下：

利用实测的加速度a_k＝[a_kx a_ky a_kz]^T减去预测的加速度v_k＝[v_kx v_ky v_kz]^T和实测的磁感应强度m_k＝[m_kx m_ky m_kz]^T减去预测的磁感应强度h_k＝[h_kx h_ky h_kz]^T得到误差函数∈(q_k)，误差函数计算公式如下：

利用快速高斯-牛顿法求出满足的四元数的最优解，快速高斯-牛顿法公式如下：

式中，表示k+1次采样时刻的最优四元数解；为k次采样时刻的一步预测四元数值；λ_k在每次迭代过程中都会改变为最优值；J(0)为对应的雅可比矩阵，雅可比矩阵J(k)计算公式如下：

选取快速高斯牛顿法求出的最优解作为观测值，即：

令得到***量测噪声方差矩阵如下：

式中，I_3×3为3阶单位矩阵。

优选的，步骤(5)中，根据建立的***状态方程和***观测模型建立如下的扩展卡尔曼滤波递推方程：

状态一步预测方程计算公式如下：

X_k,k-1＝Φ_kX_k-1

式中，X_k-1表示k-1次采样时刻状态矩阵；X_k,k-1表示k次采样时刻预测状态矩阵；

一步预测方差矩阵计算公式如下：

式中，为矩阵Φ_k的转置；Q_k-1为k-1次采样时刻的***过程噪声方差矩阵；P_k-1表示k-1次采样时刻的估计误差方差矩阵；P_k,k-1表示k次采样时刻的一步预测误差方差矩阵；

滤波增益矩阵计算公式如下：

式中，R_k表示k次采样时刻***量测噪声方差矩阵；T表示转置；H_k为4阶单位矩阵，且为k次采样时刻观测矩阵；K_k表示k次采样时刻的增益矩阵；

状态估计计算公式如下：

式中，Z_k表示k次采样时刻的由快速高斯牛顿法计算出的观测值；

估计误差方差矩阵计算公式如下：

P_k＝[I_4×4-K_kH_k]P_k,k-1

式中，I_4×4表示四阶单位矩阵；P_k表示k次采样时刻的估计误差方差矩阵。

优选的，步骤(6)中，利用每次递推得到的最佳四元数解算载体的三个姿态角：航向角为Ψ，俯仰角为θ，横滚角为γ的具体过程如下：

根据方向余弦矩阵和四元数之间的关系可得：

γ_k＝arctan[-(q_k1q_k3-q_k2q_k0)/0.5-q_k1 ²-q_k2 ²)]

Ψ_k＝[(q_k1q_k2-q_k3q_k0)/0.5-q_k1 ²-q_k3 ²)]

式中，θ_k、γ_k、Ψ_k分别表示k次采样时刻的航向角、俯仰角和横滚角。

本发明的有益效果为：(1)本发明的一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法，是一种四元数估计法，***过程方程是线性模型，具有估计精度高的优点，同时计算不复杂；(2)本发明提出了使用快速高斯-牛顿法来建立***观测模型，相对于其他的扩展卡尔曼滤波方法，它极大地简化了计算量，同时也有不错的效果；(3)本发明给出了过程噪声方差矩阵和测量噪声方差矩阵在实际过程中的计算方法，解决了现有参数计算不详等问题。

附图说明

图1为本发明的方法流程示意图。

图2为本发明采用的从AHRS设备记录的校准陀螺仪、加速度计和磁力计数据示意图。

图3为本发明通过姿态解算得到的欧拉角数据示意图。

具体实施方式

如图1所示，本发明的一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法，包括以下步骤：

步骤1：搭建姿态估计***，获取载体固定参考系坐标系下多轴传感器数据；

步骤2：对采集的加速度数据和磁感应强度数据做滤波处理，并对这两个传感器采集的数据做归一化处理；

三轴陀螺仪采集的数据为w＝[w_x w_y w_z]^T，归一化后的三轴加速度计采集的数据为a＝[a_x a_y a_z]^T，归一化后的三轴磁力计采集的数据为m＝[m_x m_y m_z]^T；

步骤3：根据四元数微分方程和姿态矩阵构建载体***的状态方程，并得到***的过程噪声方差矩阵；

步骤4：利用快速高斯-牛顿法构建***的观测模型，并得到***的测量噪声方差矩阵；

步骤5：根据建立的***状态方程和观测模型建立扩展卡尔曼滤波递推方程；

步骤6：利用每次递推得到的最佳四元数解算载体的三个姿态角：航向角为Ψ，俯仰角为θ，横滚角为γ；

上述中，构建***的状态方程，并求出***的过程噪声方差矩阵的具体过程如下：

四元数微分方程如下：

式中，q_k表示k次采样时刻四元数，其中q_k＝[q_k1 q_k2 q_k3 q_k0]^T，[q_k1 q_k2 q_k3]^T是四元数q_k的矢量部分，用表示，q_k0是标量部分；表示q_k的微分；w_k为k次采样时刻的陀螺仪采集的数据；其中Ω(w_k)的定义如下：

式中，w_k＝[w_kx w_ky w_kz]^T，其中w_kx、w_ky、w_kz分别为k次采样时刻陀螺仪采集的x轴、y轴和z轴方向上的角速度数据；T表示转置；

根据四元数微分方程得到的离散化时间模型如下：

式中，q_k+1表示k+1次采样时刻四元数；Ω_k是k次采样时刻的Ω(w_k)；q_k为k次采样时刻的四元数，q(0)为初始条件得到的四元数；T_s为采样周期；

通过四元数q_k＝[q_k1 q_k2 q_k3 q_k0]^T可以得到姿态矩阵n系(导航坐标系)：“东-北-天”地理坐标系；

以姿态四元数作为***的状态，即：

X_k＝q_k

式中，X_k表示k次采样时刻状态矩阵；q_k表示k次采样时刻四元数；

建立***的状态方程如下式

X_k+1＝Φ_kX_k+V_K

式中，X_k+1表示k+1次采样时刻状态矩阵；Φ_k表示k次采样时刻状态转移矩阵；X_k表示k次采样时刻四元数；V_K为k次采样时刻过程噪声阵；

Φ_k＝exp(Ω_kT_s)

式中，Ω_k为k次采样时刻的Ω(w_k)；T_s为采样周期；

式中，为高斯白噪声，其协方差矩阵为Σ_k＝||e_k||I，I为3阶单位矩阵；Ξ_k表示k次采样时刻的误差系数矩阵；

式中，为k次采样时刻实测的加速度向量a_k＝[a_kx a_ky a_kz]^T与加速度预测向量v_k＝[v_kx v_ky v_kz]^T之间的旋转误差，a_kx、a_ky、a_kz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴加速度数据，v_kx、v_ky、v_kz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴加速度数据；为k次采样时刻实测的磁感应强度向量m_k＝[m_kxm_ky m_kz]^T与磁感应强度预测向量h_k＝[h_kx h_ky h_kz]^T之间的旋转误差，m_kx、m_ky、m_kz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴磁感应强度数据，h_kx、h_ky、h_kz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴磁感应强度数据；

加速度预测向量v_k＝[v_kx v_ky v_kz]^T和磁感应强度预测向量h_k＝[h_kx h_ky h_kz]^T分别为理想状态下导航坐标系中的地球重力矢量和地磁场强度矢量在载体坐标系中的投影，投影过程通过姿态转换得到，计算公式如下：

b_kz＝d_kz

式中，式中[d_kx d_ky d_kz]^T是磁力计在n系的输出；[b_kx 0 b_kz]^T是假设的实际地球磁场的大小和方向；是的转置；

***过程噪声方差矩阵计算公式如下：

Q_k＝(T_s/2)²Ξ_kΣ_kΞ_k ^T。

上述技术方案中，步骤4得到观测值并求出***的测量噪声方差矩阵的具体过程如下：利用实测的加速度a_k＝[a_kx a_ky a_kz]^T减去预测的加速度v_k＝[v_kx v_ky v_kz]^T和实测的磁感应强度m_k＝[m_kx m_ky m_kz]^T减去预测的磁感应强度h_k＝[h_kx h_ky h_kz]^T得到误差函数∈(q_k)，误差函数计算公式如下：

利用快速高斯-牛顿法求出满足的四元数 的最优解，快速高斯-牛顿法公式如下：

式中，表示k+1次采样时刻的最优四元数解；为k次采样时刻的一步预测四元数值；λ_k在每次迭代过程中都会改变为最优值；J(0)为对应的雅可比矩阵，雅可比矩阵J(k)计算公式如下：

选用快速高斯牛顿法求出的最优解作为观测值，即

令得到***量测噪声方差矩阵如下：

式中，I_3×3为3阶单位矩阵；

上述技术方案中，步骤5得到的扩展卡尔曼滤波递推方程的具体过程如下：状态一步预测方程计算公式如下：

X_k,k-1＝Φ_kX_k-1

式中，X_k-1表示k-1次采样时刻状态矩阵；X_k,k-1表示k次采样时刻预测状态矩阵；

一步预测方差矩阵计算公式如下：

式中，为矩阵Φ_k的转置，Q_k-1为k-1次采样时刻的***过程噪声方差矩阵；P_k-1表示k-1次采样时刻的估计误差方差矩阵；P_k,k-1表示k次采样时刻的一步预测误差方差矩阵；

滤波增益矩阵计算公式如下：

式中，R_k表示k次采样时刻***量测噪声方差矩阵；T表示转置；H_k为4阶单位矩阵，且为k次采样时刻观测矩阵；K_k表示k次采样时刻的增益矩阵；

状态估计计算公式如下：

式中，Z_k表示k次采样时刻的由快速高斯牛顿法计算出的观测值；

估计误差方差矩阵计算公式如下：

P_k＝[I_4×4-K_kH_k]P_k,k-1

式中，I_4×4表示四阶单位矩阵；P_k表示k次采样时刻的估计误差方差矩阵；

上述技术方案中，步骤6利用每次递推得到的最佳四元数解算载体的三个姿态角：航向角为Ψ，俯仰角为θ，横滚角为γ的具体过程如下：

根据方向余弦矩阵和四元数之间的关系可得：

γ_k＝arctan[-(q_k1q_k3-q_k2q_k0)/(0.5-q_k1 ²-q_k2 ²)]

Ψ_k＝[(q_k1q_k2-q_k3q_k0)/（0.5-q_k1 ²-q_k3 ²)]

式中，θ_k、γ_k、Ψ_k分别表示k次采样时刻的航向角、俯仰角和横滚角。

尽管本发明就优选实施方式进行了示意和描述，但本领域的技术人员应当理解，只要不超出本发明的权利要求所限定的范围，可以对本发明进行各种变化和修改。

Claims (5)

1.一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法，其特征在于，包括如下步骤：

(1)搭建姿态估计***，获取载体固定参考系坐标系下多轴传感器数据，即b系，坐标原点位于惯性测量组件中心的“右-前-上”直角坐标系，陀螺仪采集的三轴角速度数据，加速度计采集的三轴加速度数据，磁力计采集的三轴磁感应强度数据；

(2)对采集的加速度数据和磁感应强度数据做滤波处理，并对这两个传感器采集的数据做归一化处理；三轴陀螺仪采集的数据为w＝[w_x w_y w_z]^T，其中w_x、w_y、w_z分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴角速度数据；T表示转置；归一化后的三轴加速度计采集的数据为a＝[a_x a_y a_z]^T，其中a_x、a_y、a_z分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴加速度数据；归一化后的三轴磁力计采集的数据为m＝[m_x m_y m_z]^T，其中m_x、m_y、m_z分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴磁感应强度数据；

(3)根据四元数微分方程和姿态矩阵构建载体***的状态方程，并得到***的过程噪声方差矩阵；

(4)利用快速高斯-牛顿法构建***的观测模型，并得到***的测量噪声方差矩阵；

(5)根据建立的***状态方程和观测模型建立扩展卡尔曼滤波递推方程；

(6)利用每次递推得到的最佳四元数解算载体的三个姿态角：航向角为Ψ，俯仰角为θ，横滚角为γ。

2.如权利要求1所述的基于四元数的扩展卡尔曼滤波算法的姿态解算方法，其特征在于，步骤(3)中，根据四元数微分方程和姿态矩阵构建***的状态方程，并得到***过程噪声方差矩阵的具体过程如下：

四元数微分方程如下：

式中，q_k表示k次采样时刻四元数，其中q_k＝[q_k1 q_k2 q_k3 q_k0]^T，[q_k1 q_k2 q_k3]^T是四元数q_k的矢量部分，用表示，q_k0是标量部分；表示q_k的微分；w_k表示k次采样时刻的陀螺仪采集的数据；其中Ω(w_k)的定义如下：

式中，w_k＝[w_kx w_ky w_kx]^T，其中w_kx、w_ky、w_kz分别为k次采样时刻陀螺仪在载体固定坐标系采集的x轴、y轴和z轴方向上的角速度数据；T表示转置；

根据四元数微分方程得到的离散时间模型为：

式中，q_k+1表示k+1次采样时刻四元数；Ω_k是k次采样时刻的Ω(w_k)；q_k为k次采样时刻的四元数，q(0)为初始条件得到的四元数；T_s为采样周期；

根据四元数q_k＝[q_k1 q_k2 q_k3 q_k0]^T与姿态矩阵(n系：“东-北-天”地理坐标系)的关系得到姿态矩阵如下：

以姿态四元数作为***的状态，即：

X_k＝q_k

式中，X_k表示k次采样时刻状态矩阵；q_k表示k次采样时刻四元数；

建立的***的状态方程如下式：

X_k+1＝Φ_kX_k+V_K

式中，X_k+1表示k+1次采样时刻状态矩阵；Φ_k表示k次采样时刻状态转移矩阵；X_k表示k次采样时刻四元数；V_K为k次采样时刻过程噪声阵；

Φ_k＝exp(Ω_kT_s)

式中，Ω_k为k次采样时刻的Ω(w_k)；T_s为采样周期；

式中，为高斯白噪声，其协方差矩阵为Σ_k＝||e_k||I，I为3阶单位矩阵；Ξ_k表示k次采样时刻的误差系数矩阵；

式中，为k次采样时刻实测的加速度a_k＝[a_kx a_ky a_kz]^T与预测的加速度v_k＝[v_kx v_kyv_kz]^T之间的旋转误差，a_kx、a_ky、a_kz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴加速度数据，v_kx、v_ky、v_kz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴加速度数据；为k次采样时刻实测的磁感应强度m_k＝[m_kx m_ky m_kz]^T与预测的磁感应强度h_k＝[h_kx h_ky h_kz]^T之间的旋转误差，m_kx、m_ky、m_kz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴磁感应强度数据，h_kx、h_ky、h_kz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴磁感应强度数据；加速度预测向量v_k＝[v_kxv_ky v_kz]^T和磁感应强度预测向量h_k＝[h_kx h_ky h_kz]^T分别为理想状态下导航坐标系中的地球重力矢量和地磁场强度矢量在载体坐标系中的投影，同时投影可以通过姿态转换得到；

***过程噪声方差矩阵计算公式如下：

Q_k＝(T_s/2)²Ξ_kΣ_kΞ_k ^T。

3.如权利要求1所述的基于四元数的扩展卡尔曼滤波算法的姿态解算方法，其特征在于，步骤(4)中，利用快速高斯-牛顿法构建***的观测模型，并得到***测量噪声方差矩阵的具体过程如下：

利用实测的加速度a_k＝[a_kx a_ky a_kz]^T减去预测的加速度v_k＝[v_kx v_ky v_kz]^T和实测的磁感应强度m_k＝[m_kx m_ky m_kz]^T减去预测的磁感应强度h_k＝[h_kx h_ky h_kz]^T得到误差函数∈(q_k)，误差函数计算公式如下：

利用快速高斯-牛顿法求出满足的四元数的最优解，快速高斯-牛顿法公式如下：

式中，表示k+1次采样时刻的最优四元数解；为k次采样时刻的一步预测四元数值；λ_k在每次迭代过程中都会改变为最优值；J(0)为对应的雅可比矩阵，雅可比矩阵J(k)计算公式如下：

选取快速高斯牛顿法求出的最优解作为观测值，即：

令得到***量测噪声方差矩阵如下：

式中，I_3×3为3阶单位矩阵。

4.如权利要求1所述的基于四元数的扩展卡尔曼滤波算法的姿态解算方法，其特征在于，步骤(5)中，根据建立的***状态方程和***观测模型建立如下的扩展卡尔曼滤波递推方程：

状态一步预测方程计算公式如下：

X_k,k-1＝Φ_kX_k-1

式中，X_k-1表示k-1次采样时刻状态矩阵；X_k,k-1表示k次采样时刻预测状态矩阵；

一步预测方差矩阵计算公式如下：

式中，为矩阵Φ_k的转置；Q_k-1为k-1次采样时刻的***过程噪声方差矩阵；P_k-1表示k-1次采样时刻的估计误差方差矩阵；P_k,k-1表示k次采样时刻的一步预测误差方差矩阵；

滤波增益矩阵计算公式如下：

式中，R_k表示k次采样时刻***量测噪声方差矩阵；T表示转置；H_k为4阶单位矩阵，且为k次采样时刻观测矩阵；K_k表示k次采样时刻的增益矩阵；

状态估计计算公式如下：

式中，Z_k表示k次采样时刻的由快速高斯牛顿法计算出的观测值；

估计误差方差矩阵计算公式如下：

P_k＝[I_4×4-K_kH_k]P_k,k-1

式中，I_4×4表示四阶单位矩阵；P_k表示k次采样时刻的估计误差方差矩阵。

5.如权利要求1所述的基于四元数的扩展卡尔曼滤波算法的姿态解算方法，其特征在于，步骤(6)中，利用每次递推得到的最佳四元数解算载体的三个姿态角：航向角为Ψ，俯仰角为θ，横滚角为γ的具体过程如下：

根据方向余弦矩阵和四元数之间的关系可得：

γ_k＝arctan[-(q_k1q_k3-q_k2q_k0)/(0.5-q_k1 ²-q_k2 ²)]

Ψ_k＝[(q_k1q_k2-q_k3q_k0)/(0.5-q_k1 ²-q_k3 ²)]

式中，θ_k、γ_k、Ψ_k分别表示k次采样时刻的航向角、俯仰角和横滚角。