嵌入式系统滤波算法大全：从原理到代码实现

在嵌入式系统开发中，滤波算法是处理传感器数据、抑制噪声、提高系统稳定性的关键技术。本文将全面介绍嵌入式开发中常用的滤波算法，包括PID控制、卡尔曼滤波、一阶/二阶滤波等，提供可直接调用的通用代码实现，并详细分析各种算法的适用场景和核心区别。

一、基础滤波算法

1. 移动平均滤波（Moving Average Filter）

适用场景：适用于缓慢变化的信号，如温度、压力等环境参数的平滑处理。能有效抑制随机噪声，但对脉冲干扰和快速变化信号响应滞后。

代码实现：

#define WINDOW_SIZE 10

typedef struct {
            
    float buffer[WINDOW_SIZE];
    uint8_t index;
    float sum;
} MovingAverageFilter;

float moving_average_update(MovingAverageFilter* filter, float new_value) {
            
    // 减去即将被替换的旧值
    filter->sum -= filter->buffer[filter->index];
    // 添加新值并更新总和
    filter->sum += new_value;
    filter->buffer[filter->index] = new_value;
    filter->index = (filter->index + 1) % WINDOW_SIZE;
    
    return filter->sum / WINDOW_SIZE;
}

// 初始化函数
void moving_average_init(MovingAverageFilter* filter) {
            
    memset(filter->buffer, 0, sizeof(filter->buffer));
    filter->index = 0;
    filter->sum = 0;
}

特点分析：

优点：实现简单，计算量小，适合资源有限的嵌入式系统
缺点：引入滞后，窗口越大平滑效果越好但滞后越明显
变体：加权移动平均（给近期数据更高权重）

2. 中值滤波（Median Filter）

适用场景：有效去除脉冲干扰（如开关噪声、传感器偶发错误），适用于存在突发异常值的信号处理，如按键检测、工业设备状态监测等。

代码实现：

#define MEDIAN_WINDOW 5 // 建议使用奇数

float median_filter(float* input) {
            
    float sorted[MEDIAN_WINDOW];
    memcpy(sorted, input, sizeof(sorted));
    
    // 冒泡排序
    for(int i = 0; i < MEDIAN_WINDOW-1; i++) {
            
        for(int j = 0; j < MEDIAN_WINDOW-i-1; j++) {
            
            if(sorted[j] > sorted[j+1]) {
            
                float temp = sorted[j];
                sorted[j] = sorted[j+1];
                sorted[j+1] = temp;
            }
        }
    }
    
    return sorted[MEDIAN_WINDOW/2]; // 返回中值
}

// 使用示例
float sensor_readings[MEDIAN_WINDOW] = {
            23.4, 22.8, 130.5, 23.1, 22.9}; // 假设130.5是异常值
float valid_value = median_filter(sensor_readings); // 返回23.1

优化技巧：

对于实时系统，可采用更高效的排序算法（如插入排序）
结合移动平均滤波使用，先中值去脉冲再平均平滑

3. 限幅滤波（Limit Filter）

适用场景：已知信号变化速率的场景，如电机转速监测、缓慢变化的物理量测量。可防止因干扰导致的数值突变。

代码实现：

typedef struct {
            
    float last_value;
    float max_delta;
} LimitFilter;

float limit_filter_update(LimitFilter* filter, float new_value) {
            
    float delta = new_value - filter->last_value;
    
    if(fabsf(delta) > filter->max_delta) {
            
        // 变化超过阈值，返回上次有效值
        return filter->last_value;
    } else {
            
        // 变化在允许范围内，更新并返回新值
        filter->last_value = new_value;
        return new_value;
    }
}

// 初始化
void limit_filter_init(LimitFilter* filter, float max_delta) {
            
    filter->last_value = 0;
    filter->max_delta = max_delta;
}

参数设置：

max_delta根据信号特性确定，如温度可能设为1°C/s
可动态调整阈值以适应不同工况

二、进阶滤波算法

1. 一阶滞后滤波（低通滤波）

适用场景：信号本身变化缓慢但含有高频噪声，如电源电压监测、去除ADC采集的高频噪声。相当于软件实现的RC低通滤波器。

原理：y[n] = α·x[n] + (1-α)·y[n-1]，其中α=滤波系数(0<α<1)

代码实现：

typedef struct {
            
    float filtered_value;
    float alpha; // 滤波系数，越小平滑效果越强
} FirstOrderFilter;

float first_order_update(FirstOrderFilter* filter, float new_value) {
            
    filter->filtered_value = filter->alpha * new_value + 
                            (1 - filter->alpha) * filter->filtered_value;
    return filter->filtered_value;
}

void first_order_init(FirstOrderFilter* filter, float alpha, float init_value) {
            
    filter->alpha = alpha;
    filter->filtered_value = init_value;
}

// 使用示例
FirstOrderFilter temp_filter;
first_order_init(&temp_filter, 0.1, 25.0); // α=0.1，初始温度25°C
float current_temp = first_order_update(&temp_filter, read_temperature());

参数选择：

α = T/(T+τ)，T=采样周期，τ=滤波器时间常数
经验值：快速响应取0.3-0.5，强滤波取0.1-0.2
可通过阶跃响应测试调整

2. 二阶IIR低通滤波

适用场景：需要更陡峭的滤波特性时，如音频信号处理、生物电信号采集等。相比一阶滤波器能提供更好的高频衰减。

差分方程：y[n] = a0·x[n] + a1·x[n-1] + a2·x[n-2] - b1·y[n-1] - b2·y[n-2]

代码实现（Butterworth低通）：

typedef struct {
            
    float a0, a1, a2, b1, b2; // 滤波器系数
    float x_1, x_2; // 前两次输入
    float y_1, y_2; // 前两次输出
} SecondOrderLowPass;

float second_order_lpf_update(SecondOrderLowPass* filter, float input) {
            
    float output = filter->a0 * input + 
                  filter->a1 * filter->x_1 + 
                  filter->a2 * filter->x_2 - 
                  filter->b1 * filter->y_1 - 
                  filter->b2 * filter->y_2;
    
    // 更新历史状态
    filter->x_2 = filter->x_1;
    filter->x_1 = input;
    filter->y_2 = filter->y_1;
    filter->y_1 = output;
    
    return output;
}

void second_order_lpf_init(SecondOrderLowPass* filter, float cutoff_freq, float sample_freq) {
            
    // Butterworth二阶低通系数计算
    float omega = 2 * 3.1415926 * cutoff_freq / sample_freq;
    float sn = sinf(omega);
    float cs = cosf(omega);
    float alpha = sn / (2 * 0.7071); // 阻尼比0.7071
    
    float a0 = 1 / (1 + alpha);
    filter->a0 = a0 * (1 - cs) / 2;
    filter->a1 = a0 * (1 - cs);
    filter->a2 = filter->a0;
    filter->b1 = a0 * (-2 * cs);
    filter->b2 = a0 * (1 - alpha);
    
    // 初始化状态
    filter->x_1 = filter->x_2 = 0;
    filter->y_1 = filter->y_2 = 0;
}

设计要点：

截止频率应低于采样频率的1/2（奈奎斯特频率）
系数计算可使用在线工具或MATLAB生成
注意防止运算溢出（适合使用浮点MCU）

3. 卡尔曼滤波（Kalman Filter）

适用场景：存在系统模型且需要最优估计的场景，如导航定位、运动追踪、传感器融合等。适用于高斯分布噪声环境。

代码实现（简化版单变量）：

typedef struct {
            
    float q; // 过程噪声协方差
    float r; // 测量噪声协方差
    float x; // 状态估计值
    float p; // 估计误差协方差
    float k; // 卡尔曼增益
} KalmanFilter;

float kalman_update(KalmanFilter* kf, float measurement) {
            
    // 预测步骤
    kf->p = kf->p + kf->q;
    
    // 更新步骤
    kf->k = kf->p / (kf->p + kf->r);
    kf->x = kf->x + kf->k * (measurement - kf->x);
    kf->p = (1 - kf->k) * kf->p;
    
    return kf->x;
}

void kalman_init(KalmanFilter* kf, float q, float r, float initial_value) {
            
    kf->q = q;
    kf->r = r;
    kf->x = initial_value;
    kf->p = 1.0f; // 初始估计误差
    kf->k = 0;
}

参数调校：

q：过程噪声，反映模型不确定性，值越大滤波器响应越快
r：测量噪声，反映传感器精度，值越大越不相信测量值
可通过实验或系统辨识确定参数

扩展应用：

多变量卡尔曼滤波（需矩阵运算）
扩展卡尔曼滤波(EKF)处理非线性系统
无迹卡尔曼滤波(UKF)用于强非线性系统

三、复合滤波算法

1. 限幅平均滤波

适用场景：存在偶发脉冲干扰但主要需要平滑处理的信号，如工业现场的压力、流量信号。

实现原理：先限幅去除异常值，再平均滤波平滑信号

代码实现：

#define AVG_WINDOW 8

typedef struct {
            
    float last_value;
    float max_delta;
    float buffer[AVG_WINDOW];
    uint8_t index;
} LimitAvgFilter;

float limit_avg_filter_update(LimitAvgFilter* filter, float new_value) {
            
    // 限幅处理
    float delta = new_value - filter->last_value;
    if(fabsf(delta) > filter->max_delta) {
            
        new_value = filter->last_value;
    }
    
    // 更新环形缓冲区
    filter->buffer[filter->index] = new_value;
    filter->index = (filter->index + 1) % AVG_WINDOW;
    
    // 计算平均值
    float sum = 0;
    for(int i = 0; i < AVG_WINDOW; i++) {
            
        sum += filter->buffer[i];
    }
    
    filter->last_value = sum / AVG_WINDOW;
    return filter->last_value;
}

2. 中位值平均滤波

适用场景：高精度测量中去除脉冲干扰和随机噪声，如精密仪器仪表。

实现原理：采样N个值，去掉最大最小后求平均

代码实现：

#define SAMPLE_SIZE 5 // 建议5-15

float median_avg_filter(float* samples) {
            
    float sorted[SAMPLE_SIZE];
    memcpy(sorted, samples, sizeof(sorted));
    
    // 排序找出最大最小
    for(int i = 0; i < SAMPLE_SIZE-1; i++) {
            
        for(int j = 0; j < SAMPLE_SIZE-i-1; j++) {
            
            if(sorted[j] > sorted[j+1]) {
            
                float temp = sorted[j];
                sorted[j] = sorted[j+1];
                sorted[j+1] = temp;
            }
        }
    }
    
    // 去掉头尾求平均
    float sum = 0;
    for(int i = 1; i < SAMPLE_SIZE-1; i++) {
            
        sum += sorted[i];
    }
    
    return sum / (SAMPLE_SIZE-2);
}

四、控制类算法

1. PID控制算法

适用场景：需要闭环控制的系统，如电机速度控制、温度控制、位置伺服等。

代码实现：

typedef struct {
            
    float kp, ki, kd;       // PID系数
    float integral;         // 积分项
    float prev_error;       // 上次误差
    float integral_limit;   // 积分限幅
    float output_limit;     // 输出限幅
} PIDController;

float pid_update(PIDController* pid, float setpoint, float measurement, float dt) {
            
    float error = setpoint - measurement;
    
    // 比例项
    float p_term = pid->kp * error;
    
    // 积分项（抗饱和处理）
    pid->integral += error * dt;
    if(pid->integral > pid->integral_limit) pid->integral = pid->integral_limit;
    if(pid->integral < -pid->integral_limit) pid->integral = -pid->integral_limit;
    float i_term = pid->ki * pid->integral;
    
    // 微分项（避免设定值突变导致微分冲击）
    float derivative = (error - pid->prev_error) / dt;
    float d_term = pid->kd * derivative;
    pid->prev_error = error;
    
    // 计算输出并限幅
    float output = p_term + i_term + d_term;
    if(output > pid->output_limit) output = pid->output_limit;
    if(output < -pid->output_limit) output = -pid->output_limit;
    
    return output;
}

void pid_init(PIDController* pid, float kp, float ki, float kd, float integral_limit, float output_limit) {
            
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->integral = 0;
    pid->prev_error = 0;
    pid->integral_limit = integral_limit;
    pid->output_limit = output_limit;
}

调参口诀：

参数整定早最佳，从小到大顺序查
先是比例后积分，最后再把微分加
曲线振荡很频繁，比例度盘要放大
曲线漂浮绕大弯，比例度盘往小扳

2. 互补滤波

适用场景：多传感器数据融合，如IMU中结合加速度计和陀螺仪数据。

代码实现：

typedef struct {
            
    float angle;        // 当前估计角度
    float alpha;        // 滤波系数(0<α<1)
    float gyro_bias;    // 陀螺仪零偏
} ComplementaryFilter;

void complementary_update(ComplementaryFilter* cf, float accel_angle, float gyro_rate, float dt) {
            
    // 加速度计角度可信低频，陀螺仪可信高频
    cf->angle = cf->alpha * (cf->angle + (gyro_rate - cf->gyro_bias) * dt) + 
               (1 - cf->alpha) * accel_angle;
}

// 使用示例：无人机姿态估计
ComplementaryFilter pitch_filter = {
            0, 0.98, 0};
while(1) {
            
    float accel_pitch = atan2f(accel_y, sqrtf(accel_x*accel_x + accel_z*accel_z));
    complementary_update(&pitch_filter, accel_pitch, gyro_x, 0.01); // dt=10ms
}

五、算法选择指南

1. 根据信号特性选择

信号特性	推荐算法
缓慢变化+随机噪声	移动平均、一阶滞后
快速变化+脉冲噪声	中值滤波
已知变化速率	限幅滤波
周期性干扰	递推平均、IIR带阻
需要最优估计	卡尔曼滤波
多传感器融合	互补滤波

2. 根据系统资源选择

资源条件	推荐算法	备注
低端MCU	移动平均、限幅、一阶滞后	计算量小，无需浮点
中等资源	中值平均、IIR二阶	需要适量RAM和计算能力
高端MCU/DSP	卡尔曼、自适应滤波	支持复杂运算和矩阵操作

3. 性能对比

算法	去噪能力	实时性	相位滞后	计算复杂度
移动平均	中	高	大	低
中值滤波	抗脉冲	中	无	中
一阶滞后	中	高	小	低
二阶IIR	强	中	中	中
卡尔曼滤波	强	低	可调	高

六、优化技巧与常见问题

1. 定点数优化

在无浮点单元的MCU上，可采用定点数实现：

// 定点数一阶滤波（Q16格式）
int32_t fixed_first_order(int32_t input, int32_t* state, int32_t alpha) {
            
    *state = (alpha * input + (65536 - alpha) * (*state)) >> 16;
    return *state;
}

2. 动态参数调整

根据工况动态调整滤波参数：

// 根据信号变化率调整α值
float dynamic_alpha(float signal_change_rate) {
            
    float alpha = 1.0 - signal_change_rate / MAX_CHANGE_RATE;
    return constrain(alpha, 0.1, 0.9);
}