软件低通滤波器（附代码）
软件低通滤波器（附代码）
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，截止频率80Hz
lpf2pInit(&gyroX_LPF, 1000.0f, 80.0f);
lpf2pInit(&gyroY_LPF, 1000.0f, 80.0f);
lpf2pInit(&gyroZ_LPF, 1000.0f, 80.0f);
// 加速度计：采样率1000Hz，截止频率30Hz
lpf2pInit(&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. 常用参数选择
浙公网安备 33010602011771号