Arduino传感器滤波算法

1.开发环境

ESP-12F、Arduino、INA226传感器检测电压

2.滑动均值滤波

// 滑动均值滤波
float slideFilter(const float Z)
{
    static const int N = 8;
    static float filterBuff[N];
    static int num = 0;
    static float sum = 0.0;

    if (num < N)
    {
        filterBuff[num] = Z;
        sum += filterBuff[num];
        num++;
        return sum / num;
    }
    else
    {
        sum -= sum / N;
        sum += Z;
        return sum / N;
    }
}

3.一阶互补滤波

// 一阶互补滤波
float firstOrderFilter(const float Z)
{
    static const float alpha = 0.7;
    static float prevData = Z;
    float filterValue = Z * alpha + (1 - alpha) * prevData;
    prevData = filterValue;
    return filterValue;
}

4.卡尔曼滤波

// 卡尔曼滤波
float kalmanFilter(float Z)
{
    static float K = 0;            // 卡尔曼增益
    static float P = 1;            // 估计误差协方差
    static const float Q = 0.0025; // 过程噪声协方差，值增大，动态响应变快，收敛稳定性变差，  Q控制误差，R控制响应速度
    static const float R = 0.2;    // 测量噪声协方差，传感器产生的噪声，值增大，动态响应变慢，收敛稳定性变好
    static float prevData = 0;

    P = P + Q;
    K = P / (P + R);
    Z = prevData + K * (Z - prevData);
    P = (1.0 - K) * P;
    prevData = Z;
    return Z;
}

5.中值滤波 

// 中值滤波
float medianFilter(const float Z)
{
    static const int N = 9;
    static float filterBuff[N];
    for (int i = 0; i < N; i++)
    {
        filterBuff[i] = Z;
    }
    // 将采样值从小到大排序(冒泡排序算法)
    bubbleSort(filterBuff, N);
    return filterBuff[(N - 1) / 2];
}

// 冒泡排序
void bubbleSort(float arr[], int n)
{
    for (int i = 0; i < n - 1; i++)
    {
        bool flag = true; // 没有排序为true，本趟有排序为false
        for (int j = 0; j < n - 1 - i; j++)
        {
            if (arr[j] > arr[j + 1])
            {
                flag = false;
                float temp = arr[j];
                arr[j] = arr[j + 1];
                arr[j + 1] = temp;
            }
        }
        if (flag)
        {
            break;
        }
    }
}

6.完整程序

// 传感器滤波算法测试
#include <Arduino.h>
#include <INA226.h>
#include <Wire.h>

INA226 battery(0x44); // 对象实例化

void initINA226(); // 采样电阻设置0.01，最大电流8A
void ArduinoSheet();
void VofaDebug();

float slideFilter(const float Z);      // 滑动均值滤波算法
float firstOrderFilter(const float Z); // 一阶低通滤波
float kalmanFilter(float Z);           // 卡尔曼滤波
float medianFilter(const float Z);     // 中值滤波
void bubbleSort(float arr[], int n);   // 冒泡排序

int rowNumber = 0;
const int rowCount = 500; // 统计500个数据

void setup()
{
    Serial.begin(115200);
    Wire.begin();
    initINA226();
}

void loop()
{
    // // 记录传感器数据
    // ArduinoSheet();

    // 采用Vofa+进行调试
    VofaDebug();
}

void initINA226()
{
    battery.setMaxCurrentShunt(8, 0.01);
}

/*********************************************************************************************/
void ArduinoSheet()
{
    if (rowNumber < rowCount)
    {
        float initData = battery.getBusVoltage();
        float filterData1 = slideFilter(initData); // 滑动均值滤波
        Serial.print(rowNumber++);                 // 行号
        Serial.print(',');
        Serial.print(initData); // 原始数据
        Serial.print(',');
        Serial.println(filterData1);
    }
}

void VofaDebug()
{
    float initData = battery.getBusVoltage();
    float filterData1 = slideFilter(initData);      // 滑动均值滤波
    float filterData2 = firstOrderFilter(initData); // 一阶互补滤波
    float filterData3 = kalmanFilter(initData);     // 卡尔曼滤波
    float filterData4 = medianFilter(initData);     // 中值滤波
    Serial.printf("%f,%f,%f,%f,%f\n", initData, filterData1, filterData2, filterData3, filterData4);
}

/*********************************************************************************************/

// 滑动均值滤波
float slideFilter(const float Z)
{
    static const int N = 8;
    static float filterBuff[N];
    static int num = 0;
    static float sum = 0.0;

    if (num < N)
    {
        filterBuff[num] = Z;
        sum += filterBuff[num];
        num++;
        return sum / num;
    }
    else
    {
        sum -= sum / N;
        sum += Z;
        return sum / N;
    }
}

// 一阶互补滤波
float firstOrderFilter(const float Z)
{
    static const float alpha = 0.7;
    static float prevData = Z;
    float filterValue = Z * alpha + (1 - alpha) * prevData;
    prevData = filterValue;
    return filterValue;
}

// 卡尔曼滤波
float kalmanFilter(float Z)
{
    static float K = 0;            // 卡尔曼增益
    static float P = 1;            // 估计误差协方差
    static const float Q = 0.0025; // 过程噪声协方差，值增大，动态响应变快，收敛稳定性变差，  Q控制误差，R控制响应速度
    static const float R = 0.2;    // 测量噪声协方差，传感器产生的噪声，值增大，动态响应变慢，收敛稳定性变好
    static float prevData = 0;

    P = P + Q;
    K = P / (P + R);
    Z = prevData + K * (Z - prevData);
    P = (1.0 - K) * P;
    prevData = Z;
    return Z;
}

// 中值滤波
float medianFilter(const float Z)
{
    static const int N = 9;
    static float filterBuff[N];
    for (int i = 0; i < N; i++)
    {
        filterBuff[i] = Z;
    }
    // 将采样值从小到大排序(冒泡排序算法)
    bubbleSort(filterBuff, N);
    return filterBuff[(N - 1) / 2];
}

// 冒泡排序
void bubbleSort(float arr[], int n)
{
    for (int i = 0; i < n - 1; i++)
    {
        bool flag = true; // 没有排序为true，本趟有排序为false
        for (int j = 0; j < n - 1 - i; j++)
        {
            if (arr[j] > arr[j + 1])
            {
                flag = false;
                float temp = arr[j];
                arr[j] = arr[j + 1];
                arr[j + 1] = temp;
            }
        }
        if (flag)
        {
            break;
        }
    }
}