处理器读取陀螺仪加速度计数据后首先需要对数据进行滤波处理,此文分析比较几种常用的滤波算法。
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;
- }
复制代码
水平静止放置然后拍击三次桌子
水平静止放置然后旋转晃动
滑动滤波大体上消除了静止时的微小抖动,平滑了波形,但是能看到少许的延时。
2.加速度计2阶低通滤波
|