改进扩展卡尔曼滤波的四旋翼姿态估计算法

摘要 为了提高标准扩展卡尔曼姿态估计算法的精确度和快速性，将运动加速度抑制的动态步长梯度下降算法融入扩展卡尔曼中，提出一种改进扩展卡尔曼的四旋翼姿态估计算法。该算法在卡尔曼测量更新中采用梯度下降法进行非线性观测，消除标准扩展卡尔曼算法在线性化时带来的线性化误差，提高算法的准确性和快速性；对梯度下降法的梯度步长进行动态处理，使算法步长与四旋翼飞行器的运动合角速度成正比，增强微型四旋翼飞行器姿态解算的动态性能；对强机动运动过程中机体产生的运动加速度进行抑制处理，消除运动加速度对姿态解算的不利影响，提高了微型四旋翼飞行器姿态解算的跟踪精度。为了验证所设计算法的可行性和有效性，基于STM32单片机搭建四旋翼实验平台系统进行实时在线性能验证。结果表明，所设计算法能提高四旋翼飞行器在强机动、高速运动情况下的姿态跟踪精度、动态性能，增强姿态融合算法的抗干扰性，保证微型四旋翼飞行器的稳定飞行