Attitude estimation based on quaternion square-root cubature Kalman filter

Considering that a multiplicative extended Kalman filter (MEKF) existed lower estimation accuracy and slower convergence rate for large initialization errors, a quaternion square-root cubature Kalman filter (QSCKF) was proposed. A quaternion nonlinear error model was derived for attitude determination system, and then the cubature numerical integration theory was used to calculate the mean and variance of the nonlinear function. The square-root forms were used to improve numerical stability. To avoid the quaternion normalization problem, a Lagrange loss function was derived to compute the average quaternion. The simulation results indicate that the proposed algorithm provides higher estimation precision with faster convergence rate and better filter's stability than MEKF and unscented quaternion estimator (USQUE) for large initialization errors. And, the estimated time of this algorithm is shortened by 1/3 compared to USQUE.