前言
之前的一次推送介绍了Mahony姿态解算算法(IMU姿态滤波算法——Mahony算法:原理与代码),这次介绍另一个经典的滤波算法:Madgwick滤波。
Madgwick滤波算法根据加速度计、陀螺仪、以及磁力计,融合计算机体四元数,计算速度快、精度较高。本文详细介绍六轴融合,即根据加速度计和陀螺仪数据,计算IMU的姿态。
算法
2.1 重力方向对齐优化
首先要指出的是,Madgwick算法假设加速度计测量的加速度完全由重力提供,即物体本体运动产生的加速度可忽略不计。这一点和Mahony算法是一样的。
假设world系中一个向量在wo