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

说明书

技术领域

本发明涉及一种自适应矩阵卡尔曼滤波姿态估计方法,为一种基于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维矩阵；