基于MACF-CKF多传感器融合的姿态解算算法

Attitude Algorithm Based on MACF-CKF Multi-Sensor Fusion

针对惯性导航测量单元姿态解算精度低的问题,提出了一种基于多传感器隶属度自适应互补滤波(Membership Adaptive Complementary Filtering,MACF)和容积卡尔曼滤波(Cubature Kalman Filter,CKF)相融合的姿态解算算法。使用指数加权移动平均修正陀螺仪噪声偏差,为了避免出现陀螺仪和加速度计的权重在互补滤波中分配不当而导致俯仰角和横滚角误差较大的现象,通过构造陀螺仪偏差的隶属度函数,判断互补滤波对陀螺仪的信任度,根据信任度动态调整互补滤波自适应因子,同时用磁力计和陀螺仪进行CKF来解决航向角发散的问题。实验表明:所提出的算法无论在静态条件还是动态条件下均能快速、准确实现姿态解算,在动态车载实验中,横滚角和俯仰角精度分别提升了24.5%和63.2%,航向角提升了48.8%,可以保证解算精度。

Targeting at the problem that the unit attitude solving accuracy is low for the inertial navigation measurement,an attitude calcu-lation algorithm based on the fusion of multi-sensor membership adaptive complementary filtering(MACF)and volumetric Kalman filter(CKF)is proposed.The exponential weighted moving average is used to correct the gyro noise deviation.In order to avoid the large error of pitch angle and roll angle caused by the improper distribution of the weight of gyroscope and accelerometer in the complementary filtering,the membership function of gyro deviation is constructed to judge the trust of complementary filtering to gyroscope,and the adaptive factor of complementary filtering is dynamically adjusted according to the trust,and the problem of course angle divergence is solved by CKF with magnetometer and gyroscope.Experimental results show that the proposed algorithm can achieve the attitude solution quickly and accurate-ly under both static and dynamic conditions.In the dynamic vehicle experiment,the accuracy of roll angle and pitch angle increases by 24.5%and 63.2%respectively,and the heading angle increases by 48.8%,which can guarantee the solution accuracy.