嵌入式信号滤波实战：卡尔曼、滑动平均与异常值剔除的C语言实现与性能对比

1. 嵌入式信号滤波的工程挑战

在开发基于Cortex-M系列MCU的传感器数据采集系统时，我经常遇到这样的场景：温度传感器读数突然跳变20℃，加速度计数据出现周期性毛刺，压力传感器输出中混入50Hz工频干扰。这些噪声不仅影响数据显示，更会导致控制系统误动作。面对有限的CPU资源和实时性要求，选择合适的滤波算法就像在走钢丝——既要保证滤波效果，又不能拖垮系统性能。

记得去年做工业烤箱温度控制项目时，PID算法因为原始温度数据的波动不断输出剧烈震荡的控制信号，导致加热管频繁通断。后来在数据采集端加入滑动平均滤波，问题立刻得到改善。这个案例让我深刻认识到：嵌入式信号处理不是纯数学游戏，而是资源、效果、实时性的三角平衡。

三种典型噪声场景需要区别对待：

• 高斯噪声（如传感器热噪声）：呈现随机分布特性
• 脉冲噪声（如开关触点火花）：瞬时大幅偏离真实值
• 周期性干扰（如电源纹波）：有固定频率特征的噪声

在Cortex-M3/M4这类资源受限的MCU上，算法选择要考虑三个硬指标：

1. RAM消耗：静态数组 vs 动态内存
2. CPU负载：浮点运算量 vs 整数运算
3. 实时延迟：单次处理耗时必须小于采样周期

2. 卡尔曼滤波：动态系统的最优估计

2.1 一维卡尔曼的简化实现

很多教程把卡尔曼滤波讲得高深莫测，其实在嵌入式场景中可以大幅简化。针对温度这类缓慢变化量，我通常使用一维版本，将状态转移矩阵简化为1。这就好比用"惯性定律"预测温度——假设下一刻温度与当前相同。

这个简化版卡尔曼在Cortex-M4上仅需不到100个时钟周期，关键结构体设计如下：

c复制typedef struct {
    float Q;    // 过程噪声方差（预测可信度）
    float R;    // 测量噪声方差（传感器精度）
    float P;    // 估计误差协方差
    float K;    // 卡尔曼增益
    float x_hat;// 最优估计值
} Kalman1D;

实测中发现两个调参秘诀：

• Q/R比值决定信任倾向：当Q/R>1时更相信传感器，反之更相信预测
• 初始P值影响收敛速度：建议设为测量噪声量级的平方

2.2 内存与性能优化技巧

在STM32F407上实测发现，将float改为q15定点数后，运算速度提升3倍，且RAM消耗减少50%。以下是优化后的迭代函数：

c复制q15_t Kalman_Update(q15_t measurement) {
    // 预测阶段
    P_prior = P + Q;
    
    // 更新阶段
    K = P_prior / (P_prior + R);
    x_hat += K * (measurement - x_hat);
    P = (1 - K) * P_prior;
    
    return x_hat;
}

特别提醒：在温度检测等慢变系统中，可以每5次采样执行1次预测更新，能降低50%CPU占用。但运动追踪等快速系统必须每次更新。

3. 滑动平均：资源受限场景的首选

3.1 环形缓冲区实现方案

滑动平均滤波最吃内存的就是历史数据存储。在开源项目中看到过各种实现，但最实用的还是环形缓冲区方案。这个设计妙处在于用取模运算替代数据搬移，将O(n)操作降为O(1)：

c复制#define WINDOW_SIZE 8
static int32_t buffer[WINDOW_SIZE];
static uint8_t index = 0;

int32_t Moving_Average(int32_t new_val) {
    static int64_t sum = 0;
    
    sum -= buffer[index]; 
    sum += new_val;
    buffer[index] = new_val;
    index = (index + 1) % WINDOW_SIZE;
    
    return sum / WINDOW_SIZE;
}

在Cortex-M0+这种没有硬件除法的芯片上，我改用窗口大小为2^n，这样可以用右移替代除法。比如窗口大小取8时，最后一句改为return sum >> 3;，速度提升7倍。

3.2 动态窗口调节策略

固定窗口大小常面临两难：大窗口滤波好但延迟高，小窗口响应快但噪声大。后来我实现了一个自适应方案：

c复制uint8_t dynamic_window(float noise_level) {
    if(noise_level > 0.5f) return 16;
    else if(noise_level > 0.2f) return 8;
    else return 4;
}

配合噪声方差估计模块，系统能在强干扰时自动加大滤波力度，环境干净时减小延迟。实测在无人机高度检测中，这种动态策略比固定窗口减少40%的延迟。

4. 异常值剔除：脉冲噪声克星

4.1 中值滤波的快速实现

传统中值滤波需要全排序，时间复杂度O(nlogn)。在嵌入式场景中，我采用部分排序法，只需O(n)时间：

c复制int32_t Quick_Median(int32_t* arr, uint8_t size) {
    uint8_t i, j;
    for(i=0; i<(size/2+1); i++) {
        uint8_t min_idx = i;
        for(j=i+1; j<size; j++) {
            if(arr[j] < arr[min_idx]) min_idx = j;
        }
        swap(&arr[i], &arr[min_idx]);
    }
    return arr[size/2];
}

这个算法只排序到中值位置就停止，在窗口大小为5时，比标准排序快2.3倍。对于要求更高的场景，可以预先计算好排序索引，运行时直接查表。

4.2 复合滤波策略

单独使用中值滤波对高斯噪声效果有限。我开发了一种级联方案：

1. 第一级用中值滤波剔除脉冲
2. 第二级用滑动平均平滑高斯噪声
3. 第三级用卡尔曼预测趋势

c复制float Hybrid_Filter(float raw) {
    float stage1 = Median_Filter(raw);
    float stage2 = Moving_Average(stage1);
    return Kalman_Update(stage2);
}

在电机电流采样中，这种组合使信噪比提升15dB，而M4内核的CPU占用仅增加8%。

5. 三种算法的实战对比

5.1 性能测试数据

在STM32F407(168MHz)上实测结果：

5.2 滤波效果可视化分析

用示波器捕获的加速度计数据对比：

• 原始信号：峰值波动±0.5g，有明显50Hz干扰
• 滑动平均：平滑效果好，但相位延迟达20ms
• 卡尔曼滤波：实时跟踪真实趋势，延迟仅5ms
• 中值滤波：完美消除突发尖峰，但基线仍有波动

特别发现：当采样率超过1kHz时，滑动平均的延迟会成为系统瓶颈。这时改用卡尔曼+中值的组合方案，既能保证实时性，又能有效抑制噪声。

6. 工程实践中的坑与经验

6.1 浮点转定点优化

在M0内核上做温度滤波时，浮点运算直接导致CPU占用率飙到70%。后来改用Q格式定点数，性能立竿见影：

c复制typedef int32_t q15_t; // Q15.16格式

q15_t float_to_q15(float f) {
    return (q15_t)(f * 65536.0f);
}

float q15_to_float(q15_t q) {
    return (float)q / 65536.0f;
}

转换代价是损失约0.001%精度，但对大多数传感器足够。注意加减法直接运算，乘除需要额外移位：

c复制q15_t q15_mul(q15_t a, q15_t b) {
    return ((int64_t)a * b) >> 16;
}

6.2 多传感器融合技巧

在IMU数据处理中，单纯滤波还不够。我总结出三轴加速度计的联合滤波策略：

1. 先对各轴独立进行中值滤波
2. 计算矢量模：√(x²+y²+z²)
3. 用模值约束各轴滤波强度

当模值偏离1g时，自动增大滤波窗口。这个技巧有效解决了运动加速度与测量噪声的区分难题。