[发明专利]自适应矩阵卡尔曼滤波姿态估计方法

摘要

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

主权项

一种自适应矩阵卡尔曼滤波姿态估计方法，其特征在于，包括以下步骤：步骤1：获取传感器实时数据，所述传感器实时数据包括陀螺仪数据和向量观测器数据；步骤2：建立系统过程方程与量测方程；姿态估计系统模型包括：(1)系统过程方程式中：Wk表示k时刻过程噪声阵；Φk表示k时刻状态转移矩阵；Xk表示k时刻状态矩阵；Xk+1表示k+1时刻状态矩阵；T表示对矩阵求转置；Wk＝(Xkεk‑εkXk)Δt式中，εk表示k时刻由陀螺噪声向量构造的斜对称矩阵；Δt表示采样时间间隔；Ω(·)表示由ωk构成的斜对称矩阵；||·||表示对向量取模值；I4表示4维单位阵；ωk表示k时刻载体旋转角速率；其中ηv，k为高斯白噪声，其期望与方差满足下式：E(ηv，k)＝0式中，I3表示3维单位阵；表示陀螺单轴噪声方差值；Δt表示采样时间间隔；E(·)表示取数学期望；(2)系统量测方程Yk+1＝Xk+1+Vk+1式中：Vk+1表示k+1时刻量测噪声；Xk+1表示k+1时刻状态矩阵；Yk+1表示k+1时刻系统量测；其中量测矩阵表示为：式中，bk+1，i表示时刻第i个量测向量；rk+1，i表示k+1时刻第i个参考向量；tr(·)表示对矩阵取迹操作；S为由bk+1，i和rk+1，i构成的3维矩阵；z表示由bk+1，i和rk+1，i构成的3维向量；σ表示矩阵B的迹；B表示由bk+1，i和rk+1，i构成的3维矩阵；其中，Vk+1，i表示k+1时刻第i个量测噪声矩阵；ai表示第i个量测噪声权值；N(k+1)表示k+1时刻有效向量观测器个数；式中，δbk+1，i表示k+1时刻第i个观测噪声；rk+1，i表示k+1时刻第i个参考向量；tr(·)表示对矩阵取迹操作；Sb，i为由δbk+1，i和rk+1，i构成的3维矩阵；zb，i表示由δbk+1，i和rk+1，i构成的3维向量；κb，i表示矩阵Bb，i的迹；Bb，i表示由δbk+1，i和rk+1，i构成的3维矩阵；步骤3：针对上述系统模型，利用矩阵卡尔曼滤波方法估计姿态K矩阵；步骤3.1初始条件；式中，表示初始时刻最优状态估计；P0|0表示初始时刻状态误差协方差矩阵；Y0表示初始时刻量测矩阵；R0表示初始时刻量测噪声方差阵；步骤3.2时间更新；其中，Φk表示k时刻状态转移矩阵；表示k时刻最优状态估计；表示k+1时刻状态一步预测；Pk|k表示k时刻状态误差协方差矩阵；Pk+1|k表示k+1时刻状态误差协方差矩阵一步预测；Qk表示系统噪声方差阵；表示克罗内克积；表示由状态转移矩阵构成的传递矩阵；Qk＝cov[Wk]＝cov[vec(Wk)]式中，vec(·)表示对矩阵做向量转换；cov[·]表示协方差运算；表示k时刻最优状态估计；Υi表示第i个中间变换矩阵；ei表示3维单位矩阵的第i列，I3＝[e1 e2 e3]；I4表示4维单位阵；步骤3.3量测更新；Sk+1＝Pk+1|k+Rk+1式中，为表示k+1时刻状态一步预测；Yk+1表示k+1时刻系统量测；表示k+1时刻系统新息；Pk+1|k表示k+1时刻状态误差协方差矩阵一步预测；Rk+1表示k+1时刻量测噪声方差阵；Sk+1表示k+1时刻残差协方差矩阵；Gk+1表示k+1时刻滤波增益矩阵；表示k+1时刻最优状态估计；其中和Elj满足下式定义：其中，χlj表示Elj中元素；量测噪声方差阵Rk+1表示为：Rk+1＝cov[Vk]＝cov[vec(Vk)]M＝[‑[e1×] ‑e1 _[e2×] ‑e2 _[e3×] ‑e3 I3 0]式中，Ri表示第i个量测噪声；ak+1，i表示k+1时刻第i个权值；Λi表示中间变量矩阵；bk+1，i表示k+1时刻第i个有效观测向量；为向量观测器k+1时刻第i个量测噪声方差值；rk+1，i表示k+1时刻第i个有效参考向量；表示由参考向量rk+1，i构成的斜对称矩阵；M表示由单位阵列向量构成的参数矩阵；步骤4：基于残差匹配的自适应矩阵卡尔曼滤波算法(AMKF)；由K矩阵的定义，采用如下公式定义残差：式中，υk+1表示k+1时刻残差项；表示k+1时刻新息项；表示k+1时刻一步预测四元数；Vk+1表示k+1时刻量测噪声阵；新息匹配公式：根据对残差求解数学期望可知：假设每次量测噪声都具有相同的误差类型，则上式可简化为：式中，表示k+1时刻真实四元数，ek+1表示四元数矢量部分，表示四元数标量部分；Θk+1，i表示k+1时刻由真实四元数与第i个参考向量rk+1，i构成的参数矩阵；表示k+1时刻量测噪声方差值；N(k+1)表示k+1时刻有效观测向量数；[×]表示向量叉乘矩阵；依据上式，采用四元数一步预测作为真实四元数的最优估计，可以得到基于残差匹配的量测噪声估计：式中，表示k+1时刻的一步预测四元数；表示k+1时刻由一步预测四元数与第i个参考向量rk+1，i构成的参数矩阵；bk+1，i表示k+1时刻第i个量测向量；Yk+1表示k+1时刻系统量测；表示k+1时刻量测噪声方差值的估计；步骤5：根据步骤4计算得到的最优状态量，计算最优四元数，利用乘性误差四元数，得出误差角的大小：步骤5.1最优四元数的提取；根据步骤3和步骤4交互式滤波得到最优K矩阵，根据Wahba问题可知：通过上式，再根据四元数规范化约束，采用拉格朗日乘子，可以得到K矩阵最大特征值对应的特征向量就是最优四元数，计算方法如下式：式中，与为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估计；步骤5.2估计载体坐标系与真实载体坐标系旋转误差角计算；根据乘性误差四元数，在评估估计姿态角与真实姿态角之间的误差时，可以采用估计载体坐标系与真实载体坐标系之间的旋转角来表示，计算如下：式中：δqk|k表示k时刻旋转误差四元数；表示k时刻最优估计四元数的逆；o表示四元数乘法；表示k时刻旋转误差四元数标量；δφk|k表示k时刻估计载体坐标系与真实载体坐标系之间的旋转误差角；qk表示k时刻真实旋转四元数；步骤5.3自适应矩阵卡尔曼滤波增益因子及过程噪声；由于矩阵卡尔曼滤波采用增益矩阵的形式更新最优估计，此处采用对增益矩阵求欧几里德范数作为滤波增益因子：ρ＝||Gk+1||E式中，Gk+1表示k+1时刻滤波增益矩阵；||·||E表示取欧几里德范数；ρ表示滤波增益因子；过程噪声计算同样采用上述运算：式中，Qk表示系统噪声方差阵；表示过程噪声方差；步骤6：姿态估计非线性离散系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺常值漂移结果，完成姿态估计，若k＜M，表示滤波过程未完成，则重复上述步骤3至步骤5，直至姿态估计系统运行结束。