四旋翼基础算法学习2-IMU输入滤波算法

前言：

处理器读取陀螺仪加速度计数据后首先需要对数据进行滤波处理，此文分析比较几种常用的滤波算法。

参考学习：四轴加速度计滤波

IMU：

IMU使用MPU9250（即MPU6500），设置加速度量程±8G，陀螺仪±2000dps，数字低通滤波设置42Hz。

IMU采集频率：Crazepony 100Hz（10ms），匿名小四 1000Hz（1ms），圆点博士小四333Hz（3ms）。本次测试使用250Hz（4ms）。

在从传感器读取的原始数据滤波之前，一般需要进行零偏校准。一般陀螺仪需要上电校准零偏，加计的零偏与IMU安装有关，校准一次以后上电始终使用校准值即可（或者不予校准）。零偏校准方法基本都是水平静止放置机体多次采样取均值。

三轴加速度三轴陀螺仪原始数据 –> 减去零偏 –> 数据滤波

加速度计滤波：

滤波方法有 滑动均值滤波，2阶低通滤波

使用匿名地面站显示加速度数据波形，滤波后数据由陀螺仪通道波形显示。

ACC_X 原始加速度x轴数据

ACC_Y 原始加速度y轴数据

ACC_Z 原始加速度z轴数据

GYRO_X 滤波后加速度x轴数据

GYRO_Y 滤波后加速度x轴数据

GYRO_Z 滤波后加速度z轴数据

1.加速度计滑动均值滤波

代码：

#define SAF_NUM 10

void Slide_Average_Filter(int16_t acc_x,int16_t acc_y,int16_t acc_z,int16_t *acc_out_x,int16_t *acc_out_y,int16_t *acc_out_z)
{
    static uint8_t filter_cnt=0,cnt=0;
    static int16_t filter_buffer[3][SAF_NUM];
    int32_t temp1=0,temp2=0,temp3=0;
    
    filter_buffer[0][filter_cnt]=acc_x;
    filter_buffer[1][filter_cnt]=acc_y;
    filter_buffer[2][filter_cnt]=acc_z;
    
    for(cnt=0;cnt<SAF_NUM;cnt++)
    {
        temp1 += filter_buffer[0][cnt];
        temp2 += filter_buffer[1][cnt];
        temp3 += filter_buffer[2][cnt];
    }
    *acc_out_x = temp1/SAF_NUM;
    *acc_out_y = temp2/SAF_NUM;
    *acc_out_z = temp3/SAF_NUM;
    
    filter_cnt++;
    if(filter_cnt==SAF_NUM)
        filter_cnt=0;
}

水平静止放置然后拍击三次桌子 下图

水平静止放置然后旋转晃动 下图

滑动滤波大体上消除了静止时的微小抖动，平滑了波形，但是能看到少许的延时。

2.加速度计2阶低通滤波