四旋翼基础算法学习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-IMU输入滤波算法

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

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

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

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

上一篇:day06_雷神_面向对象初识


下一篇:mysql之创建数据库,创建数据表