CN105066996B - 自适应矩阵卡尔曼滤波姿态估计方法 - Google Patents

Abstract

本发明公开一种自适应矩阵卡尔曼滤波姿态估计方法，包括以下几个步骤：步骤1：获取传感器实时数据；步骤2：建立系统过程方程与量测方程；步骤3：在建立的系统模型的基础上，利用矩阵卡尔曼滤波估计最优K矩阵；步骤4：建立基于残差匹配的AMKF滤波方程；步骤5：利用姿态四元数中乘性误差四元数计算方法，得到估计姿态与真实姿态之间的误差角，采用矩阵欧几里德范数运算表示滤波增益因子及矩阵求迹运算计算过程噪声；步骤6：姿态估计离散系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺常值漂移结果，完成姿态估计，若k＜M，表示滤波过程未完成，则重复上述步骤三至步骤五，直至姿态估计系统运行结束。

Description

自适应矩阵卡尔曼滤波姿态估计方法

技术领域

本发明涉及一种自适应矩阵卡尔曼滤波姿态估计方法,为一种基于Wahba问题的最优K矩阵姿态估计方法，采用自适应方法解决量测噪声不确定时姿态估计问题,属于载体姿态估计领域.

背景技术

载体姿态估计技术是导航技术领域中的关键技术，采用陀螺仪与向量观测器(星敏感器、磁强计、水平测量仪等)组成的姿态估计系统具有测姿精度高、可靠性好、自主性强等优点。而采用向量观测进行姿态估计时，主要有两种方法：一种是采用最小二乘技术的姿态估计方法，该方法主要基于Wahba损失函数，利用观测向量与参考向量之间的关系建立矩阵，对矩阵进行结算得到载体姿态四元数；另一种方法是基于非线性滤波理论的四元数估计法。而采用矩阵的姿态估计方法具有系统模型线性、计算量小、估计精度高等优点。众多学者在此基础上提出了多种姿态估计方法，其中D.Choukroun等人研究的基于矩阵卡尔曼滤波(matrix Kalman filter,MKF)算法的最优矩阵方法具有鲁棒性强，计算精度高等优点，但是四元数矩阵卡尔曼滤波的递推过程中对系统模型进行了多次近似处理，而且是基于系统量测噪声准确已知的条件下进行的，但在实际过程中，量测噪声往往随实际系统差异存在一定的误差，使得算法估计精度下降，扰动噪声变大。

为克服MKF算法在量测噪声统计不准确时估计精度下降的问题，采用一种基于残差匹配技术的自适应矩阵卡尔曼滤波算法(Adaptive matrix Kalman filter,AMKF)。在算法迭代过程中对量测噪声进行实时估计，将实时计算的量测噪声反馈给MKF算法，提高系统更新过程中残差协方差的计算精度，从而提高系统估计精度。

发明内容

发明目的：本发明的目的在于为了提高姿态估计精度和滤波算法的自适应性，提出了一种自适应矩阵卡尔曼滤波算法(AMKF)。发明采用MKF算法估计载体姿态四元数，采用基于残差匹配技术的自适应方法对量测噪声进行实时估计，提高系统的估计精度.

技术方案：本发明所述的自适应矩阵卡尔曼滤波姿态估计方法，包括以下步骤：

步骤1：获取传感器实时数据，所述传感器实时数据包括陀螺仪数据和向量观测器数据；

步骤2：建立系统过程方程与量测方程；

所述的姿态估计系统模型包括：

(1)系统过程方程

式中：W_k表示k时刻过程噪声阵；Φ_k表示k时刻状态转移矩阵；X_k表示k时刻状态矩阵；X_k+1表示k+1时刻状态矩阵；T表示对矩阵求转置；

W_k＝(X_kε_k-ε_kX_k)Δt

式中，ε_k表示k时刻由陀螺噪声向量构造的斜对称矩阵；Δt表示采样时间间隔；Ω(·)表示由ω_k构成的斜对称矩阵；||·||表示对向量取模值；I₄表示4维单位阵；ω_k表示k时刻载体旋转角速率；其中η_v,k为高斯白噪声，其期望与方差满足下式：

E(η_v,k)＝0

式中，I₃表示3维单位阵；表示陀螺单轴噪声方差值；Δt表示采样时间间隔；E(·)表示取数学期望；

(2)系统量测方程

Y_k+1＝X_k+1+V_k+1

式中：V_k+1表示k+1时刻量测噪声；X_k+1表示k+1时刻状态矩阵；Y_k+1表示k+1时刻系统量测；其中量测矩阵表示为：

式中，b_k+1,i表示时刻第i个量测向量；r_k+1,i表示k+1时刻第i个参考向量；tr(·)表示对矩阵取迹操k+1作；S为由b_k+1,i和r_k+1,i构成的3维矩阵；z表示由b_k+1,i和r_k+1,i构成的3维向量；σ表示矩阵B的迹；B表示由b_k+1,i和r_k+1,i构成的3维矩阵；

其中，V_k+1,i表示k+1时刻第i个量测噪声矩阵；a_i表示第i个量测噪声权值；N(k+1)表示k+1时刻有效向量观测器个数；

式中，δb_k+1,i表示k+1时刻第i个观测噪声；r_k+1,i表示k+1时刻第i个参考向量；tr(·)表示对矩阵取迹操作；S_b,i为由δb_k+1,i和r_k+1,i构成的3维矩阵；z_b,i表示由δb_k+1,i和r_k+1,i构成的3维向量；κ_b,i表示矩阵B_b,i的迹；B_b,i表示由δb_k+1,i和r_k+1,i构成的3维矩阵；

步骤3：针对上述系统模型，利用矩阵卡尔曼滤波方法估计姿态K矩阵；

步骤3.1初始条件；

式中，表示初始时刻最优状态估计；P_0|0表示初始时刻状态误差协方差矩阵；Y₀表示初始时刻量测矩阵；R₀表示初始时刻量测噪声方差阵；

步骤3.2时间更新；

其中，Φ_k表示k时刻状态转移矩阵；表示k时刻最优状态估计；表示k+1时刻状态一步预测；P_k|k表示k时刻状态误差协方差矩阵；P_k+1|k表示k+1时刻状态误差协方差矩阵一步预测；Q_k表示系统噪声方差阵；表示克罗内克积；表示由状态转移矩阵构成的传递矩阵；

Q_k＝cov[W_k]＝cov[vec(W_k)]

式中，vec(·)表示对矩阵做向量转换；cov[·]表示协方差运算；表示k时刻最优状态估计；Υ_i表示第i个中间变换矩阵；e_i表示3维单位矩阵的第i列，I₃＝[e₁ e₂ e₃]；I₄表示4维单位阵；

步骤3.3量测更新；

S_k+1＝P_k+1|k+R_k+1

式中，为表示k+1时刻状态一步预测；Y_k+1表示k+1时刻系统量测；表示k+1时刻系统新息；P_k+1|k表示k+1时刻状态误差协方差矩阵一步预测；R_k+1表示k+1时刻量测噪声方差阵；S_k+1表示k+1时刻残差协方差矩阵；G_k+1表示k+1时刻滤波增益矩阵；表示k+1时刻最优状态估计；其中和E^lj满足下式定义：

其中，χ_lj表示E^lj中元素；量测噪声方差阵R_k+1表示为：

R_k+1＝cov[V_k]＝cov[vec(V_k)]

M＝[-[e₁×] -e₁ -[e₂×] -e₂ -[e₃×] -e₃ I₃ 0]

式中，R_i表示第i个量测噪声；a_k+1,i表示k+1时刻第i个权值；Λ_i表示中间变量矩阵；b_k+1,i表示k+1时刻第i个有效观测向量；为向量观测器k+1时刻第i个量测噪声方差值；r_k+1,i表示k+1时刻第i个有效参考向量；表示由参考向量r_k+1,i构成的斜对称矩阵；M表示由单位阵列向量构成的参数矩阵；

步骤4：基于残差匹配的自适应矩阵卡尔曼滤波算法(AMKF)；

由K矩阵的定义，采用如下公式定义残差：

式中，υ_k+1表示k+1时刻残差项；表示k+1时刻新息项；表示k+1时刻一步预测四元数；V_k+1表示k+1时刻量测噪声阵；

新息匹配公式：

根据对残差求解数学期望可知：

假设每次量测噪声都具有相同的误差类型，则上式可简化为：

式中，表示k+1时刻真实四元数，e_k+1表示四元数矢量部分，表示四元数标量部分；Θ_k+1,i表示k+1时刻由真实四元数与第i个参考向量r_k+1,i构成的参数矩阵；表示k+1时刻量测噪声方差值；N(k+1)表示k+1时刻有效观测向量数；[×]表示向量叉乘矩阵；

依据上式，采用四元数一步预测作为真实四元数的最优估计，可以得到基于残差匹配的量测噪声估计：

式中，表示k+1时刻的一步预测四元数；表示k+1时刻由一步预测四元数与第i个参考向量r_k+1,i构成的参数矩阵；b_k+1,i表示k+1时刻第i个量测向量；Y_k+1表示k+1时刻系统量测；表示k+1时刻量测噪声方差值的估计；

步骤5：根据步骤4计算得到的最优状态量，计算最优四元数，利用乘性误差四元数，得出误差角的大小：

步骤5.1最优四元数的提取；

根据步骤3和步骤4交互式滤波得到最优K矩阵，根据Wahba问题可知：

通过上式，再根据四元数规范化约束，采用拉格朗日乘子，可以得到K矩阵最大特征值对应的特征向量就是最优四元数，计算方法如下式：

式中，与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计。

步骤5.2估计载体坐标系与真实载体坐标系旋转误差角计算；

根据乘性误差四元数，在评估估计姿态角与真实姿态角之间的误差时，可以采用估计载体坐标系与真实载体坐标系之间的旋转角来表示，计算如下：

式中：δq_k|k表示k时刻旋转误差四元数；表示k时刻最优估计四元数的逆；ο表示四元数乘法；表示k时刻旋转误差四元数标量；δφ_k|k表示k时刻估计载体坐标系与真实载体坐标系之间的旋转误差角；q_k表示k时刻真实旋转四元数；

步骤5.3自适应矩阵卡尔曼滤波增益因子及过程噪声；

由于矩阵卡尔曼滤波采用增益矩阵的形式更新最优估计，此处采用对增益矩阵求欧几里德范数作为滤波增益因子：

ρ＝||G_k+1||_E

式中，G_k+1表示k+1时刻滤波增益矩阵；||·||_E表示取欧几里德范数；ρ表示滤波增益因子；

过程噪声计算同样采用上述运算：

式中，Q_k表示系统噪声方差阵；表示过程噪声方差；

步骤6：姿态估计非线性离散系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺常值漂移结果，完成姿态估计，若k＜M，表示滤波过程未完成，则重复上述步骤3至步骤5，直至姿态估计系统运行结束。

进一步地,步骤2中，陀螺量测噪声标准差为输出频率为10Hz；向量观测器测量标准差为μ＝1°，输出频率为1Hz；设定载体坐标系与参考坐标系固连，此时w＝[0 0 0]^Tdeg/s。

进一步地,步骤2中，陀螺量测噪声标准差为输出频率为步骤3和步骤4中，自适应滤波参数均选取为μ＝0.01°，均在k≥2000开始使用自适应滤波估计载体姿态角。

进一步地,步骤2中，陀螺量测噪声标准差为输出频率为其特征在于，步骤6中，M＝5400。

本发明与现有技术相比，其有益效果是：(1)本发明采用矩阵卡尔曼滤波算法估计姿态矩阵，具有估计精度高的优点，同时采用线性模型，具有计算简便的优点；

(2)本发明采用基于残差的自适应滤波，能够在算法递推过程中实时估计量测噪声，提高了系统的实时性和估计精度；

(3)本发明采用归一化的四元数构造系统残差，将量测与预测同时作为残差的输入信息，并得到标量残差，减小了残差匹配过程中的计算量，提高了估计的实时性。

附图说明

图1是自适应矩阵卡尔曼滤波算法流程图；

图2是自适应矩阵卡尔曼滤波姿态估计旋转误差角曲线图；

图3是自适应矩阵卡尔曼滤波增益因子曲线图；

图4是本发明实施例1的估计量测噪声曲线图；

图5是本发明实施例1的估计过程噪声曲线图.

具体实施方式

下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。

实施例1：

本实施例的技术方案具体如下：

一种自适应矩阵卡尔曼滤波姿态估计方法，包括以下几个步骤：

步骤1：获取传感器实时数据；

步骤2：建立系统过程方程与量测方程；

步骤3：在建立的系统模型的基础上，利用矩阵卡尔曼滤波估计最优K矩阵；

步骤4：建立基于残差匹配的AMKF滤波方程；

步骤5：利用姿态四元数中乘性误差四元数计算方法，得到估计姿态与真实姿态之间的误差角，采用矩阵欧几里德范数运算表示滤波增益因子及矩阵求迹运算计算过程噪声；

步骤6：姿态估计离散系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺常值漂移结果，完成姿态估计，若k＜M，表示滤波过程未完成，则重复上述步骤三至步骤五，直至姿态估计系统运行结束。

本实施例将本发明提出的一种自适应矩阵卡尔曼滤波姿态估计方法通过Matlab仿真软件进行仿真实验效果，从而证明考虑输入滤波量测噪声不准确时，自适应滤波的优势。仿真硬件环境均为Intel(R)Core(TM)T9600CPU 2.80GHz，4G RAM，Windows 7操作系统。如图2和图3所示，为自适应矩阵卡尔曼滤波估计旋转误差角与增益因子曲线图，系统真实量测噪声为μ＝1°，滤波算法输入时μ＝0.01°，由于输入滤波算法量测噪声较小，导致估计结果更加倾向于采用量测信息，使得滤波估计噪声较大，增益因子也较大。而在仿真时间M＝2000之后，采用AMKF算法进行姿态估计，提高了量测噪声的精度，旋转误差角噪声减小，同时增益因子也减小，表明此时估计精度提高，与滤波算法理论推导一致；如图4和图5所示，表明估计量测噪声与过程噪声曲线图，采用自适应滤波之后，量测噪声量级与真实滤波模型噪声相当，而由于过程噪声与最优状态相关，所以在采用自适应滤波之后，过程噪声也得到抑制，表明姿态估计精度提高。

本发明是一种自适应矩阵卡尔曼滤波姿态估计方法，算法流程如图1所示，包括以下几个步骤：

步骤1：获取传感器实时数据，所述传感器实时数据包括陀螺仪数据和向量观测器数据；

步骤2：建立系统过程方程与量测方程；

所述的姿态估计系统模型包括：

(1)系统过程方程

式中：W_k表示k时刻过程噪声阵；Φ_k表示k时刻状态转移矩阵；X_k表示k时刻状态矩阵；X_k+1表示k+1时刻状态矩阵；T表示对矩阵求转置；

W_k＝(X_kε_k-ε_kX_k)Δt

式中，η_v,k表示k时刻陀螺噪声向量；ε_k表示k时刻由陀螺噪声向量构造的斜对称矩阵；Δt表示采样时间间隔；Ω(·)表示由ω_k构成的斜对称矩阵；||·||表示对向量取模值；I₄表示4维单位阵；ω_k表示k时刻载体旋转角速率；其中η_v,k为高斯白噪声，其期望与方差满足下式：

E(η_v,k)＝0

式中，I₃表示3维单位阵；表示陀螺单轴噪声方差值；Δt表示采样时间间隔；E(·)表示取数学期望；

(2)系统量测方程

Y_k+1＝X_k+1+V_k+1

式中：V_k+1表示k+1时刻量测噪声；X_k+1表示k+1时刻状态矩阵；Y_k+1表示k+1时刻系统量测；其中量测矩阵表示为：

式中，b_k+1,i表示k+1时刻第i个量测向量；r_k+1,i表示k+1时刻第i个参考向量；tr(·)表示对矩阵取迹操作；S为由b_k+1,i和r_k+1,i构成的3维矩阵；z表示由b_k+1,i和r_k+1,i构成的3维向量；σ表示矩阵B的迹；B表示由b_k+1,i和r_k+1,i构成的3维矩阵；

其中，V_k+1,i表示k+1时刻第i个量测噪声矩阵；a_i表示第i个量测噪声权值；N(k+1)表示k+1时刻有效向量观测器个数；

式中，δb_k+1,i表示k+1时刻第i个观测噪声；r_k+1,i表示k+1时刻第i个参考向量；tr(·)表示对矩阵取迹操作；S_b,i为由δb_k+1,i和r_k+1,i构成的3维矩阵；z_b,i表示由δb_k+1,i和r_k+1,i构成的3维向量；κ_b,i表示矩阵B_b,i的迹；B_b,i表示由δb_k+1,i和r_k+1,i构成的3维矩阵；

步骤3：针对上述系统模型，利用矩阵卡尔曼滤波方法估计姿态K矩阵；

步骤3.1初始条件；

式中，表示初始时刻最优状态估计；P_0|0表示初始时刻状态误差协方差矩阵；Y₀表示初始时刻量测矩阵；R₀表示初始时刻量测噪声方差阵；

步骤3.2时间更新；

其中，Φ_k表示k时刻状态转移矩阵；表示k时刻最优状态估计；表示k+1时刻状态一步预测；P_k|k表示k时刻状态误差协方差矩阵；P_k+1|k表示k+1时刻状态误差协方差矩阵一步预测；Q_k表示系统噪声方差阵；表示克罗内克积；表示由状态转移矩阵构成的传递矩阵；

Q_k＝cov[W_k]＝cov[vec(W_k)]

式中，vec(·)表示对矩阵做向量转换；cov[·]表示协方差运算；表示k时刻最优状态估计；Υ_i表示第i个中间变换矩阵；e_i表示3维单位矩阵的第i列，I₃＝[e₁ e₂ e₃]；I₄表示4维单位阵；

步骤3.3量测更新；

S_k+1＝P_k+1|k+R_k+1

式中，为表示k+1时刻状态一步预测；Y_k+1表示k+1时刻系统量测；表示k+1时刻系统新息；P_k+1|k表示k+1时刻状态误差协方差矩阵一步预测；R_k+1表示k+1时刻量测噪声方差阵；S_k+1表示k+1时刻残差协方差矩阵；G_k+1表示k+1时刻系统互协方差矩阵；表示k+1时刻最优状态估计；其中和E^lj满足下式定义：

其中，χ_lj表示E^lj中元素；量测噪声方差阵R_k+1表示为：

R_k+1＝cov[V_k]＝cov[vec(V_k)]

M＝[-[e₁×] -e₁ -[e₂×] -e₂ -[e₃×] -e₃ I₃ 0]

式中，R_i表示第i个量测噪声；a_k+1,i表示k+1时刻第i个权值；Λ_i表示中间变量矩阵；b_k+1,i表示k+1时刻第i个有效观测向量；为向量观测器k+1时刻第i个量测噪声方差值；r_k+1,i表示k+1时刻第i个有效参考向量；表示由参考向量r_k+1,i构成的斜对称矩阵；M表示由单位阵列向量构成的参数矩阵；

步骤4：基于残差匹配的自适应矩阵卡尔曼滤波算法(AMKF)；

由K矩阵的定义，采用如下公式定义残差：

式中，υ_k+1表示k+1时刻残差项；表示k+1时刻新息项；表示k+1时刻一步预测四元数；V_k+1表示k+1时刻量测噪声阵；

新息匹配公式：

根据对残差求解数学期望可知：

假设每次量测噪声都具有相同的误差类型，则上式可简化为：

式中，表示k+1时刻真实四元数，e_k+1表示四元数矢量部分，表示四元数标量部分；Θ_k+1,i表示k+1时刻由真实四元数与第i个参考向量r_k+1,i构成的参数矩阵；表示k+1时刻量测噪声方差值；N(k+1)表示k+1时刻有效观测向量数；[×]表示向量叉乘矩阵；

依据上式，采用四元数一步预测作为真实四元数的最优估计，可以得到基于残差匹配的量测噪声估计：

式中，表示k+1时刻的一步预测四元数；表示k+1时刻由一步预测四元数与第i个参考向量r_k+1,i构成的参数矩阵；b_k+1,i表示k+1时刻第i个量测向量；Y_k+1表示k+1时刻系统量测；表示k+1时刻量测噪声方差值的估计；

步骤5：根据步骤4计算得到的最优状态量，计算最优四元数，利用乘性误差四元数，得出误差角的大小：

步骤5.1最优四元数的提取；

根据步骤3和步骤4交互式滤波得到最优K矩阵，根据Wahba问题可知：

通过上式，再根据四元数规范化约束，采用拉格朗日乘子，可以得到K矩阵最大特征值对应的特征向量就是最优四元数，计算方法如下式：

式中，与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计。

步骤5.2估计载体坐标系与真实载体坐标系旋转误差角计算；

根据乘性误差四元数，在评估估计姿态角与真实姿态角之间的误差时，可以采用估计载体坐标系与真实载体坐标系之间的旋转角来表示，计算如下：

式中：δq_k|k表示k时刻旋转误差四元数；表示k时刻最优估计四元数的逆；表示四元数乘法；表示k时刻旋转误差四元数标量；δφ_k|k表示k时刻估计载体坐标系与真实载体坐标系之间的旋转误差角；q_k表示k时刻真实旋转四元数；

步骤5.3自适应矩阵卡尔曼滤波增益因子及过程噪声；

由于矩阵卡尔曼滤波采用增益矩阵的形式更新最优估计，此处采用对增益矩阵求欧几里德范数作为滤波增益因子：

ρ＝||G_k+1||_E

式中，G_k+1表示k+1时刻滤波增益矩阵；||·||_E表示取欧几里德范数；ρ表示滤波增益因子；

过程噪声计算同样采用上述运算：

式中，Q_k表示系统噪声方差阵；表示过程噪声方差；

对本发明的有益效果说明如下：

MATLAB仿真实验，在以下的仿真条件下，对该方法进行仿真实验：

陀螺量测噪声标准差为输出频率为10Hz；向量观测器测量标准差为μ＝1°，输出频率为1Hz；设定载体坐标系与参考坐标系固连，此时w＝[0 0 0]^Tdeg/s。适应滤波参数均选取为μ＝0.01°，在k≥2000开始使用自适应滤波估计载体姿态角。步骤6中，M＝5400。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (4)

1.一种自适应矩阵卡尔曼滤波姿态估计方法，其特征在于，包括以下步骤：

步骤1：获取传感器实时数据，所述传感器实时数据包括陀螺仪数据和向量观测器数据；

步骤2：建立系统过程方程与量测方程；

姿态估计系统模型包括：

(1)系统过程方程

式中：W_k表示k时刻过程噪声阵；Φ_k表示k时刻状态转移矩阵；X_k表示k时刻状态矩阵；X_k+1表示k+1时刻状态矩阵；T表示对矩阵求转置；

W_k＝(X_kε_k-ε_kX_k)Δt

式中，ε_k表示k时刻由陀螺噪声向量构造的斜对称矩阵；Δt表示采样时间间隔；Ω(·)表示由ω_k构成的斜对称矩阵；||·||表示对向量取模值；I₄表示4维单位阵；ω_k表示k时刻载体旋转角速率；其中η_v，k为高斯白噪声，其期望与方差满足下式：

E(η_v，k)＝0

式中，I₃表示3维单位阵；表示陀螺单轴噪声方差值；Δt表示采样时间间隔；E(·)表示取数学期望；

(2)系统量测方程

Y_k+1＝X_k+1+V_k+1

式中：V_k+1表示k+1时刻量测噪声；X_k+1表示k+1时刻状态矩阵；Y_k+1表示k+1时刻系统量测；其中量测矩阵表示为：

式中，b_k+1，i表示时刻第i个量测向量；r_k+1，i表示k+1时刻第i个参考向量；tr(·)表示对矩阵取迹操作；S为由b_k+1，i和r_k+1，i构成的3维矩阵；z表示由b_k+1，i和r_k+1，i构成的3维向量；σ表示矩阵B的迹；B表示由b_k+1，i和r_k+1，i构成的3维矩阵；

其中，V_k+1，i表示k+1时刻第i个量测噪声矩阵；a_i表示第i个量测噪声权值；N(k+1)表示k+1时刻有效向量观测器个数；

式中，δb_k+1，i表示k+1时刻第i个观测噪声；r_k+1，i表示k+1时刻第i个参考向量；tr(·)表示对矩阵取迹操作；S_b，i为由δb_k+1，i和r_k+1，i构成的3维矩阵；z_b，i表示由δb_k+1，i和r_k+1，i构成的3维向量；κ_b，i表示矩阵B_b，i的迹；B_b，i表示由δb_k+1，i和r_k+1，i构成的3维矩阵；

步骤3：针对上述系统模型，利用矩阵卡尔曼滤波方法估计姿态K矩阵；

步骤3.1初始条件；

式中，表示初始时刻最优状态估计；P_0|0表示初始时刻状态误差协方差矩阵；Y₀表示初始时刻量测矩阵；R₀表示初始时刻量测噪声方差阵；

步骤3.2时间更新；

其中，Φ_k表示k时刻状态转移矩阵；表示k时刻最优状态估计；表示k+1时刻状态一步预测；P_k|k表示k时刻状态误差协方差矩阵；P_k+1|k表示k+1时刻状态误差协方差矩阵一步预测；Q_k表示系统噪声方差阵；表示克罗内克积；表示由状态转移矩阵构成的传递矩阵；

Q_k＝cov[W_k]＝cov[vec(W_k)]

式中，vec(·)表示对矩阵做向量转换；cov[·]表示协方差运算；表示k时刻最优状态估计；Υ_i表示第i个中间变换矩阵；e_i表示3维单位矩阵的第i列，I₃＝[e₁ e₂ e₃]；I₄表示4维单位阵；

步骤3.3量测更新；

S_k+1＝P_k+1|k+R_k+1

式中，为表示k+1时刻状态一步预测；Y_k+1表示k+1时刻系统量测；表示k+1时刻系统新息；P_k+1|k表示k+1时刻状态误差协方差矩阵一步预测；R_k+1表示k+1时刻量测噪声方差阵；S_k+1表示k+1时刻残差协方差矩阵；G_k+1表示k+1时刻滤波增益矩阵；表示k+1时刻最优状态估计；其中和E^lj满足下式定义：

其中，χ_lj表示E^lj中元素；量测噪声方差阵R_k+1表示为：

R_k+1＝cov[V_k]＝cov[vec(V_k)]

M＝[-[e₁×] -e₁ _[e₂×] -e₂ _[e₃×] -e₃ I₃ 0]

式中，R_i表示第i个量测噪声；a_k+1，i表示k+1时刻第i个权值；Λ_i表示中间变量矩阵；b_k+1，i表示k+1时刻第i个有效观测向量；为向量观测器k+1时刻第i个量测噪声方差值；r_k+1，i表示k+1时刻第i个有效参考向量；表示由参考向量r_k+1，i构成的斜对称矩阵；M表示由单位阵列向量构成的参数矩阵；

步骤4：基于残差匹配的自适应矩阵卡尔曼滤波算法(AMKF)；

由K矩阵的定义，采用如下公式定义残差：

式中，υ_k+1表示k+1时刻残差项；表示k+1时刻新息项；表示k+1时刻一步预测四元数；V_k+1表示k+1时刻量测噪声阵；

新息匹配公式：

根据对残差求解数学期望可知：

假设每次量测噪声都具有相同的误差类型，则上式可简化为：

式中，表示k+1时刻真实四元数，e_k+1表示四元数矢量部分，表示四元数标量部分；Θ_k+1，i表示k+1时刻由真实四元数与第i个参考向量r_k+1，i构成的参数矩阵；表示k+1时刻量测噪声方差值；N(k+1)表示k+1时刻有效观测向量数；[×]表示向量叉乘矩阵；

依据上式，采用四元数一步预测作为真实四元数的最优估计，可以得到基于残差匹配的量测噪声估计：

式中，表示k+1时刻的一步预测四元数；表示k+1时刻由一步预测四元数与第i个参考向量r_k+1，i构成的参数矩阵；b_k+1，i表示k+1时刻第i个量测向量；Y_k+1表示k+1时刻系统量测；表示k+1时刻量测噪声方差值的估计；

步骤5：根据步骤4计算得到的最优状态量，计算最优四元数，利用乘性误差四元数，得出误差角的大小：

步骤5.1最优四元数的提取；

根据步骤3和步骤4交互式滤波得到最优K矩阵，根据Wahba问题可知：

通过上式，再根据四元数规范化约束，采用拉格朗日乘子，可以得到K矩阵最大特征值对应的特征向量就是最优四元数，计算方法如下式：

式中，与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计；

步骤5.2估计载体坐标系与真实载体坐标系旋转误差角计算；

根据乘性误差四元数，在评估估计姿态角与真实姿态角之间的误差时，可以采用估计载体坐标系与真实载体坐标系之间的旋转角来表示，计算如下：

式中：δq_k|k表示k时刻旋转误差四元数；表示k时刻最优估计四元数的逆；o表示四元数乘法；表示k时刻旋转误差四元数标量；δφ_k|k表示k时刻估计载体坐标系与真实载体坐标系之间的旋转误差角；q_k表示k时刻真实旋转四元数；

步骤5.3自适应矩阵卡尔曼滤波增益因子及过程噪声；

由于矩阵卡尔曼滤波采用增益矩阵的形式更新最优估计，此处采用对增益矩阵求欧几里德范数作为滤波增益因子：

ρ＝||G_k+1||_E

式中，G_k+1表示k+1时刻滤波增益矩阵；||·||_E表示取欧几里德范数；ρ表示滤波增益因子；

过程噪声计算同样采用上述运算：

式中，Q_k表示系统噪声方差阵；表示过程噪声方差；

步骤6：姿态估计非线性离散系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺常值漂移结果，完成姿态估计，若k＜M，表示滤波过程未完成，则重复上述步骤3至步骤5，直至姿态估计系统运行结束。

2.根据权利要求1所述的一种自适应矩阵卡尔曼滤波姿态估计方法，其特征在于，步骤2中，陀螺量测噪声标准差为输出频率为10Hz；向量观测器测量标准差为μ＝1°，输出频率为1Hz；设定载体坐标系与参考坐标系固连，此时w＝[0 0 0]^Tdeg/s。

3.根据权利要求1所述的一种自适应矩阵卡尔曼滤波姿态估计方法，其特征在于，步骤3和步骤4中，自适应滤波参数均选取为μ＝0.01°，均在k≥2000开始使用自适应滤波估计载体姿态角。

4.根据权利要求1所述的一种自适应矩阵卡尔曼滤波姿态估计方法，其特征在于，步骤6中，M＝5400。