Filter Algorithms

Posted on 2022-05-30 Edited on 2023-02-23 In Summary Waline:

本文结合网络资源对常见的几种滤波算法进行了总结整理。

滤波算法

1 限幅滤波法（程序判断滤波法）

1.1 方法

根据经验判断，确定两次采样允许的最大偏差值（设为 $A$ ）
每次检测到新值时判断：
如果 $\text{本次值与上次值之差}\le A$ ，则本次值有效；
如果 $\text{本次值与上次值之差}>A$ ，则本次值无效，放弃本次值，用上次值代替本次值。

1.2 优点

能有效克服因偶然因素引起的脉冲干扰

1.3 缺点

无法抑制那种周期性的干扰
平滑度差

1.4 代码

/*
A 值可根据实际情况调整
value 为有效值，new_value 为当前采样值
滤波程序返回有效的实际值
 */
#define A 10
char value

char filter(void) {
    char new_value;
    new_value = get_ad();
    if((new_value - value > A) || (value - new_value > A)) {
        return value;
    } else {
        return new_value;
    }
}

2 中位值滤波法

2.1 方法

连续采样 $N$ 次（ $N$ 取奇数）
把 $N$ 次采样值按大小排列
取中间值为本次有效值

2.2 优点

能有效克服因偶然因素引起的波动干扰
对温度、液位的变化缓慢的被测参数有良好的滤波效果

2.3 缺点

对流量、速度等快速变化的参数不宜

2.4 代码

/*
N 值可根据实际情况调整
排序采用冒泡法
 */
#define N 11

char filter(void) {
    char value_buf[N];
    char i, j, temp;
    for(i = 0; i < N; i++) {
        value_buf[i] = get_ad();
        delay();
    }
    for(j = 0; j < N - 1; j++) {
        for(i = 0; i < N - j; i++) {
            if(value_buf[i] > value_buf[i + 1]) {
                temp = value_buf[i];
                value_buf[i] = value_buf[i + 1];
                value_buf[i + 1] = temp;
            }
        }
    }
    return value_buf[(N - 1) / 2];
}

3 算术平均滤波法

3.1 方法

连续取 $N$ 个采样值进行算术平均运算
$N$ 值较大时：信号平滑度较高，但灵敏度较低
$N$ 值较小时：信号平滑度较低，但灵敏度较高
$N$ 值的选取：一般流量， $N=12$ ；压力： $N=4$

3.2 优点

适用于对一般具有随机干扰的信号进行滤波
这样信号的特点是有一个平均值，信号在某一数值范围附近上下波动

3.3 缺点

对于测量速度较慢或要求数据计算速度较快的实时控制不适用
比较浪费 RAM

3.4 代码

#define N 11

char filter(void) {
    int sum = 0, i = 0;
    for(i = 0; i < N; i++) {
        sum += get_ad();
        delay();
    }
    return (char)(sum / N);
}

4 递推平均滤波法

4.1 方法

把连续取 $N$ 个采样值看成一个队列
队列的长度固定为 $N$
每次采样到一个新数据放入队尾，并扔掉原来队首的一次数据（先进先出原则）
把队列中的 $N$ 个数据进行算术平均运算，就可获得新的滤波结果
$N$ 值的选取：流量， $N=12$ ；压力： $N=4$ ；液面， $N=4\sim12$ ；温度， $N=1\sim4$

4.2 优点

对周期性干扰有良好的抑制作用，平滑度高
适用于高频振荡的系统

4.3 缺点

灵敏度低
对偶然出现的脉冲性干扰的抑制作用较差
不易消除由于脉冲干扰所引起的采样值偏差
不适用于脉冲干扰比较严重的场合
比较浪费 RAM

4.4 代码

#define N 12

char value_buf[N];
char i = 0;

char filter(void) {
    char count = 0;
    int sum = 0;

    value_buf[i++] = get_ad();
    if(i == N) {
        i = 0;    //先进先出
    }
    for(count = 0; count < N; count++) {
        sum += value_buf[count];
    }
    return (char)(sum / N);
}

5 中位值平均滤波法（防脉冲干扰平均滤波法）

5.1 方法

相当于“中位值滤波法”+“算术平均滤波法”
连续采样 $N$ 个数据，去掉一个最大值和一个最小值
然后计算 $N-2$ 个数据的算术平均值
$N$ 值的选取： $3\sim14$

5.2 优点

融合了两种滤波法的优点
对于偶然出现的脉冲性干扰，可消除由于脉冲干扰所引起的采样值偏差

5.3 缺点

测量速度较慢，和算术平均滤波法一样
比较浪费 RAM

5.4 代码

#define N 12

char filter(void) {
    char i = 0, j = 0, temp = 0;
    char value_buf[N];
    int sum = 0;

    for(i = 0; i < N; i++){
        value_buf[i] = get_ad();
        delay();
    }

    for(j = 0; j < N - 1; j++) {
        for(i = 0; i < N - j; i++) {
            if(value_buf[i] > value_buf[i + 1]) {
                temp = value_buf[i];
                value_buf[i] = value_buf[i + 1];
                value_buf[i + 1] = temp;
            }
        }
    }
    for(i = 1; i < N - 1; i++) {
        sum += value_buf[i];
    }
    return (char)(sum / (N - 2));
}

6 限幅平均滤波法

6.1 方法

相当于“限幅滤波法”+“递推平均滤波法”
每次采样到的新数据先进行限幅处理
再送入队列进行递推平均滤波处理

6.2 优点

融合了两种滤波法的优点
对于偶然出现的脉冲性干扰，可消除由于脉冲干扰所引起的采样值偏差

6.3 缺点

比较浪费 RAM

6.4 代码

1
2
3

/*
结合程序 1 和 4
 */

7 一阶滞后滤波法

7.1 方法：

取 $a=[0,1]$
$\text{本次滤波结果}=(1-a)*\text{本次采样值}+a*\text{上次滤波结果}$

7.2 优点

对周期性干扰具有良好的抑制作用
适用于波动频率较高的场合

7.3 缺点

相位滞后，灵敏度低
滞后程度取决于 $a$ 值大小
不能消除滤波频率高于采样频率的 $1/2$ 的干扰信号

7.4 代码

/*
为加块程序处理速度，假定基数 a = 0 ~ 100
 */
char value;

char filter(void) {
    char new_value = 0;

    new_value = get_ad();
    return (a * value + (100 - a) * new_value) / 100;
}

8 加权递推平均滤波法

8.1 方法

是对递推平均滤波法的改进，即不同时刻的数据加以不同的权
通常是，越接近现时刻的数据，权取得越大
给予新采样值的权系数越大，则灵敏度越高，但信号平滑度越低

8.2 优点

适用于有较大纯滞后时间常数的对象
和采样周期较短的系统

8.3 缺点

对于纯滞后时间常数较小，采样周期较长，变化缓慢的信号
不能迅速反应系统当前所受干扰的严重程度，滤波效果差

8.4 代码

/*
coe数组为加权系数表
 */
#define N 12

char coe[N] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};
char sum_coe = 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12;

char filter(void) {
    char i = 0;
    char value_buf[N];
    int sum = 0;

    for(i = 0; i < N; i++) {
        value_buf[i] = get_ad();
        delay();
    }
    for(i = 0; i < N; i++) {
        sum += value_buf[i] * coe[i];
    }
    return (char)(sum / sum_coe);
}

9 消抖滤波法

9.1 方法

设置一个滤波计数器
将每次采样值与当前有效值比较：
如果 $\text{采样值}＝\text{当前有效值}$ ，则计数器清零
如果 $\text{采样值}\ne\text{当前有效值}$ ，则 $\text{计数器}+1$ ，并判断计数器是否 $\ge$ 上限 $N$ （溢出）
如果计数器溢出，则将本次值替换当前有效值，并清计数器

9.2 优点

对于变化缓慢的被测参数有较好的滤波效果
可避免在临界值附近控制器的反复开/关跳动或显示器上数值抖动

9.3 缺点

对于快速变化的参数不宜
如果在计数器溢出的那一次采样到的值恰好是干扰值,则会将干扰值当作有效值导入系统

9.4 代码

#define N 12

char filter(void) {
    char i = 0;
    char new_value = 0, value = 0;

    new_value = get_ad();

    while(value != new_value) {
        i++;
        if(i > N) {
            return new_value;
        }
        delay();
        new_value = get_ad();
    }
    return value;
}

10 限幅消抖滤波法

10.1 方法

相当于“限幅滤波法”+“消抖滤波法”
先限幅，后消抖

10.2 优点

继承了“限幅”和“消抖”的优点
改进了“消抖滤波法”中的某些缺陷，避免将干扰值导入系统

10.3 缺点

对于快速变化的参数不宜

10.4 代码

1
2
3

/*
参考程序 1 和 9
 */

11 滑动窗口滤波法

11.1 方法一

前提先要获得一组数据，大小排序去除明显无效的数据，然后指定一个宽度为4的滑块从做向右滑动，计算滑块最右端和最左端的差值，该差值应小于预设阈值（否则丢弃），找出差值最小的4个数据然后求平均输出。缺点：如果源数据由于（距离）某种原因，数据产生的速度变化较大。很长时间才来一个数据就不太合适
主控制器读取数据，并将数据进行从小到大排序，并删除溢出值，设置宽度为４的滑动窗口，从排序好的数据最前端，自左向右进行滑动，并记录窗口尾部与头部的差值，保留差值最小的数据簇。此时，该组数据即为这组测量过程中，最集中的一组数据。将该差值与预先设定好的阈值进行比较，当满足阈值规定，则认为该组数据有效，若不满足，即表明该组数据离散程度太高，无法保证数据的有效性。设定滑动窗口大小为４，如果窗口大小过小，则无法保证数据是否处于有效值范围内，窗口大小过大，增大了对原始数据的要求，特别是当测量环境不佳的情况下，有效值本身的数量较少，过大的窗口限制了测量的适用性。阈值设定的作用是防止数据过于离散，甚至可能出现所有数据都非有效值的情况，阈值设定过大，则容易导致误差较大，阈值设定过小，限制了测量的量程。窗口大小以及阈值大小的设定，通过实验经验设置，取得了较好的测量效果。该算法在常见的平均取值法上，增加了滑动窗口的概念，以及阈值比较的想法，与平均取值法相比，有了更坚实的理论基础。

11.2 方法二

维护一个奇数长度（5）的窗口，根据预测和众数（数据重复的次数或者误差在一定范围内出现的次数）的方法来决定输出那个数据，次数小于一半的不输出，即 5 个数据中最少有 2 个相同的才输出该值（或这三个值的平均值），不同的值一定要留下，5 个元素中离新值差异最大的值丢弃。

12 卡尔曼滤波法

12.1 状态估计

首先，对于一个我们关心的物理量，我们假设它符合下面的规律

$x_k=ax_{k-1}$

其中， $x_k$ 为该物理量本周期的实际值， $x_{k-1}$ 为该物理量上一个周期的实际值，当然这个物理量可能不符合这个规律，这里只是做了一个假设。下面我们再来看一下这个物理量的测量公式：

$z_k=v_k+x_k$

其中， $z_k$ 是这个物理量的测量值， $v_k$ 是测量噪声。这个公式体现的是实际值与测量值的关系。实际中，物理量一般不会像我们上面的公式那样简单，一般我们用下面的公式来表示：

$x_k=ax_{k-1}+bu_{k}$

其中， $bu_k$ 代表了处理噪声，这个噪声是处理模型与实际情况的差异，比如车速，他会受到人为加速、减速、路面不平等外界因素的影响。

卡尔曼滤波的基本思想是综合利用上一次的状态和测量值来对物理量的状态进行预测估计。我们用 $\hat{x}_k$ 来表示 $x_k$ 的估计值，则有下面的公式：

$\hat{x}_k=\hat{x}_{k-1}+g_k(z_k-\hat{x}_{k-1})$

在这个公式中，综合利用了上一个周期的估计值和这个周期的测量值来对进行估计。其中， $g_k$ 叫做卡尔曼增益，这个公式与一阶滞后滤波很相似，只不过卡尔曼增益是会变的，每个周期都会更新，一阶滤波的系数则是固定值。考虑极端的情况来分析增益的作用，当 $g_k=0$ 时，增益为 $0$ ，这时，这表示我们这个周期的估计值与上个周期是相同的，不信任当前的测量值；当 $g_k=1$ 时，增益为 $1$ ，这 $z_k=\hat{x}_{k-1}$ 时，这表示我们这个周期的估计值与测量值是相同的，不信任上个周期的估计值。在实际应用时， $g_k$ 介于 $0\sim1$ 之间，它代表了对测量值的信任程度。

12.2 卡尔曼增益

我们通过下面两个公式来计算并在每个周期进行卡尔曼增益的迭代更新：

\begin{gather} g_k=p_{k-1}/(p_{k-1}+r)\\ p_k=(1-g_k)p_{k-1} \end{gather}

在上述公式中， $r$ 是测量噪声 $v_k$ 的平均值，测量噪声是符合高斯分布的，一般可以从传感器厂商那里获得测量噪声的均值，如果无法获得可以根据采集到的数据给出一个经验值。 $r$ 的大小对最终滤波效果的影响是比较大的。 $p_k$ 为本周期的预测误差。我们采用分析卡尔曼增益的方法来分析预测误差的作用，即采用假设极端情况的方法。假设前一次的预测误差 $p_{k-1}=0$ ，根据第一个公式则 $g_k=0$ ，根据上面的分析，这种情况估计值为上个周期的估计值；如果前一次的预测误差 $p_{k-1}=1$ ，则增益变为 $g_k=1/(1+r)$ ， $r$ 一般取值很小，所以 $g_k\approx1$ ，这种情况以新测量的值作为估计值。

对于第二个公式，当卡尔曼增益为 $0$ 时， $p_k=p_{k-1}$ ，即采用上一个周期的预测误差；当增益为1时， $p_k=0$ 。

12.3 卡尔曼滤波算法

有了上面的推导，我们在下面列出来完成卡尔曼滤波的公式，卡尔曼滤波分为预测过程和更新过程两个过程，在公式中，我们又引入了缩放系数 $h$ ，和协方差 $q$ 。

预测过程

\begin{gather} \hat{x}_k=a\hat{x}_{k-1}+bu_k\\ p_k=ap_{k-1}a+q \end{gather}

更新过程

\begin{gather} g_k=p_kh/(hp_kh+r)\\ \hat{x}_k=\hat{x}_{k-1}+g_k(z_k-h\hat{x}_{k-1})\\ p_k=(1-g_kh)p_k \end{gather}

上面的公式适合一维变量的卡尔曼滤波，将变量扩展到多维，用向量和矩阵替换上面的变量，就可以实现多维变量的卡尔曼滤波，下面的公式适用于多维变量：

预测过程

\begin{gather} \hat{x}_k=A\hat{x}_{k-1}+Bu_k\\ P_k=AP_{k-1}A^T+Q \end{gather}

更新过程

\begin{gather} G_k=P_kH(HP_kH+R)^{-1}\\ \hat{x}_k=\hat{x}_{k-1}+G_k(z_k-H\hat{x}_{k-1})\\ P_k=(1-G_kH)P_k \end{gather}

12.4 代码

typedef struct{
    float filterValue; //滤波后的值
    float kalmanGain; //Kalamn增益
    float A; //状态矩阵
    float H; //观测矩阵
    float Q; //状态矩阵的方差
    float R; //观测矩阵的方差
    float P; //预测误差
    float B;
    float u;
}KalmanInfo;

void Kalm::initKalmanFilter(KalmanInfo *info)
{
    info->A = 1;
    info->H = 1;
    info->P = 0.1;
    info->Q = 0.05;
    info->R = 0.1;
    info->B = 0.1;
    info->u = 0;
    info->filterValue = 0;
}

float Kalm::kalmanFilterFun(KalmanInfo *info, float new_value)
{
    float predictValue = info->A*info->filterValue+info->B*info->u; //计算预测值
    info->P = info->A*info->A*info->P + info->Q; //求协方差
    info->kalmanGain = info->P * info->H /(info->P * info->H * info->H + info->R); //计算卡尔曼增益
    info->filterValue = predictValue + (new_value - predictValue)*info->kalmanGain; //计算输出的值
    info->P = (1 - info->kalmanGain* info->H)*info->P; //更新协方差
    return info->filterValue;
}