式中：Vk+1表示k+1时刻量测噪声；Xk+1表示k+1时刻状态矩阵；Yk+1表示k+1时刻 系统量测；其中量测矩阵表不为：
[0138]式中，bk+lii表示k+1时刻第i个量测向量；rk+lii表示k+1时刻第i个参考向量；tr( ?)表示对矩阵取迹操作；S为由bk+lidPrk+lii构成的3维矩阵；z表示由bk+lijPrk+lii 构成的3维向量；〇表示矩阵B的迹；B表示由bk+1^Prk+lii构成的3维矩阵；
[0140]其中，Vk+lil表示k+1时刻第i个量测噪声矩阵；ai表示第i个量测噪声权值；N(k+1)表示k+1时刻有效向量观测器个数；
[0143]式中，Sbk+lil表示k+1时刻第i个观测噪声；rk+lil表示k+1时刻第i个参考向量； tr(_)表示对矩阵取迹操作；SM为由Sbk+Ul和rk+Ul构成的3维矩阵；zM表示由Sbk+lil 和rk+lii构成的3维向量；KM表示矩阵心,;的迹；BM表示由5bk+1,JPrk+lii构成的3维矩 阵；
[0144] 步骤3:针对上述系统模型，利用矩阵卡尔曼滤波方法估计姿态K矩阵；
[0145] 步骤3.1初始条件；
[0146] -^qjo= ' ^〇1〇 =
[0147] 式中，表示初始时刻最优状态估计；Pid1。表示初始时刻状态误差协方差矩阵； Y。表示初始时刻量测矩阵；R。表示初始时刻量测噪声方差阵；
[0148] 步骤3. 2时间更新；
[0152] 其中，表示k时刻状态转移矩阵；Sitljl表示k时刻最优状态估计；尤；+)|(；表示k+1 时刻状态一步预测；Pklk表示k时刻状态误差协方差矩阵；Pk+1|k表示k+1时刻状态误差协方 差矩阵一步预测；Qk表示系统噪声方差阵；?表示克罗内克积；?^表示由状态转移矩阵构 成的传递矩阵；
[0157]式中，vec(?)表示对矩阵做向量转换；cov[?]表示协方差运算；表示k时刻 最优状态估计；T1表示第i个中间变换矩阵；ei表示3维单位矩阵的第i列，I3=[e:e2 e3] ;I4表示4维单位阵；
[0158]步骤3. 3量测更新；
[0164] 式中，矣?为表示k+1时刻状态一步预测；Yk+1表示k+1时刻系统量测；^+1表示 k+1时刻系统新息；Pk+1|k表示k+1时刻状态误差协方差矩阵一步预测；Rk+1表示k+1时刻量 测噪声方差阵；Sk+1表不k+1时刻残差协方差矩阵；Gk+1表不k+1时刻系统互协方差矩阵； M表示k+1时刻最优状态估计；其中Gf1和Eli满足下式定义：
[0173]M= [_Ee1X] ^e1_ [e2X] _e2_ [e3X] _e3I3 0]
[0174] 式中，R1表示第i个量测噪声；ak+lil表示k+1时刻第i个权值；A:表示中间变量 矩阵；bk+lil表示k+1时刻第i个有效观测向量；为向量观测器k+1时刻第i个量测噪 声方差值；rk+lii表示k+1时刻第i个有效参考向量；爲表示由参考向量rk+lii构成的斜对称 矩阵;M表示由单位阵列向量构成的参数矩阵；
[0175] 步骤4 :基于残差匹配的自适应矩阵卡尔曼滤波算法（AMKF);
[0176]由K矩阵的定义，采用如下公式定义残差：
[0178] 式中，uk+1表示k+1时刻残差项；表示k+1时刻新息项；么，表示k+1时刻一 步预测四元数；Vk+1表示k+1时刻量测噪声阵；
[0179] 新息匹配公式：
[0186] 式中，表示k+1时刻真实四元数，ek+1表示四元数矢量部分，&+1 表示四元数标量部分；?k+lii表示k+1时刻由真实四元数与第i个参考向量rk+li4^成的参 数矩阵；pL:表示k+1时刻量测噪声方差值；N(k+1)表示k+1时刻有效观测向量数；[X]表 示向量叉乘矩阵；
[0187] 依据上式，采用四元数一步预测作为真实四元数的最优估计，可以得到基于残差 匹配的量测噪声估计：
[0189] 式中，ii+1纟表示k+1时刻的一步预测四元数；@i+u表示k+1时刻由一步预测四元 数与第i个参考向量rk+lii构成的参数矩阵；bk+lii表示k+1时刻第i个量测向量；Yk+1表示 k+1时刻系统量测；表示k+1时刻量测噪声方差值的估计；
[0190] 步骤5:根据步骤4计算得到的最优状态量，计算最优四元数，利用乘性误差四元 数，得出误差角的大小：
[0191] 步骤5. 1最优四元数的提取；
[0192] 根据步骤3和步骤4交互式滤波得到最优K矩阵，根据Wahba问题可知：
[0194] 通过上式，再根据四元数规范化约束，采用拉格朗日乘子，可以得到K矩阵最大特 征值对应的特征向量就是最优四元数，计算方法如下式：
[0196] 式中，^、％与％为1^时刻从最优估计K矩阵.中按照K矩阵定义提取的最 优估计。
[0197] 步骤5. 2估计载体坐标系与真实载体坐标系旋转误差角计算；
[0198] 根据乘性误差四元数，在评估估计姿态角与真实姿态角之间的误差时，可以采用 估计载体坐标系与真实载体坐标系之间的旋转角来表示，计算如下：
[0199]
[0200] 式中：Sqk|k表示k时刻旋转误差四元数；以表示k时刻最优估计四元数的逆 表示四元数乘法；表示k时刻旋转误差四元数标量；5 (J)klk表示k时刻估计载体坐标系 与真实载体坐标系之间的旋转误差角；qk表示k时刻真实旋转四元数；
[0201] 步骤5. 3自适应矩阵卡尔曼滤波增益因子及过程噪声；
[0202] 由于矩阵卡尔曼滤波采用增益矩阵的形式更新最优估计，此处采用对增益矩阵求 欧几里德范数作为滤波增益因子：
[0203] P=I|Gk+1||E
[0204] 式中，Gk+1表示k+1时刻滤波增益矩阵；II?II￡表示取欧几里德范数；P表示滤 波增益因子；
[0205] 过程噪声计算同样采用上述运算：
[0207] 式中，(\表不系统噪声方差阵；O'〗表不过程噪声方差；
[0208] 对本发明的有益效果说明如下：
[0209] MTLAB仿真实验，在以下的仿真条件下，对该方法进行仿真实验：
为y=r，输出频率为IHz;设定载体坐标系与参考坐标系固连，此时w= [0 0 0]Tdeg/s。适应滤波参数均选取为y=0.01°，在2000开始使用自适应滤波估计载体姿态 角。步骤6中，M= 5400。
[0211] 如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释 为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对 其在形式上和细节上作出各种变化。
【主权项】
1. 一种自适应矩阵卡尔曼滤波姿态估计方法，其特征在于，包括以下步骤： 步骤1 :获取传感器实时数据，所述传感器实时数据包括陀螺仪数据和向量观测器数 据； 步骤2 :建立系统过程方程与量测方程； 所述的姿态估计系统模型包括： (1) 系统过程方程式中：Wk表示k时刻过程噪声阵；Φ k表示k时刻状态转移矩阵；X k表示k时刻状态矩 阵；Xk+1表示k+Ι时刻状态矩阵；T表示对矩阵求转置；式中，ε k表示k时刻由陀螺噪声向量构造的斜对称矩阵；△ t表示采样时间间隔； Ω (·)表示由COk构成的斜对称矩阵；I I · I I表示对向量取模值；I 4表示4维单位阵；ω k 表示k时刻载体旋转角速率；其中Ilvik为高