第4章传感器数据预处理

第4章：传感器数据预处理——让原始数据“说人话”

各位同学，欢迎来到传感器融合的第一道关卡。

说实话，我刚入行那会儿，拿到IMU数据就直接往姿态解算算法里塞。结果呢？飞控抖得像筛糠，电机嗡嗡响，飞机根本稳不住。后来我才明白——传感器出来的原始数据，就像没经过训练的实习生，你得先教会它“规矩”。

这一章，我们就来聊聊数据预处理的几种基本功。说白了，就是给传感器信号“洗个澡”，把脏东西去掉，留下干净的部分。

4.1 为什么需要预处理？

你想想看，MPU6050输出的加速度计数据，是不是经常跳来跳去？稍微震动一下，数值就乱飘。陀螺仪呢？静态时明明没动，输出却慢慢漂移。

这就是传感器噪声和偏差在作怪。如果不处理，飞控会误以为飞机在剧烈运动，然后疯狂输出错误的控制量。

预处理的目标很明确：

去噪声：把高频抖动、随机毛刺滤掉
去漂移：把缓慢变化的偏差消除
保真度：不能把真实的运动信号也滤没了

核心原则：预处理不是越狠越好。滤得太干净，真实信号也丢了；滤得太少，噪声还在。这个度，得靠经验和调试来找。

4.2 低通滤波——专治高频噪声

低通滤波，名字听着唬人，其实道理很简单：让低频信号通过，挡住高频噪声。

加速度计对震动特别敏感。电机一转，高频振动就叠加到重力加速度上。这时候低通滤波就派上用场了。

4.2.1 一阶RC低通滤波（最常用）

代码实现极其简单，就一行：

// 一阶低通滤波
output = alpha * input + (1 - alpha) * output_prev;

其中 alpha 是滤波系数，范围0~1。alpha越小，滤波越强，但响应也越慢。

我在项目中遇到过一个问题：alpha设得太小，飞机快速倾斜时，加速度计数据跟不上，导致姿态估计滞后。后来我总结了一个经验公式：

// 根据采样频率和截止频率计算alpha
float dt = 1.0f / sample_rate;  // 采样周期
float RC = 1.0f / (2 * PI * cutoff_freq);  // 时间常数
float alpha = dt / (RC + dt);  // 滤波系数

采样频率	截止频率	alpha值	效果
100 Hz	5 Hz	0.048	强滤波，响应慢
100 Hz	20 Hz	0.167	中等滤波
1000 Hz	50 Hz	0.048	适合高采样率

我的习惯：陀螺仪用20~30Hz截止频率，加速度计用5~10Hz。因为加速度计对震动更敏感，需要更狠地滤。

4.3 高通滤波——专治低频漂移

高通滤波和低通正好相反：让高频信号通过，挡住低频漂移。

陀螺仪有个毛病：静态时输出不为零，会慢慢漂移。这个漂移是低频的，用高通滤波就能干掉。

// 一阶高通滤波
output = alpha * (output_prev + input - input_prev);
// 或者更简单的形式：
output = input - lowpass_output;  // 原始信号减去低通结果

嗯，这里要注意：高通滤波会引入相位超前，对实时性要求高的场合要小心。我曾在某个项目中，高通滤波参数设得太大，导致角速度信号超前太多，飞控误判为超调，结果越控越抖。

避坑指南：高通滤波的截止频率一般设0.1~1Hz。太高会把真实的运动信号也滤掉，太低又滤不掉漂移。我曾经把截止频率设到5Hz，结果飞机缓慢转弯时，陀螺仪数据被当成漂移滤掉了——飞控以为飞机没动，实际上已经转了90度。

4.4 滑动平均滤波——简单粗暴但有效

滑动平均滤波，就是把最近N个数据取平均。N越大，滤波越平滑，但延迟也越大。

// 滑动平均滤波（环形缓冲区实现）
#define WINDOW_SIZE 10
float buffer[WINDOW_SIZE];
int index = 0;
float sum = 0;

float moving_average(float new_data) {
    sum -= buffer[index];       // 去掉最旧的数据
    buffer[index] = new_data;   // 存入新数据
    sum += buffer[index];       // 更新总和
    index = (index + 1) % WINDOW_SIZE;
    return sum / WINDOW_SIZE;   // 返回平均值
}

说实话，这个滤波器我用的不多。为什么？因为它对脉冲噪声（比如突然的毛刺）很敏感。一个异常大的值，要等N个采样周期才能被完全移除。相比之下，中值滤波对付毛刺更拿手。

4.5 中值滤波——专治“野值”

中值滤波的原理很简单：把窗口内的数据排序，取中间那个值。

它对付传感器偶尔出现的“野值”（比如静电干扰导致的跳变）特别有效。我曾在无人机降落时，超声波传感器突然跳出一个2米的值（实际高度只有0.5米），要不是中值滤波挡着，飞控直接以为飞机飞高了，然后猛加油门——后果不堪设想。

// 中值滤波（窗口大小3，最简单的情况）
float median_filter_3(float a, float b, float c) {
    // 排序取中间值
    if (a > b) { float t = a; a = b; b = t; }
    if (b > c) { float t = b; b = c; c = t; }
    if (a > b) { float t = a; a = b; b = t; }
    return b;  // 中间值
}

我的建议：窗口大小一般取3~7。太大计算量上去了，而且会丢失细节。在STM32F4上，窗口5的中值滤波大概耗时2~3微秒，完全可以接受。

4.6 卡尔曼滤波入门——从“猜”到“准”

前面几种滤波都是“静态”的——它们不知道传感器的物理模型。卡尔曼滤波不一样，它会“猜”下一秒的状态，然后用测量值来修正。

说白了，卡尔曼滤波就两步：

预测：根据上一刻的状态，猜这一刻的状态
更新：用传感器测量值，修正猜测

举个最简单的例子——估计一维位置：

// 一维卡尔曼滤波（位置估计）
float x = 0;      // 状态估计（位置）
float P = 1;      // 估计误差协方差
float Q = 0.01;   // 过程噪声（模型不准的程度）
float R = 0.1;    // 测量噪声（传感器不准的程度）

float kalman_filter(float z) {  // z是测量值
    // 预测
    float x_pred = x;           // 位置不变（假设静止）
    float P_pred = P + Q;       // 误差增大
    
    // 更新
    float K = P_pred / (P_pred + R);  // 卡尔曼增益
    x = x_pred + K * (z - x_pred);    // 修正估计
    P = (1 - K) * P_pred;             // 更新误差
    
    return x;
}

你看，卡尔曼增益K是关键。K大，说明我更相信测量值；K小，说明我更相信预测值。Q和R就是调节这个信任度的旋钮。

入门要点：卡尔曼滤波不是万能药。它需要准确的模型，而且调参（Q和R）很考验经验。我建议初学者先从低通滤波开始，等理解了噪声特性，再上卡尔曼。

4.7 知识体系总览

下面这张图，帮你理清这几种滤波的关系：

4.8 如何选择？

说了这么多，到底用哪个？我个人的经验是：

陀螺仪：低通滤波（去高频噪声）+ 高通滤波（去低频漂移）
加速度计：低通滤波（去震动噪声），截止频率设低一些
磁力计：中值滤波（去偶尔的电磁干扰毛刺）
气压计：滑动平均滤波（平滑高度数据）
需要高精度估计：上卡尔曼滤波，但要做好调参的心理准备

重要提醒：滤波不是越多越好。串联多个滤波器会引入巨大的相位延迟。我见过有人把低通、滑动平均、中值串在一起用，结果数据延迟了200毫秒——飞控根本没法用。记住：能用一级解决的，绝不用两级。

好了，这一章的内容就到这里。数据预处理是传感器融合的基石，看似简单，但里面的坑不少。下一章我们会把这些滤波方法用在真正的IMU数据上，看看实际效果如何。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录