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

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

Improved extended Kalman filter for attitude estimation of quadrotor

In order to improve the rapidity and tracking accuracy of Extended Kalman Filter (EKF), an improved EKF for attitude estimation of quadrotor was proposed by introducing a dynamic step gradient descent algorithm with acceleration restraint. Gradient descent algorithm was used to carry out nonlinear observation in the Kalman measurement update, eliminate the linearity error caused by the linearization of the standard extended Kalman algorithm and improve the accuracy and rapidity of the algorithm. The gradient step of gradient descent algorithm was dynamically processed to be proportional to the angular velocity of the quadrotor, thus enhancing the dynamic performance of the quadrotor. The motion acceleration generated during strong maneuverability was restrained to remove the adverse effect to attitude calculation and improve tracking accuracy of quadrotor's attitude estimation. To verify the feasibility and effectiveness of proposed algorithm, a quadrotor experimental platform was set up based on STM32 microcontroller. The experimental results show that the proposed algorithm has higher estimation accuracy, better dynamic performance and anti-interference characteristics under strong maneuverability and high-speed motion, and can ensure the stable flight of the quadrotor.