基于状态矩阵卡尔曼滤波的姿态估计算法研究

摘要

设计一种组合GPS/速率陀螺定姿系统.系统以方向余弦矩阵表示姿态,建立GPS/速率陀螺组合状态模型和观测模型.结合kalman滤波算法,提出一种状态矩阵卡尔曼滤波(State Matrix Kalman Kilter,SMKF)姿态估计算法,并采用拉格朗日算法对姿态矩阵进行正交化约束.与传统的基于四元数的扩展卡尔曼滤波(EKF)算法相比,基于方向余弦矩阵的姿态系统状态方程与测量方程均为线性方程,无需线性化处理,对初始姿态误差更具有较好的鲁棒性.数值仿真表明,该方法具有精度高和稳定性强等优点.