软件低通滤波器（附代码）
1. 什么是低通滤波器
低通滤波器（Low-Pass Filter, LPF）就像是一个数字世界的“筛子”。它允许低频信号（平缓的变化，如飞机的姿态）通过，而阻挡高频信号（剧烈的震动，如电机的噪声）。
在传感器数据处理中，信号通常由两部分组成：
有用信号： 变化相对缓慢的真实物理运动。
噪声： 由电子干扰或机械振动引起的高频随机跳动。
低通滤波器的核心任务就是：抹除尖锐的毛刺，保留平滑的趋势。
一阶低通滤波器 (1st Order LPF)
一阶滤波器是最基础的形式，通常被称为加权指数平均滤波。
差分方程
y[n]=α⋅x[n]+(1-α)⋅y[n-1]
x[n]：本次采样值。
y[n-1]：上一次的滤波输出。
α：平滑系数（0 到 1 之间）。
一阶滤波器优缺点：
优点： 算法极简，计算量极小，只需要存储一个上一次的值。
缺点： “杀伤力”有限。它在截止频率之后的衰减速度只有 -20dB/dec。这意味着如果你想强力过滤噪声就必须把α设得很小，但这会导致严重的信号延迟（相位滞后）
二阶低通滤波器 (2nd Order LPF)
硬件类比： 如果一阶滤器是一层海绵，二阶滤器就是弹簧 + 减震筒的组合。
衰减速度： 它的衰减速度是 -40dB/dec。在同样的截止频率下，二阶滤器对高频噪声的抑制能力是一阶的 100 倍左右。
巴特沃斯特性： 代码中实现的巴特沃斯（Butterworth）型，最大的特点是在通带内（你想保留的频率范围内）非常平坦，不会让你的原始数据变形。
性能对比
应该如何选择
选一阶： 当你的单片机性能极差，或者噪声非常微弱，只需要稍微润色一下数据时。
选二阶： 当你处理的是陀螺仪、加速度计这种处于高振动环境（如无人机、平衡车）下的传感器时。
2. PT1 实现代码
#ifndef __FILTER_H
#define __FILTER_H
#include <stdint.h>
#include <math.h>
#include <stdbool.h>
/*=================== 一阶低通滤波器 (PT1) ===================*/
typedef struct {float state; // 滤波器状态 (上一次的输出值)float RC; // RC时间常数 = 1/(2π*fc)float dT; // 采样周期 (秒)
} pt1Filter_t;
// 初始化
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);
// 应用滤波
float pt1FilterApply(pt1Filter_t *filter, float input);
// 重置
void pt1FilterReset(pt1Filter_t *filter, float input);
#endif //__FILTER_H#include "filter.h"#define M_PI_F 3.14159265f/*** @brief 初始化PT1滤波器* @param filter 滤波器结构体指针* @param f_cut 截止频率 (Hz)* @param dT 采样周期 (秒)*/
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
{filter->RC = 1.0f / (2.0f * M_PI_F * f_cut); // RC = 1/(2πfc)filter->dT = dT;filter->state = 0.0f;
}
/*** @brief 应用一阶低通滤波* @param filter 滤波器结构体指针* @param input 输入样本值* @return 滤波后的输出值* * 公式: y[n] = y[n-1] + (dt/(RC+dt)) * (x[n] - y[n-1])*/
float pt1FilterApply(pt1Filter_t *filter, float input)
{filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);return filter->state;
}
/*** @brief 重置滤波器状态*/
void pt1FilterReset(pt1Filter_t *filter, float input)
{filter->state = input;
}// 示例1：一阶滤波器 (PT1) - 遥控信号平滑#include "filter.h"
// 定义滤波器 (必须保持状态，用static或全局变量)
static pt1Filter_t rcRollFilter;
static pt1Filter_t rcPitchFilter;
void init(void)
{// 初始化：截止频率10Hz，采样周期20ms (50Hz更新率)pt1FilterInit(&rcRollFilter, 10, 0.02f);pt1FilterInit(&rcPitchFilter, 10, 0.02f);
}
void mainLoop(void)
{// 读取原始遥控值 (0-65535)uint16_t rawRoll = getRCChannel(ROLL_CHANNEL);// 应用滤波 (平滑后的值)float rollSmooth = pt1FilterApply(&rcRollFilter, (float)rawRoll);// 使用滤波后的值setpoint.attitude.roll = rollSmooth;
}
3. lpf2p 实现代码
#ifndef __FILTER_H
#define __FILTER_H
#include <stdint.h>
#include <math.h>
#include <stdbool.h>typedef struct {float a1, a2; // 反馈系数 (分母)float b0, b1, b2; // 前馈系数 (分子)float delay_element_1; // 状态变量 y[n-1]float delay_element_2; // 状态变量 y[n-2]
} lpf2pData;
// 初始化
void lpf2pInit(lpf2pData* lpfData, float sample_freq, float cutoff_freq);
// 应用滤波
float lpf2pApply(lpf2pData* lpfData, float sample);
// 重置
float lpf2pReset(lpf2pData* lpfData, float sample);#endif //__FILTER_H/*=================== 二阶低通滤波器实现 ===================*/#include "filter.h"
#define M_PI_F 3.14159265f
/*** @brief 初始化二阶低通滤波器* @param lpfData 滤波器结构体指针* @param sample_freq 采样频率 (Hz)* @param cutoff_freq 截止频率 (Hz)* * 使用双线性变换设计的Butterworth滤波器*/
void lpf2pInit(lpf2pData* lpfData, float sample_freq, float cutoff_freq)
{if (lpfData == NULL || cutoff_freq <= 0.0f) {return;}float fr = sample_freq / cutoff_freq;float ohm = tanf(M_PI_F / fr);float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;// 计算滤波器系数lpfData->b0 = ohm * ohm / c;lpfData->b1 = 2.0f * lpfData->b0;lpfData->b2 = lpfData->b0;lpfData->a1 = 2.0f * (ohm * ohm - 1.0f) / c;lpfData->a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;// 清零状态lpfData->delay_element_1 = 0.0f;lpfData->delay_element_2 = 0.0f;
}
/*** @brief 应用二阶低通滤波* @param lpfData 滤波器结构体指针* @param sample 输入样本值* @return 滤波后的输出值* * 差分方程:* y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2]*/
float lpf2pApply(lpf2pData* lpfData, float sample)
{// 计算中间变量 (输入部分)float delay_element_0 = sample - lpfData->delay_element_1 * lpfData->a1 - lpfData->delay_element_2 * lpfData->a2;// 检查非法值if (!isfinite(delay_element_0)) {delay_element_0 = sample;}// 计算输出float output = delay_element_0 * lpfData->b0 + lpfData->delay_element_1 * lpfData->b1 + lpfData->delay_element_2 * lpfData->b2;// 移位更新状态lpfData->delay_element_2 = lpfData->delay_element_1;lpfData->delay_element_1 = delay_element_0;return output;
}
/*** @brief 重置滤波器状态*/
float lpf2pReset(lpf2pData* lpfData, float sample)
{float dval = sample / (lpfData->b0 + lpfData->b1 + lpfData->b2);lpfData->delay_element_1 = dval;lpfData->delay_element_2 = dval;return lpf2pApply(lpfData, sample);
}#include "filter.h"
// 三轴传感器需要3个独立的滤波器
static lpf2pData gyroX_LPF, gyroY_LPF, gyroZ_LPF;
static lpf2pData accX_LPF, accY_LPF, accZ_LPF;
void sensorsInit(void)
{// 陀螺仪：采样率1000Hz，截止频率80Hzlpf2pInit(&gyroX_LPF, 1000.0f, 80.0f);lpf2pInit(&gyroY_LPF, 1000.0f, 80.0f);lpf2pInit(&gyroZ_LPF, 1000.0f, 80.0f);// 加速度计：采样率1000Hz，截止频率30Hzlpf2pInit(&accX_LPF, 1000.0f, 30.0f);lpf2pInit(&accY_LPF, 1000.0f, 30.0f);lpf2pInit(&accZ_LPF, 1000.0f, 30.0f);
}
void sensorTask(void)
{// 读取原始传感器数据float raw_gx = readGyroX();float raw_gy = readGyroY();float raw_gz = readGyroZ();// 应用滤波float filt_gx = lpf2pApply(&gyroX_LPF, raw_gx);float filt_gy = lpf2pApply(&gyroY_LPF, raw_gy);float filt_gz = lpf2pApply(&gyroZ_LPF, raw_gz);// 使用滤波后的数据imuUpdate(filt_gx, filt_gy, filt_gz, ...);
}4. 常用参数选择
