4、软件滤波算法实现：移动平均滤波、中值滤波、卡尔曼滤波在编码器信号处理中的应用、实时性对比与选型建议

编码器的原始信号，说白了就是一堆带毛刺的方波或正余弦波。电机一转，电磁干扰、机械振动、线缆耦合……各种噪声全往信号线上招呼。你要是直接把这种信号拿去换相，轻则抖动，重则直接失步。

所以，软件滤波是绕不开的一关。但滤波算法那么多，怎么选？我个人的习惯是：先看你的实时性预算，再看噪声类型，最后才定算法。别一上来就上卡尔曼，杀鸡用牛刀，还把自己MCU跑死。

4.1 移动平均滤波：简单粗暴，但够用

移动平均滤波的原理，说白了就是取最近N个采样值的平均数。你想想看，如果噪声是高频的、零均值的，多采几次一平均，噪声自然就抵消了。

代码实现也很直接：

// 移动平均滤波 - 循环队列实现
#define MA_WINDOW_SIZE 8

static int32_t ma_buffer[MA_WINDOW_SIZE];
static uint8_t ma_index = 0;
static int32_t ma_sum = 0;

int32_t moving_average_filter(int32_t new_sample) {
    ma_sum -= ma_buffer[ma_index];          // 去掉最旧的值
    ma_buffer[ma_index] = new_sample;       // 存入新值
    ma_sum += new_sample;                   // 累加
    ma_index = (ma_index + 1) % MA_WINDOW_SIZE;
    return ma_sum / MA_WINDOW_SIZE;         // 返回平均值
}

这里有个坑，我踩过。窗口大小N怎么选？N越大，平滑效果越好，但延迟也越大。对于编码器换相，我建议N取4或8。取4时延迟约1-2个采样周期，换相还能跟上；取16以上，相位滞后就明显了，高速时容易换相滞后。

4.2 中值滤波：专治脉冲噪声

移动平均滤波怕什么？怕那种突然冒出来的大脉冲。比如电机电刷打火，一下把信号拉到电源轨。平均滤波会把这次异常值带进结果，导致输出跳变。中值滤波就是干这个的——它取中间值，极端值直接被丢弃。

我曾在直流无刷电机的霍尔信号上用过中值滤波。霍尔信号容易受换相瞬间的电流突变干扰，产生窄脉冲。中值滤波窗口取3，一个周期内就能把毛刺削掉。

// 中值滤波 - 窗口大小为3
int32_t median_filter_3(int32_t a, int32_t b, int32_t c) {
    // 两两比较，找出中间值
    if (a > b) {
        int32_t tmp = a; a = b; b = tmp;
    }
    if (b > c) {
        int32_t tmp = b; b = c; c = tmp;
    }
    if (a > b) {
        int32_t tmp = a; a = b; b = tmp;
    }
    return b;  // 中间值
}

但要注意，中值滤波的窗口不能太大。窗口5以上，排序计算量就上来了，而且每次都要维护一个有序数组，内存开销也大。对于编码器信号，窗口3或5足够了。

4.3 卡尔曼滤波：降维打击，但别滥用

卡尔曼滤波，听起来高大上。它本质上是一个最优状态估计器，能融合预测值和观测值，给出最小方差估计。对于编码器信号，如果你需要从带噪声的测量中估计出真实位置和速度，卡尔曼滤波确实是最优解。

但我要泼盆冷水：卡尔曼滤波在编码器换相场景下，90%的情况是过度设计。为什么？因为换相只需要知道当前在哪个扇区，精度要求没那么高。而且卡尔曼滤波涉及矩阵运算，对MCU算力要求高。

不过，如果你在做伺服驱动，需要高精度位置环和速度环，那卡尔曼滤波就值得上了。下面是一个简化的一维卡尔曼滤波实现（只估计位置）：

// 一维卡尔曼滤波 - 位置估计
typedef struct {
    float x;      // 状态估计值
    float p;      // 估计误差协方差
    float q;      // 过程噪声协方差
    float r;      // 测量噪声协方差
} kalman_1d_t;

void kalman_init(kalman_1d_t *k, float init_x, float init_p, float q, float r) {
    k->x = init_x;
    k->p = init_p;
    k->q = q;
    k->r = r;
}

float kalman_update(kalman_1d_t *k, float measurement) {
    // 预测
    float p_pred = k->p + k->q;
    
    // 更新
    float k_gain = p_pred / (p_pred + k->r);
    k->x = k->x + k_gain * (measurement - k->x);
    k->p = (1 - k_gain) * p_pred;
    
    return k->x;
}

调参是关键。Q和R怎么设？我的经验是：先让电机静止，采集一组数据算方差，作为R的初始值。Q从0.001开始试，看响应速度和平滑度的平衡。

4.4 实时性对比与选型建议

三种算法在实时性上的差异很明显。我整理了一张表，方便你对比：

滤波算法	单次计算量	内存开销	延迟（采样周期）	适用噪声类型
移动平均滤波	O(1)（循环队列）	N个int32	N/2	高频随机噪声
中值滤波（窗口3）	O(1)（3次比较）	3个int32	1	脉冲噪声、毛刺
卡尔曼滤波（一维）	O(1)（约20次浮点运算）	4个float	0（实时估计）	高斯噪声、系统不确定性

选型建议，我按场景给你列出来：

• 普通BLDC方波驱动：移动平均滤波就够了。窗口4-8，延迟可接受，代码简单，不占资源。
• 霍尔信号有毛刺：加一级中值滤波（窗口3）预处理，再进移动平均。两级串联效果很好。
• 伺服驱动、高精度位置环：上卡尔曼滤波。但建议只在位置环和速度环用，换相扇区判断还是用原始信号或简单滤波后的信号。
• MCU资源紧张（如8位机）：只用移动平均滤波，窗口取4。别碰卡尔曼，浮点运算能把8位机跑死。

4.5 本章知识体系

下面这张图总结了三种滤波算法的核心逻辑和选型路径，你可以对照着理解：