斯白噪声，其期望与方差满足下式：式中，I3表示3维单位阵；表示陀螺单轴噪声方差值；△ t表示采样时间间隔；E ( ·) 表示取数学期望； (2) 系统量测方程 Yk+1= X k+1+Vk+l 式中：vk+1表示k+1时刻量测噪声；X k+1表示k+1时刻状态矩阵；Y k+1表示k+1时刻系统 量测；其中量测矩阵表示为：式中，bk+lii表示时刻第i个量测向量；r k+lii表示k+1时刻第i个参考向量；tr ( ·)表 示对矩阵取迹操k+Ι作；S为由bk+lil和r k+lil构成的3维矩阵；z表示由b k+lil和r k+lil构成 的3维向量；σ表示矩阵B的迹；B表示由bk+1jP r k+lii构成的3维矩阵；其中，Vk+lil表示k+Ι时刻第i个量测噪声矩阵；表示第i个量测噪声权值；N(k+1)表 示k+Ι时刻有效向量观测器个数；式中，δ bk+lii表示k+Ι时刻第i个观测噪声；rk+lii表示k+Ι时刻第i个参考向量；tr (·) 表不对矩阵取迹操作；SM为由δ b k+1,JP r k+lii构成的3维矩阵；z M表不由δ b k+1,jP r k+lii 构成的3维向量；K M表示矩阵B M的迹；B M表示由δ b k+1jP r k+lii构成的3维矩阵； 步骤3 :针对上述系统模型，利用矩阵卡尔曼滤波方法估计姿态K矩阵； 步骤3. 1初始条件；式中，表示初始时刻最优状态估计；Pid1。表示初始时刻状态误差协方差矩阵；Y。表 示初始时刻量测矩阵；R。表示初始时刻量测噪声方差阵； 步骤3. 2时间更新；其中，表示k时刻状态转移矩阵；表示k时刻最优状态估计；；表示k+Ι时 刻状态一步预测；Pk|k表不k时刻状态误差协方差矩阵；P !^卜表不k+Ι时刻状态误差协方差 矩阵一步预测；Qk表示系统噪声方差阵；表示克罗内克积；巧表示由状态转移矩阵构成 的传递矩阵；式中，vec( ·)表示对矩阵做向量转换；cov[ ·]表示协方差运算；名;|(1表示k时刻最优 状态估计；T1表示第i个中间变换矩阵；e i表示3维单位矩阵的第i列，I 3= [e i e2 e3]; I4表示4维单位阵； 步骤3. 3量测更新；式中，尤+]μ.为表示k+Ι时刻状态一步预测；Yk+1表示k+Ι时刻系统量测；$+1表示k+1 时刻系统新息；Pk+1|k表示k+1时刻状态误差协方差矩阵一步预测；Rk+1表示k+1时刻量测噪 声方差阵；S k+1表不k+Ι时刻残差协方差矩阵；Gk+1表不k+Ι时刻滤波增益矩阵；名叫川表不 k+Ι时刻最优状态估计；其中每^和E11满足下式定义：其中，X υ表示E 元素；量测噪声方差阵R k+1表示为：式中，R1表示第i个量测噪声；ak+lil表示k+1时刻第i个权值；Λ i表示中间变量矩阵； bk+lil表示k+Ι时刻第i个有效观测向量；为向量观测器k+Ι时刻第i个量测噪声方差 值；rk+lii表示k+Ι时刻第i个有效参考向量；為表示由参考向量rk+lii构成的斜对称矩阵； M表示由单位阵列向量构成的参数矩阵； 步骤4:基于残差匹配的自适应矩阵卡尔曼滤波算法（AMKF); 由K矩阵的定义，采用如下公式定义残差：式中，υ k+1表示k+Ι时刻残差项；表示k+Ι时刻新息项表示k+Ι时刻一步预 测四元数；Vk+1表示k+Ι时刻量测噪声阵； 新息匹配公式： -^(υ?ΗυΑ + 1 ) = υ?+\υ?+1 根据对残差求解数学期望可知：假设每次量测噪声都具有相同的误差类型，则上式可简化为：式中，SW]表示k+1时刻真实四元数，ek+1表示四元数矢量部分；+1表示 四元数标量部分；? k+lii表示k+Ι时刻由真实四元数与第i个参考向量;r k+lii构成的参数矩 阵；的2+1表示k+Ι时刻量测噪声方差值；N(k+1)表示k+Ι时刻有效观测向量数；[X]表示向 量叉乘矩阵； 依据上式，采用四元数一步预测作为真实四元数的最优估计，可以得到基于残差匹配 的量测噪声估计：式中，4m表示k+1时刻的一步预测四元数；表示k+1时刻由一步预测四元数与 第i个参考向量rk+lii构成的参数矩阵；b k+lii表示k+Ι时刻第i个量测向量；Yk+1表示k+1 时刻系统量测；.處 +1表示k+1时刻量测噪声方差值的估计； 步骤5 :根据步骤4计算得到的最优状态量，计算最优四元数，利用乘性误差四元数，得 出误差角的大小： 步骤5.1最优四元数的提取； 根据步骤3和步骤4交互式滤波得到最优K矩阵，根据Wahba问题可知：通过上式，再根据四元数规范化约束，采用拉格朗日乘子，可以得到K矩阵最大特征值 对应的特征向量就是最优四元数，计算方法如下式：式中，^与^为k时刻从最优估计K矩阵中按照K矩阵定义提取的最优估 计。 步骤5. 2估计载体坐标系与真实载体坐标系旋转误差角计算； 根据乘性误差四元数，在评估估计姿态角与真实姿态角之间的误差时，可以采用估计 载体坐标系与真实载体坐标系之间的旋转角来表示，计算如下：式中：δ qk|k表示k时刻旋转误差四元数；?表示k时刻最优估计四元数的逆；。表 示四元数乘法；表示k时刻旋转误差四元数标量；δ (J)klk表示k时刻估计载体坐标系 与真实载体坐标系之间的旋转误差角；qk表示k时刻真实旋转四元数； 步骤5. 3自适应矩阵卡尔曼滤波增益因子及过程噪声； 由于矩阵卡尔曼滤波采用增益矩阵的形式更新最优估计，此处采用对增益矩阵求欧几 里德范数作为滤波增益因子： P=II Gk+111E 式中，Gk+1表示k+i时刻滤波增益矩阵；M · 11 E表示取欧几里德范数；P表示滤波增 益因子； 过程噪声计算同样采用上述运算：式中，Qk表不系统噪声方差阵；表不过程噪声方差； 步骤6 :姿态估计非线性离散系统的运行时间为M，若k = M，则输出姿态四元数及陀螺 常值漂移结果，完成姿态估计，若k < M，表示滤波过程未完成，则重复上述步骤3至步骤5， 直至姿态估计系统运行结束。2. 根据权利要求1所述的一种自适应矩阵卡尔曼滤波姿态估计方法，其特征在于，步 骤2中，陀螺量测噪声标准差为σ,. = 0.1 'V，输出频率为IOHz ;向量观测器测量标准差为 μ = 1°，输出频率为IHz ;设定载体坐标系与参考坐标系固连，此时w = [0 0 0]Tdeg/s。3. 根据权利要求1所述的一种自适应矩阵卡尔曼滤波姿态估计方法，其特征在于，步 骤3和步骤4中，自适应滤波参数均选取为μ = 0. 0Γ，均在k彡2000开始使用自适应滤 波估计载体姿态角。4. 根据权利要求1所述的一种用于卫星姿态确定的交互式滤波方法，其特征在于，步 骤 6 中，M = 5400。
【专利摘要】本发明公开一种自适应矩阵卡尔曼滤波姿态估计方法，包括以下几个步骤：步骤1：获取传感器实时数据；步骤2：建立系统过程方程与量测方程；步骤3：在建立的系统模型的基础上，利用矩阵卡尔曼滤波估计最优K矩阵；步骤4：建立基于残差匹配的AMKF滤波方程；步骤5：利用姿态四元数中乘性误差四元数计算方法，得到估计姿态与真实姿态之间的误差角，采用矩阵欧几里德范数运算表示滤波增益因子及矩阵求迹运算计算过程噪声；步骤6：姿态估计离散系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺常值漂移结果，完成姿态估计，若k＜M，表示滤波过程未完成，则重复上述步骤三至步骤五，直至姿态估计系统运行结束。
【IPC分类】G01C21/20
【公开号】CN105066996
【申请号】CN201510426626
【发明人】徐晓苏, 徐祥, 杨冬瑞, 王捍兵
【申请人】东南大学
【公开日】2015年11月18日
【申请日】2015年7月20日