数字滤波算法（一）

数字滤波算法

算数平均滤波

比较简单的一种滤波算法。顾名思义，对多次采样值取平均，将平均值作为有效采样值。

#define SAMPLE_NUM	5

int imu_acc[SAMPLE_NUM];

int filter(void)
{
    int i, sum = 0;
    
    for(i = 0; i<SAMPLE_NUM; i++)
    {
        sum += imu_acc[i];
    }
    
    return sum / SAMPLE_NUM;
}

算法很简单，但是有很大的缺陷。当遇到数据异常，即采样值明显偏大或者偏小的时候，算数平均值会存在较大误差。对此，我们在滤波前先将最大值和最小值剔除。

int filter(void)
{
    int i, sum = 0, max = 0, min = 0;
    
    for(i = 0; i<SAMPLE_NUM; i++)
    {
        if(imu_acc[i] > max)
        {
            max = imu_acc[i];
        }
        else if(imu_acc[i] < min)
        {
            min = imu_acc[i];
        }
        sum += imu_acc[i];
    }
    sum -= (min + max);
    
    return sum / SAMPLE_NUM;
}

如果一次采样中出现多次数据异常，可以加大采样量，一秒钟之内采集10次变为采集20次，排序后去掉两个最大值和最小值，再对剩下的数求算数平均

滑动均值滤波

算数平均滤波需要多次采样才能得出一个有效值，适用于检测量变化较慢的场景，对于检测量变化较快的场景，表现为系统反应迟钝。

滑动均值滤波是将当前采样值与之前连续采样值进行平均，这样每次采样之后都可以得到有效输出。因为参与计算的历史采样值个数固定并且会不断更新，像滑块平移一样，所以叫滑动均值滤波。

一般来说新输出的数据权重比较大，所以要根据权重进行加权平均

#define SAMPLE_WINDOW_NUM	5
int imu_history_data[SAMPLE_WINDOW_NUM];//前SAMPLE_WINDOW_NUM-1个数据是最近的采样数据
int buff_init = 0;//数组填充标志位
int index = 0;//标志最新数据放入的位置
int factor[SAMPLE_WINDOW_NUM] = {1,2,3,4,5};//加权系数
int factor_sum = 15;//1+2+3+4+5
int filter(int current_sample_value)
{
    int sum = 0;
    int i;
    int factor_index = 0;
    if(buff_init == 0)					//history数组还没有填充完成
    {
        imu_history_data[index] = current_sample_value;
        index++;
        if(index >= SAMPLE_WINDOW_NUM-1)
        {
            buff_init = 1;
        }
    }
    else								//history数组填充完成，进行滑动滤波
    {
		imu_history_data[index] = current_sample_value;
        index++;
        if(index >= SAMPLE_WINDOW_NUM)
        {
            index = 0;
        }
    }
    factor_index = index;
    for(i = 0; i<SAMPLE_WINDOW_NUM; i++)
    {
        sum += imu_history_data[factor_index] * factor[i];
        factor_index++;
        if(factor_index >= SAMPLE_WINDOW_NUM)
        {
            factor_index = 0;
        }
    }
    return sum / factor_sum;
}