基于四元数平方根UKF算法的SINS/GPS紧组合导航系统研究

在较大初始姿态误差角下，针对SINS/GPS紧组合导航系统扩展卡尔曼滤波（extenthed Kalman filter, EKF）算法定位精度下降的问题，提出了一种基于四元数的平方根无迹卡尔曼滤波（square root unscented Kalman filter, SRUKF）算法。为解决SRUKF算法中四元数正交规范化的限制，通过构造姿态矩阵代价函数将四元数预测均值问题转化为代价函数最小时的四元数向量求解问题，保证了均值四元数的规范化；利用乘性四元数误差表示四元数预测值与均值之间的距离，求取四元数的预测协方差矩阵，保证了算法的合理性。在此基础上，给出了SINS/GPS紧组合系统四元数平方根无迹卡尔曼滤波算法的具体步骤。在较大初始姿态误差角下的仿真实验结果表明，与EKF算法相比，该算法精度更高，稳定性更强。

SINS/GPS tightly integrated navigation system based on quaternion square root unscented Kalman filter

Considering the divergence phenomenon of extenthed Kalman filter (EKF) in SINS/GPS tightly integrated navigation system, a square root unscented Kalman filter (QSRUKF) algorithm based on quaternion  is proposed. In order to ensure normalization of unit quaternion in SRUKF algorithm, an attitude matrix cost function is established to solve the problem of predicted average quaternion by minimizing the cost function; the multiplicative quaternion error is used to predict covariance, since it represents the distance between predicted quaternion and mean quaternion. On the basis, the specific steps of QSRUKF in SINS/GPS integrated navigation system are given. Simulation results show that the proposed algorithm has higher precision and stronger robustness under the larger initial attitude error angle compared with the EKF algorithm.