Fusion融合算法使用 Madgwick AHRS

最近又研究起了陀螺仪，以前在电赛方面没有用到很好的角度融合算法，在算法中的四元数看的头疼，恍然间看见github上有MadgwickAHRS的融合算法，便下载下来，测试了一下我的6轴陀螺仪icm20608，效果还不错，记录一下。

下载库

下载Fusion库：

下载好后是一个压缩包，进行解压得到源文件。

导入文件

将解压好的文件中的Fusion导入项目中（C/C++是这样，python另行研究）：

在项目需要的地方添加头文件：

#include "Fusion/Fusion.h"

例子

话不多说直接上代码：

//头文件中定义
FusionAhrs ahrs;
FusionVector gyroscope,accelerometer;
FusionEuler euler;

//初始化变量
FusionAhrsInitialise(&ahrs);


//在陀螺仪采样结束后执行
gyroscope = {{gyro_x_act,gyro_y_act,gyro_z_act}};  //陀螺仪采样的数据
accelerometer = {{accel_x_act,accel_y_act,accel_z_act}};  //陀螺仪采样的数据
FusionAhrsUpdateNoMagnetometer(&ahrs,gyroscope,accelerometer,0.02f);//采样周期20ms
euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
pitch = euler.angle.pitch;
roll = euler.angle.roll;
yaw = euler.angle.yaw;

效果：

其中的pitch、roll非常的稳，yaw在缓慢增加，原因是没有地磁数据，9轴陀螺仪没有暂时不研究。