数据滤波算法

前言：之前分享了基于imu660ra的角速度环控制流程，但我们最好还要加上滤波。本篇以此为切入分享一下数据滤波算法，系统了解各种常用滤波算法对控制工程非常重要。

为什么要滤波角速度？

1.陀螺仪数据本身有噪声，小抖动（±1~2）就是正常的传感器噪声

2.在积分过程噪声会积累，时间长了，积分误差会越来越大（俗称“漂移”）

3.控制器会因为噪声导致抖动，如果直接用未滤波的角速度来 PID，控制会抖得很厉害

一阶低通滤波（指数滤波）

抑制高频噪声，防止积分发散

当前值 = α × 上次值 + (1 - α) × 本次值

简单易调，响应可调        有延迟，不适合检测突变

float gz_filtered = 0;
float alpha = 0.95f;  // 越接近1，滤波越强，反应越慢

float low_pass_filter(float input)
{
    gz_filtered = alpha * gz_filtered + (1 - alpha) * input;
    return gz_filtered;
}

我们可以在角速度积分前使用滤波

float yaw = 0;
float gz_filtered = 0;
float alpha = 0.95f;
float dt = 0.01f;

void update_yaw()
{
    float gz_raw = imu660ra_get_gyro_z_dps();
    gz_filtered = alpha * gz_filtered + (1 - alpha) * gz_raw;
    yaw += gz_filtered * dt;
}

一般好像推荐α = 0.90 ~ 0.98，这样加上滤波，就能抑制短时间噪声，让积分更平滑

滑动平均滤波

去随机噪声

最近 N 次数据求平均

平滑效果好        突变时反应慢

#define FILTER_SIZE 5
float moving_average_filter(float input)
{
    static float buffer[FILTER_SIZE] = {0};
    static int index = 0;
    float sum = 0;

    buffer[index++] = input;
    if (index >= FILTER_SIZE) index = 0;

    for (int i = 0; i < FILTER_SIZE; i++) 
        sum += buffer[i];

    return sum / FILTER_SIZE;
}

N越大，更平滑，响应更慢

一阶高通滤波

突变检测、去直流偏

当前输出 = α × (上次输出 + 当前输入 - 上次输入)

突变响应敏感        容易放大噪声

float high_pass_filter(float input, float alpha)
{
    static float output = 0;
    static float last_input = 0;

    output = alpha * (output + input - last_input);
    last_input = input;
    return output;
}

imu场景α推荐0.9~0.99

α 越大，只剩非常快的变化；α 越小，可以检测到比较慢的变化

中值滤波

抑制突发脉冲干扰

排序后取中位数

抗尖峰干扰极好        算法复杂（排序），延迟大

float median3_filter(float input)
{
    static float buffer[3] = {0};
    buffer[0] = buffer[1];
    buffer[1] = buffer[2];
    buffer[2] = input;
    // 冒泡排序
    float temp;
    for (int i = 0; i < 2; i++)
    {
        for (int j = 0; j < 2 - i; j++)
        {
            if (buffer[j] > buffer[j + 1])
            {
                temp = buffer[j];
                buffer[j] = buffer[j + 1];
                buffer[j + 1] = temp;
            }
        }
    }
    return buffer[1];  // 中值
}

imu推荐3点

均值中值混合滤波

抑制强干扰、尖峰脉冲

均值+中值取舍

抗突变能力强

//中值 + 均值
float mixed_median_mean_filter(float input)
{
    float median = median3_filter(input);
    return moving_average_filter(median);
}

先中值，再均值 —— 先去掉尖刺，再平滑

先均值，再中值 —— 先平滑，再去异常

抖动严重，增加均值窗口；尖刺多，增加中值窗口（通常不要大于5）

卡尔曼滤波

传感器融合，状态估计

预测 + 测量更新，动态调整滤波系数

精度高        复杂，需建模

float kalman_filter(float input)
{
    static float estimate = 0;
    static float error_estimate = 1;
    float error_measure = 2;
    float q = 0.05;  // 过程噪声
    float k;         // 卡尔曼增益

    // 更新估计误差
    error_estimate += q;

    // 计算卡尔曼增益
    k = error_estimate / (error_estimate + error_measure);

    // 更新估计值
    estimate = estimate + k * (input - estimate);

    // 更新估计误差
    error_estimate = (1 - k) * error_estimate;

    return estimate;
}

一维卡尔曼，简化版本（卡尔曼推荐直接用库，不建议手写） 

互补滤波

姿态估计

陀螺仪积分的高频信息 + 加速度的低频信息

简单的传感器融合，抑制漂移，快速响应        对陀螺仪积分要求高，误差无法完全消除

float complementary_filter(float gyro_angle, float acc_angle, float alpha)
{
    static float angle = 0;
    angle = alpha * (angle + gyro_angle) + (1 - alpha) * acc_angle;
    return angle;
}

α 大（0.98），更依赖陀螺仪，反应快；α 小（0.90）， 更依赖加速度，反应慢，抖动小 

我的建议 

• 控制环用低通 + 校准

• 变化检测用高通

• 防漂移用互补滤波