滤波算法

该算法主要用于消除尖脉冲，同时又有很好的阶跃响应能力。
尖脉冲干扰和阶跃响应是数据采集中的两个很矛盾的问题。往往为了消除尖脉冲，会损失阶跃响应能力，造成很大的延时。该算法在这个问题上有所改善。

实例场景：在很短的时间 T 内，连续测量某一电压值 n 次。得到一个集合 Vset = {v1, v2, … , vn};
假设前提：如果我们相信所用电压表正常，那么通过这一组数据应该能得到一个最接近真实值的结果 Vreal；并且理想情况下 Vreal ≈ v1, v2, … , vn；
实际情况：在集合 Vset 中，很可能出现若干个 Verror 与 Vreal 偏差太大。如果把这些值参与运算，会对结果造成很大的影响
算法思想：如果 Vset 中的数据真实可靠，那么一定存在一个子集 Vright ，满足 Vright 中的所有元素都相近。换句话说，如果 Vset 中的所有数据都千差万别，那么 Vset 所代表的这组测量数据就不可靠。
算法目标：找到上述 Vright 集合，对其所有元素求算术平均数。注意，这里的 Verror 不会被立即丢弃，它们有的将会参与到后续的计算中

下面的代码实现了上述思想；
1、理想情况下：输出结果是除尖脉冲以外的采样值的算术平均值
2、最糟糕的的情况下：输出结果是所有采样值的算术平均值

/*
* File: Filter_Statistics.h
* Date: 2016-10-09
* Author: Hy
*/

#ifndef _FILTER_STATISTICS_H
#define _FILTER_STATISTICS_H

// 队列元素结构体
typedef struct _FILTER_STATISTICS_ITEM
{
    u32 Value;     // 采样数值
    // 可靠度
    u32 Matrix  : 27; // 和其它元素之间的关系：投票名单；
        // 一个位表示一个元素：某一位为 1 ，表示这两个数据相近；为 0 ，表示相差很大
    u32 Counter : 5;  // 与此元素相似的元素的个数：票数
        // 也就是 Matrix 中 1 的个数
}FILTER_STATISTICS_ITEM, *PFILTER_STATISTICS_ITEM;

typedef struct _FILTER_STATISTICS_OBJECT
{
    PFILTER_STATISTICS_ITEM QueBody;       // 队列
    u8  QueLength;     // 队列长度（小于等于 27）
    u8  Probability;   // 概率的期望值（在 QueLength 次采样中，有 Probability 个是可靠的）
    u16 Deviation;     // 允许偏差（按误差类型不同表达的意义不同）
        // 绝对误差 - 两个采样值之间的差值小于此数，则被认定为相近关系； 
        // 相对误差 - 两个采样值之间的差值与此数之积小于较大的采样值，则被认定为相近关系
    u8  DeviationType; // 误差类型：0 - 绝对误差； 1 - 相对误差
    u8  Cursor;        // 游标的位置；
        // 游标指示哪一个采样值进入队列时间最早（最早的采样值将会被新的采样值替换）
    u8  Counter;       // 可靠数据的个数：入选数量
    u8  QueSize;       // 队列大小

    u32 CredibleSum;   // 可靠数据的和
    u32 SuspectSum;    // 不可靠数据的和
}FILTER_STATISTICS_OBJECT, *PFILTER_STATISTICS_OBJECT;

// 初始化滤波器对象
void Filter_Statistics_Init(PFILTER_STATISTICS_OBJECT pd);
// 滤波：输入一个采样数据，返回一个结果
u32  Filter_Statistics_Pass(PFILTER_STATISTICS_OBJECT pd, u32 i);
// 读取当前结果的可信度
u32  Filter_Statistics_Counter(PFILTER_STATISTICS_OBJECT pd);

#endif


/*
* File: Filter_Statistics.c
* Date: 2016-10-09
* Author: Hy
*/

#include "Filter_Statistics.h"

/*
* 初始化滤波器对象
*/
void Filter_Statistics_Init(PFILTER_STATISTICS_OBJECT pobj)
{
    u32 i;
// 队列长度限制
    if(pobj->QueLength > 27)
        pobj->QueLength = 27;
// 初始化队列
    for(i = 0; i < pobj->QueLength; i ++)
    {
        pobj->QueBody[i].Value = 0;
        pobj->QueBody[i].Matrix = 0x07FFFFFF;     // 所有的元素都是 0 ，因此都相似
        pobj->QueBody[i].Counter = pobj->QueLength; // 所有的元素都是 0 ，因此相似个数为 pobj->QueLength
    }
// 默认参数
    // 
    if(pobj->Probability > pobj->QueLength)
        pobj->Probability = pobj->QueLength;
    // 为了避免浮点数运算提高效率（STM32没有硬件浮点数单元），我一般用小单位，比如：毫伏、毫安、毫秒
    // 这里的 1000 就代表 1 了，既默认偏差为 1 （伏、安、秒）
    if(pobj->DeviationType && pobj->Deviation > 1000)
        pobj->Deviation = 1000;

// 内部寄存器
    pobj->Cursor = 0; // 
    pobj->Counter = pobj->QueLength;
    pobj->CredibleSum = 0;
    pobj->SuspectSum = 0;
    pobj->QueSize = 0;
}

/*
* 功能：滤波
* 输入：新的采样数据
* 输出：滤波后的结果
* 说明：该函数维护一个循环队列；新的数据会覆盖最早的数据
*/
u32 Filter_Statistics_Pass(PFILTER_STATISTICS_OBJECT pobj, u32 i)
{
    u32 dev, thr;
    FILTER_STATISTICS_ITEM fi = {0};
    fi.Value = i;

// 统计新采样值、将被替换的采样值与其它采样值之间的相似度
    for(i = 0; i < pobj->QueLength; i ++)
    {
        // 旧采样值与其它采样值之间的相似
        if(pobj->QueBody[pobj->Cursor].Matrix & (1 << i))
        {
            // 在 i 的名单上去掉 Cursor 
            pobj->QueBody[i].Matrix &= (~(1 << pobj->Cursor));
            // 与编号为 i 的采样值相近的值少了一个
            pobj->QueBody[i].Counter --;
            // 票数减少导致出局（i 变得不可信）
            if(pobj->QueBody[i].Counter + 1 == pobj->Probability)
            {
                // 整体可信度降低
                pobj->Counter --;
                // 
                pobj->CredibleSum -= pobj->QueBody[i].Value;
                pobj->SuspectSum += pobj->QueBody[i].Value;
            }
        }

        // 计算新采样值与其它采样值之间的偏差
        if(pobj->QueBody[i].Value > fi.Value)
        {
            thr = pobj->QueBody[i].Value;
            dev = (thr - fi.Value);
        }
        else
        {
            thr = fi.Value;
            dev = (thr - pobj->QueBody[i].Value);
        }
        // 初始化时，误差类型为“相对误差”
        if(pobj->DeviationType)
            dev *= pobj->Deviation;
        // 初始化时，误差类型为“绝对误差”
        else
            thr = pobj->Deviation;
        // 两个采样值之间的差值在允许误差范围内
        if(dev < thr)
        {
            // 将采样值 i 记录到新采样值的名单中
            fi.Matrix |= (1 << i);
            // 新采样值可信度增加
            fi.Counter ++;
            // 将新采样值记录到 i 的名单中
            pobj->QueBody[i].Matrix |= (1 << pobj->Cursor);
            // 采样值 i 的可信度增加
            pobj->QueBody[i].Counter ++;
            // 票数增加导致入局
            if(pobj->QueBody[i].Counter == pobj->Probability)
            {
                // 整体可信度增加
                pobj->Counter ++;
                pobj->CredibleSum += pobj->QueBody[i].Value;
                pobj->SuspectSum -= pobj->QueBody[i].Value;
            }
        }
    }

// 清理旧采样值
    // 旧采样值是可信的
    if(pobj->QueBody[pobj->Cursor].Counter >= pobj->Probability)
    {
        // 整体可信度减小
        pobj->Counter --;
        pobj->CredibleSum -= pobj->QueBody[pobj->Cursor].Value;
    }
    else
        pobj->SuspectSum -= pobj->QueBody[pobj->Cursor].Value;

// 新采样值
    // 新采样值可信度较高
    if(fi.Counter >= pobj->Probability)
    {
        // 整体可信度增加
        pobj->Counter ++;
        pobj->CredibleSum += fi.Value;
    }
    else
        pobj->SuspectSum += fi.Value;

    // 
    pobj->QueBody[pobj->Cursor] = fi;

// 游标、计数器
    pobj->Cursor ++; // 后面的一个采样值将变成最老的值，在下一次将会被替换
    if(pobj->Cursor == pobj->QueLength)
        pobj->Cursor = 0;

// 结果
    if(pobj->QueSize == pobj->QueLength)
    {
        // 有可靠数据存在；求可靠数据的平均数
        if(pobj->Counter)
            return pobj->CredibleSum / pobj->Counter;
        // 所有数据都不太可靠；求所有数据的平均值
        else
            return pobj->SuspectSum / pobj->QueLength;
    }
    else
    {
        pobj->QueSize ++;
        return (pobj->CredibleSum + pobj->SuspectSum) / pobj->QueSize;
    }
}

/*
* 读取当前结果的可信度
*/
u32  Filter_Statistics_Counter(PFILTER_STATISTICS_OBJECT pobj)
{
    return pobj->Counter;
}

// ----------------- END -----------------------------------------------

使用实例：

#define FILTER_VOLTAGE_QUELENGTH 10
#define FILTER_VOLTAGE_DEVIATION 100
#define FILTER_VOLTAGE_PROBABILITY 5

// 定义队列
FILTER_STATISTICS_ITEM   l_fi_Voltage[FILTER_VOLTAGE_QUELENGTH] = {0};
// 定义滤波对象
FILTER_STATISTICS_OBJECT l_fo_Voltage = {0};

// 初始化
l_fo_Voltage.QueBody = l_fi_Voltage;
l_fo_Voltage.QueLength   = FILTER_VOLTAGE_QUELENGTH;
l_fo_Voltage.Deviation   = FILTER_VOLTAGE_DEVIATION; // 40mA
l_fo_Voltage.Probability = FILTER_VOLTAGE_PROBABILITY; // 40%
Filter_Statistics_Init((PFILTER_STATISTICS_OBJECT)(&l_fo_Voltage));
// 
u32 l_Voltage_Sample; // 原始值
u32 l_Voltage_Filter; // 滤波后的值
// 周期性地调用滤波函数
l_Voltage_Sample = ADC_GET_ANALOG(ADC_VOLTAGE); // 读取原始采样值
l_Voltage_Filter = Filter_Statistics_Pass((PFILTER_STATISTICS_OBJECT)(&l_fo_Voltage), l_Voltage_Sample); // 滤波

延伸探讨：
关于滤波，一般都是单通道多采样几次然后进行分析，这样总会有延迟；高速滤波可采用多个 ADC 通道同时采集同一个电压然后放入滤波器。