四轴基础算法学习——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
{
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;
}