4、传感器数据预处理：滤波去噪（均值滤波、中值滤波、卡尔曼滤波入门）

各位同学，咱们今天聊点实在的。

传感器数据，说白了就是一堆带着噪声的原始信号。你想想看，一个激光雷达扫出来的点，或者一个IMU报出来的加速度值，要是直接拿去做轨迹纠偏，那结果肯定惨不忍睹。我刚开始做项目那会儿，就吃过这个亏——传感器数据没处理，直接扔给控制算法，结果机器人原地转圈，差点把实验台撞翻。

所以，滤波去噪是传感器融合的第一步，也是最重要的一步。今天咱们就讲三种最常用的方法：均值滤波、中值滤波，还有卡尔曼滤波的入门。

4.1 为什么需要滤波？

传感器噪声的来源很多。温度漂移、电磁干扰、机械振动，都会让数据变得不干净。你想想看，一个超声波传感器测距，如果旁边有台电机在转，读数可能跳来跳去，误差能到好几厘米。

滤波的目的，就是把这些噪声去掉，保留真实信号。嗯，这里要注意：滤波不是万能的，它只能抑制噪声，不能完全消除。我个人的习惯是，先搞清楚噪声的类型，再选滤波方法。

4.2 均值滤波：简单粗暴，但有效

均值滤波，说白了就是取平均值。你连续采N个数据，然后求个平均数，作为当前输出值。这个方法我最早是在做温度传感器时用的——一个热电偶测炉温，数据跳得厉害，用均值滤波一搞，曲线立马平滑了。

数学表达式：

y[k] = (x[k] + x[k-1] + ... + x[k-N+1]) / N

其中，N是窗口大小。N越大，平滑效果越好，但响应速度越慢。我建议你根据实际场景调这个参数。比如，机器人低速运动时，N可以设大一点；高速运动时，N要小一点，否则会滞后。

代码示例（C语言风格）：

#define WINDOW_SIZE 5
float buffer[WINDOW_SIZE] = {0};
int index = 0;

float mean_filter(float new_value) {
    float sum = 0;
    buffer[index] = new_value;
    index = (index + 1) % WINDOW_SIZE;
    
    for (int i = 0; i < WINDOW_SIZE; i++) {
        sum += buffer[i];
    }
    return sum / WINDOW_SIZE;
}

4.3 中值滤波：对付脉冲噪声的利器

中值滤波的思路更简单：把窗口内的数据排序，取中间那个值。为什么它能对付脉冲噪声？因为一个异常值要么排在最前面，要么排在最后面，永远取不到中间。

我记得有一次做激光雷达的障碍物检测，数据里经常出现「飞点」——就是突然跳出来一个很远的点。用均值滤波根本压不住，换成中值滤波，问题立马解决。

数学表达式：

y[k] = median(x[k], x[k-1], ..., x[k-N+1])

中值滤波的窗口大小一般取奇数，比如3、5、7。窗口越大，对脉冲噪声的抑制越强，但也会丢失更多细节。

代码示例：

#define WINDOW_SIZE 5
float buffer[WINDOW_SIZE];

float median_filter(float new_value) {
    // 滑动窗口更新
    for (int i = 0; i < WINDOW_SIZE - 1; i++) {
        buffer[i] = buffer[i+1];
    }
    buffer[WINDOW_SIZE - 1] = new_value;
    
    // 排序取中值（简单冒泡）
    float temp[WINDOW_SIZE];
    memcpy(temp, buffer, sizeof(temp));
    for (int i = 0; i < WINDOW_SIZE - 1; i++) {
        for (int j = 0; j < WINDOW_SIZE - i - 1; j++) {
            if (temp[j] > temp[j+1]) {
                float t = temp[j];
                temp[j] = temp[j+1];
                temp[j+1] = t;
            }
        }
    }
    return temp[WINDOW_SIZE / 2];
}

4.4 卡尔曼滤波入门：状态估计的王者

均值滤波和中值滤波，说白了都是「死板」的方法——它们不考虑系统的动态特性。卡尔曼滤波就不一样了，它用数学模型来预测状态，再用测量值来修正预测值。说白了，就是「预测+修正」的循环。

我第一次接触卡尔曼滤波是在做无人机姿态估计的时候。IMU的数据噪声大，加速度计和陀螺仪各有各的毛病。用卡尔曼滤波一融合，姿态角稳得像块石头。

核心思想：

• 预测：根据上一时刻的状态，预测当前时刻的状态。
• 更新：用当前测量值，修正预测值。

五个基本公式（一维情况）：

// 预测
x_pred = A * x_prev + B * u
P_pred = A * P_prev * A^T + Q

// 更新
K = P_pred * H^T / (H * P_pred * H^T + R)
x_curr = x_pred + K * (z - H * x_pred)
P_curr = (1 - K * H) * P_pred

别被公式吓到。咱们拆开来看：

• x：状态量（比如位置、速度）
• P：状态协方差（表示对状态估计的不确定性）
• Q：过程噪声协方差（模型的不确定性）
• R：测量噪声协方差（传感器的噪声水平）
• K：卡尔曼增益（决定你更相信预测还是测量）

一维卡尔曼滤波代码示例：

typedef struct {
    float x;  // 状态估计
    float P;  // 估计误差协方差
    float Q;  // 过程噪声协方差
    float R;  // 测量噪声协方差
} KalmanFilter;

void kalman_init(KalmanFilter *kf, float init_x, float init_P, float Q, float R) {
    kf->x = init_x;
    kf->P = init_P;
    kf->Q = Q;
    kf->R = R;
}

float kalman_update(KalmanFilter *kf, float z) {
    // 预测
    float x_pred = kf->x;  // 假设状态不变（A=1）
    float P_pred = kf->P + kf->Q;
    
    // 更新
    float K = P_pred / (P_pred + kf->R);
    kf->x = x_pred + K * (z - x_pred);
    kf->P = (1 - K) * P_pred;
    
    return kf->x;
}

4.5 三种滤波方法的对比

咱们用一张表来总结一下：

方法	适用噪声类型	计算量	响应速度	是否需要模型
均值滤波	高斯白噪声	低	慢（有滞后）	否
中值滤波	脉冲噪声	中	中等	否
卡尔曼滤波	各种噪声（需调参）	高	快（自适应）	是

我个人建议：新手先从均值滤波和中值滤波入手，把基础打牢。等你对传感器特性熟悉了，再上卡尔曼滤波。别一上来就搞卡尔曼，调参能调到你怀疑人生。

4.6 本章知识体系

下面这张图，帮你理清三种滤波方法的关系和适用场景：

好了，这一章的内容就到这里。滤波去噪是传感器融合的基础，你把这三种方法吃透了，后面的轨迹纠偏才能玩得转。记住，没有最好的滤波方法，只有最合适的。多动手调参，多观察效果，慢慢你就有感觉了。