Quaternion Central Divided Difference Kalman Filtering Algorithm and Its applications to Initial Alignment of SINS

摘要

Considering the characteristic and computation superiority of quaternion representing body attitude movement information, and aiming at the initial alignment procedure of strap-down inertial navigation system (SINS) with large initial misalignment angles, this paper develops its multiplicative quaternion error model. With the faults analysis of traditional quaternion mean calculation methods, it developed its new calculation method in which the attitude matrix cost function was constructed to calculate its maximum eigenvalues, and it selected the eigenvector which correspond to the maximum eigenvalue as the predicted quaternion mean to guarantee its unit normalization and the sign invariability when the sign of calculating quaternion changed. The multiplicative quaternion error representing the distance between quaternion Sigma-points and the predicted mean quaternion calculated the quaternion prediction error variance matrix, and which can effectively overcome the application limits for SPKF algorithms in quaternion filtering implementation. Combined with the central divided difference filtering (CDKF) algorithm which belongs to SPKF algorithms, it proposed a new quaternion CDKF algorithm (QCDKF) for quaternion filtering problems. With large initial misalignment angles and based on QCDKF algorithm the SINS simulation experiments was being performed. The simulations results show that, compared with EKF algorithm, the proposed algorithm can significantly improve the filtering precision of both attitude misalignment angles estimation errors and velocity estimation errors and the numerical calculation stabilization the filtering algorithm.