基于四元数EKF算法的小型无人机姿态估计

针对小型无人机设计的姿态测量系统,提出一种用于小型无人机姿态估计的四元数扩展Kalman滤波算法.该算法通过建立四元数姿态运动模型和航姿传感器测量模型,构建了以四元数和陀螺仪随机漂移为状态向量、以加速度计测量值和磁阻仪解算的航向角为观测向量的扩展Kalman滤波器,并设计了自适应测量噪声协方差矩阵修正法.实验结果表明,该算法不但解决了微机电系统惯性器件用于载体姿态测量时精度低、易发散、易被干扰的问题,而且显著减小了陀螺仪随机漂移对姿态估计的影响,有效提高了姿态估计的精度.

Small UAV Attitude Estimation Based on the Algorithm of Quaternion Extended Kalman Filter

A new algorithm of quaternion extended Kalman filter was adopted to estimate the attitude of small UAV. After the establishment of themodels of quaternion attitude movement and magnetic, angular rate, and gravity sensor measurement, a new designed extended Kalman filter was given with the quaternion and random drifting of gyro being the static vectors and the accelerometer measurements and the heading solved by the magnetometer being the observational vectors. What’s more, an innovation amendment method based on an adaptive approach to construct the measurement noise covariance matrix was designed. Experimental results show that the algorithm not only solves the problems of micro electro mechanical systems inertial sensors used for attitude measurement to show low accuracy and to be easy to diverge and to disturb, but also significantlyreduces the effect of random drifting of gyro scope on attitude estimation, andproves to be effective at improving the accuracy of attitude estimation.