基于Mahony与误差状态卡尔曼滤波融合的姿态解算方法

Fused attitude estimation method based on Mahony filtering and Error-State Kalman filtering

针对惯性测量单元(IMU)精度较低的问题以及传统姿态解算算法误差较大的缺点,提出了一种基于IMU的融合Mahony滤波与误差状态卡尔曼滤波(ESKF)的姿态解算方法。为降低因非重力加速度及磁干扰所带来的误差,根据加速度计和磁力计的置信度设计两个不同的自适应PI控制器改进Mahony算法,IMU测量数据经其解算,作为ESKF的量测值。ESKF将真实状态定义为名义状态与误差状态的组合,间接完成对系统状态的估计。因误差量为小量,所以误差状态线性化时的误差更小,雅克比矩阵的计算更简单。经实验验证,相较于传统姿态解算算法,融合算法能有效减少高频噪声、数据漂移带来的误差,提高姿态解算精度。

In order to solve the problem of the low accuracy of Inertial Measurement Unit(IMU)and the large error of traditional attitude estimation algorithm,an attitude estimation method based on IMU combining Mahony filter and Error State Kalman Filter(ESKF)was proposed.In order to reduce the error caused by non-gravity acceleration and magnetic interference,two adaptive PI controllers are used to improve the Mahony algorithm,and the IMU measurement data are solved by the improved Mahony algorithm as the ESKF measurement value.ESKF defines the true state as the combination of the nominal state and the error state,and indirectly completes the estimation of the system state.Because for small amount of error,error state linearization error is smaller,the calculation of Jacobi matrix is simpler.Experimental results show that compared with the traditional attitude calculation algorithm,the fusion algorithm can effectively reduce the error caused by high-frequency noise and data drift,and improve the accuracy of attitude calculation.