基于共轭梯度的EKF姿态估计算法

EKF attitude estimation algorithm based on conjugate gradient

针对在低成本捷联惯性测量器件中,扩展卡尔曼姿态估计算法存在的精度低、抗干扰性弱等问题,提出一种基于共轭梯度的EKF姿态估计算法。以姿态四元数为状态量,在观测模型中将基于共轭梯度算法估计出来的姿态四元数作为观测量;将非重力运动加速度引入噪声协方差矩阵,实现噪声的自适应动态估计;采用倾角补偿的方法计算偏航角,提高飞行器姿态估计的精度和抗干扰能力。为验证所设计姿态估计算法的有效性,搭建基于ARM Cortex M4的四旋翼飞控系统,实验结果表明,该算法提高了姿态估计的精度以及四旋翼飞行器在非重力运动加速度和磁干扰下的抗干扰能力。

The common extended Kalman filter attitude estimation algorithm has such problems as low precision,weak anti-jamming for the low-cost strapdown inertial measurement device.An EKF for attitude estimation algorithm based on conjugate gradient was presented.The attitude quaternion was taken as status quantity.In the observation model,attitude quaternion based on conjugate gradient algorithm was taken as observation quantity.The non-gravity motion acceleration was introduced into the noise covariance matrix,which realized self-adaptive and dynamic estimation of noise.The yaw angle was calculated using the method of inclination compensation,which improved the accuracy and anti-jamming ability of the aircraft.The quadrotor control system based on ARM Cortex M4 was built to verify the effective of proposed algorithm.Experimental results show that the proposed algorithm can improve the accuracy of attitude estimation and the anti-jamming ability of quadrotor under non-gravity motion acceleration and magnetic interference.