自适应矩阵卡尔曼滤波姿态估计方法
【技术领域】
[0001] 本发明涉及一种自适应矩阵卡尔曼滤波姿态估计方法，为一种基于Wahba问题的 最优K矩阵姿态估计方法，采用自适应方法解决量测噪声不确定时姿态估计问题，属于载 体姿态估计领域.
【背景技术】
[0002] 载体姿态估计技术是导航技术领域中的关键技术，采用陀螺仪与向量观测器（星 敏感器、磁强计、水平测量仪等）组成的姿态估计系统具有测姿精度高、可靠性好、自主性 强等优点。而采用向量观测进行姿态估计时，主要有两种方法：一种是采用最小二乘技术的 姿态估计方法，该方法主要基于Wahba损失函数，利用观测向量与参考向量之间的关系建 立矩阵，对矩阵进行结算得到载体姿态四元数；另一种方法是基于非线性滤波理论的四元 数估计法。而采用矩阵的姿态估计方法具有系统模型线性、计算量小、估计精度高等优点。 众多学者在此基础上提出了多种姿态估计方法，其中D.Choukroun等人研究的基于矩阵卡 尔曼滤波（matrixKalmanfilter,MKF)算法的最优矩阵方法具有鲁棒性强，计算精度高等 优点，但是四元数矩阵卡尔曼滤波的递推过程中对系统模型进行了多次近似处理，而且是 基于系统量测噪声准确已知的条件下进行的，但在实际过程中，量测噪声往往随实际系统 差异存在一定的误差，使得算法估计精度下降，扰动噪声变大。
[0003] 为克服MKF算法在量测噪声统计不准确时估计精度下降的问题，采用一种基于残 差匹配技术的自适应矩阵卡尔曼滤波算法（AdaptivematrixKalmanfilter,AMKF)。在算 法迭代过程中对量测噪声进行实时估计，将实时计算的量测噪声反馈给MKF算法，提高系 统更新过程中残差协方差的计算精度，从而提高系统估计精度。
【发明内容】
[0004] 发明目的：本发明的目的在于为了提高姿态估计精度和滤波算法的自适应性，提 出了一种自适应矩阵卡尔曼滤波算法（AMKF)。发明采用MKF算法估计载体姿态四元数，采 用基于残差匹配技术的自适应方法对量测噪声进行实时估计，提高系统的估计精度.
[0005] 技术方案：本发明所述的自适应矩阵卡尔曼滤波姿态估计方法，包括以下步骤：
[0006] 步骤1 :获取传感器实时数据，所述传感器实时数据包括陀螺仪数据和向量观测 器数据；
[0007] 步骤2 :建立系统过程方程与量测方程；
[0008] 所述的姿态估计系统模型包括：
[0009] (1)系统过程方程
[0010] 乂減二(^kX灰 +Wk
[0011] 式中：wk表示k时刻过程噪声阵；?k表示k时刻状态转移矩阵；Xk表示k时刻状 态矩阵；\+1表示k+1时刻状态矩阵；T表示对矩阵求转置；
[0012] Wk=(XkEk-EkXk)At
[0016] 式中，ek表示k时刻由陀螺噪声向量构造的斜对称矩阵；At表示采样时间间隔； Q(?)表示由wk构成的斜对称矩阵；II?II表示对向量取模值；I4表示4维单位阵k 表示k时刻载体旋转角速率；其中nVik为高斯白噪声，其期望与方差满足下式：
[0017] E(nv,k) = 〇
[0019] 式中，I3表示3维单位阵；％2表示陀螺单轴噪声方差值；At表示采样时间间隔； E(_)表示取数学期望；
[0020] ⑵系统量测方程
[0021] Yk+1=Xk+1+Vk+1
[0022] 式中：Vk+1表示k+1时刻量测噪声；Xk+1表示k+1时刻状态矩阵；Yk+1表示k+1时刻 系统量测；其中量测矩阵表不为：
[0025] 式中，bk+lii表示时刻第i个量测向量；rk+lii表示k+1时刻第i个参考向量；tr( ?) 表示对矩阵取迹操k+1作；S为由bk+lil和rk+lil构成的3维矩阵；z表示由bk+lil和rk+lil构 成的3维向量；〇表示矩阵B的迹；B表示由bk+1jPrk+lii构成的3维矩阵；
[0027] 其中，Vk+lil表示k+1时刻第i个量测噪声矩阵；ai表示第i个量测噪声权值；N(k+1)表示k+1时刻有效向量观测器个数；
[0030] 式中，Sbk+lil表示k+1时刻第i个观测噪声；rk+lil表示k+1时刻第i个参考向量； tr(_)表示对矩阵取迹操作为由Sbk+Ul和rk+Ul构成的3维矩阵表示由Sbk+lil 和rk+lii构成的3维向量；KM表示矩阵心,;的迹；BM表示由5bk+1,JP rk+lii构成的3维矩 阵；
[0031] 步骤3:针对上述系统模型，利用矩阵卡尔曼滤波方法估计姿态K矩阵；
[0032] 步骤3. 1初始条件；
[0034] 式中，1_表示初始时刻最优状态估计；Pid1。表示初始时刻状态误差协方差矩阵； Y。表示初始时刻量测矩阵；R。表示初始时刻量测噪声方差阵；
[0035] 步骤3. 2时间更新；
[0039] 其中，? k表示k时刻状态转移矩阵；爱_表示k时刻最优状态估计；表示k+1 时刻状态一步预测；Pklk表示k时刻状态误差协方差矩阵；Pk+1|k表示k+1时刻状态误差协方 差矩阵一步预测；(\表示系统噪声方差阵；<8表示克罗内克积；?^表示由状态转移矩阵构 成的传递矩阵；
[0044] 式中，vec( ?)表示对矩阵做向量转换；cov[ ?]表示协方差运算；表示k时刻 最优状态估计；T1表示第i个中间变换矩阵；ei表示3维单位矩阵的第i列，I3=[e:e2 e3] ;I4表示4维单位阵；
[0045] 步骤3. 3量测更新；
[0051] 式中，矣_为表示k+1时刻状态一步预测；Yk+1表示k+1时刻系统量测；1丨+1表示 k+1时刻系统新息；Pk+1|k表示k+1时刻状态误差协方差矩阵一步预测；Rk+1表示k+1时刻量 测噪声方差阵；Sk+1表不k+1时刻残差协方差矩阵；Gk+1表不k+1时刻滤波增益矩阵； 表示k+1时刻最优状态估计；其中G7UpE1]满足下式定义：
[0060]M= [_Ee1X] ^e1_ [e2X] _e2_ [e3X] _e3I3 0]
[0061] 式中，R1表示第i个量测噪声；ak+1;1表示k+1时刻第i个权值；A:表示中间变量 矩阵；bk+lil表示k+1时刻第i个有效观测向量；为向量观测器k+1时刻第i个量测噪 声方差值；rk+lii表示k+1时刻第i个有效参考向量；義表示由参考向量;rk+lii构成的斜对称 矩阵;M表示由单位阵列向量构成的参数矩阵；
[0062] 步骤4 :基于残差匹配的自适应矩阵卡尔曼滤波算法（AMKF);
[0063] 由K矩阵的定义，采用如下公式定义残差：
[0065] 式中，uk+1表示k+1时刻残差项；_名+1:表示k+1时刻新息项；表.表示k+1时刻一 步预测四元数；Vk+1表示k+1时刻量测噪声阵；
[0066] 新息匹配公式：
[0067] E(O^oll) =UinUll
[0068] 根据对残差求解数学期望可知：
[0070] 假设每次量测噪声都具有相同的误差类型，则上式可简化为：
[0073] 式中，氣+1: =[eAT+1 表示k+1时刻真实四元数，ek+1表示四元数矢量部分 表示四元数标量部分；?k+lii表示k+1时刻由真实四元数与第i个参考向量rk+li4^成的参 数矩阵；/^+1表示k+1时刻量测噪声方差值；N(k+1)表示k+1时刻有效观测向量数；[X