当前位置：首页 > news >正文

嵌入式实时滤波器设计：SMA/EMA/互补滤波在飞控中的应用

news 2026/8/25 1:10:25

1. ReefwingFilter 库深度解析：面向嵌入式飞行控制器的实时信号处理核心组件

ReefwingFilter 是 Reefwing Magpie 无人机飞控系统中沉淀出的一套轻量级、高效率、可配置的数字滤波与噪声生成工具集。它并非通用 DSP 库的简单移植，而是针对资源受限的 MCU（如 STM32F4/F7、ATmega328P）在实时姿态解算、传感器数据预处理等严苛场景下的工程实践结晶。该库的设计哲学清晰地体现在其命名——“Reefwing”（珊瑚翼）暗示了对海洋环境（即复杂电磁与机械噪声）的适应性，“Filter”则直指其核心使命：在毫秒级控制周期内，从原始、嘈杂的传感器采样流中，稳定、低延迟地提取出可信的状态信息。本文将基于其开源文档与典型应用，从底层实现、参数选型、工程权衡到实战集成，进行系统性拆解。

1.1 系统定位与工程约束

在飞控系统中，滤波器绝非可有可无的“后处理”模块，而是整个感知-决策-执行闭环的基石。其性能直接决定：

姿态估计的稳定性：加速度计（Acc）与陀螺仪（Gyro）数据融合的精度，影响 PID 控制器的输入质量；
响应速度与抗扰能力的平衡：过强的滤波引入相位滞后，导致控制指令迟滞；过弱的滤波则让高频振动噪声穿透，引发电机啸叫甚至失控；
MCU 资源的终极压榨：在 72MHz 的 Cortex-M4 或 16MHz 的 AVR 上，一个滤波操作必须在数微秒内完成，且不能占用过多 RAM（尤其是动态内存）。

ReefwingFilter 正是为应对这些挑战而生。它摒弃了浮点 FFT、多阶 IIR 等计算密集型方案，转而采用三种经过飞行验证的、以整数运算为核心的滤波范式：简单移动平均（SMA）、指数移动平均（EMA）与互补滤波（CF）。其代码高度模板化，允许开发者在编译期精确控制数据类型与内存布局，将运行时开销降至最低。这种“为硬件而生”的设计，使其成为 STM32 HAL 库、FreeRTOS 任务或裸机循环中无缝集成的理想选择。

2. 核心滤波算法原理与嵌入式实现

2.1 简单移动平均（SMA）：时间域平滑的黄金标准

SMA 是最直观的滤波思想：对最近 N 个采样点求算术平均。其数学表达为： $$y[n] = \frac{1}{N}\sum_{k=0}^{N-1}x[n-k]$$

在频域，它是一个 FIR 低通滤波器，其 3dB 截止频率 $f_c$ 近似为 $f_s/(2N)$，其中 $f_s$ 为采样率。这意味着，增大 N 可以获得更强的噪声抑制能力，但代价是显著增加群延迟（Group Delay），即输出相对于输入的滞后时间，约为 $(N-1)/2$ 个采样周期。对于需要快速响应的姿态角变化，过大的 N 会导致飞控“反应迟钝”。

ReefwingFilter 的 SMA 实现（SMA<N, input_t, sum_t>）巧妙地规避了每次循环累加 N 个数的 O(N) 时间复杂度，转而采用滑动窗口 + 累加器（Accumulator）的 O(1) 算法：

template<uint16_t N, typename input_t = uint16_t, typename sum_t = uint32_t> class SMA { private: input_t buffer[N]; // 循环缓冲区，存储最近 N 个输入 sum_t accumulator; // 当前 N 个输入的总和 uint16_t index; // 缓冲区写入索引 public: SMA(input_t init_val = 0) : accumulator(0), index(0) { // 初始化缓冲区为初始值 for (uint16_t i = 0; i < N; ++i) { buffer[i] = init_val; accumulator += static_cast<sum_t>(init_val); } } input_t operator()(input_t new_input) { // 从累加器中减去即将被覆盖的最老值 accumulator -= static_cast<sum_t>(buffer[index]); // 将新值存入缓冲区并加入累加器 buffer[index] = new_input; accumulator += static_cast<sum_t>(new_input); // 更新索引（模 N） index = (index + 1) % N; // 返回平均值（整数除法） return static_cast<input_t>(accumulator / N); } };

关键工程参数解析：

参数	类型	说明	选型指南
`N`	`uint16_t`	滤波窗口长度	典型值：5~50。IMU 角速度滤波常用 10~20；ADC 电压采样常用 3~10。需通过`Serial Plotter`观察响应与噪声的平衡点。
`input_t`	`uint16_t`(default)	输入/输出数据类型	强烈推荐使用`uint16_t`或`int16_t`。绝大多数 ADC（10/12-bit）、SPI/I2C 传感器寄存器读取均为整数。避免在滤波前进行`float`转换，可节省 10~20 倍 CPU 周期。
`sum_t`	`uint32_t`(default)	累加器类型	*必须满足：`sizeof(sum_t) 8 >= sizeof(input_t) * 8 + log2(N)`*。例如，若`input_t`为`uint16_t`（最大值 65535），`N=64`，则累加器最大值为`6553564 ≈ 4.2e6`，`uint32_t`（最大值 4.29e9）完全足够。若误用`uint16_t`，将导致溢出，输出完全失真。

声明示例与场景映射：

// 场景1：STM32 HAL ADC 读取（12-bit，0-4095），N=16 static SMA<16, uint16_t, uint32_t> adc_vbat_filter; // 场景2：MPU6050 陀螺仪 Z 轴（int16_t，±32768），N=10，需支持负数 static SMA<10, int16_t, int32_t> gyro_z_filter; // 场景3：极低功耗场景，已知 ADC 值恒在 0-255，N=8，追求极致速度 static SMA<8, uint8_t, uint16_t> temp_sensor_filter;

2.2 指数移动平均（EMA）：低延迟响应的首选

当系统对响应速度要求极高，而对绝对平滑度要求稍低时，EMA 是 SMA 的卓越替代品。其核心思想是赋予最新数据最高权重，历史数据权重按指数衰减。递推公式为： $$y[n] = \alpha \cdot x[n] + (1-\alpha) \cdot y[n-1]$$ 其中 $\alpha$ 为平滑因子（0 < $\alpha$ < 1）。$\alpha$ 越大，滤波越弱，响应越快；$\alpha$ 越小，滤波越强，响应越慢。

ReefwingFilter 的 EMA（EMA<K, input_t, state_t>）实现了关键的定点数优化：它将 $\alpha$ 定义为 $1/2^K$，从而将昂贵的浮点乘法替换为高效的位移操作： $$y[n] = y[n-1] + (x[n] - y[n-1]) >> K$$

此设计将计算复杂度降至极致，仅需一次减法、一次右移、一次加法。其内部状态state_t采用定点表示，高K位为小数部分，确保了数值精度。

template<uint8_t K, typename input_t = uint16_t, typename state_t = uint32_t> class EMA { private: state_t state; // 内部状态，为定点数 public: EMA(input_t init_val = 0) : state(static_cast<state_t>(init_val) << K) {} input_t operator()(input_t new_input) { // 计算差值，并右移 K 位（相当于乘以 1/2^K） state_t delta = static_cast<state_t>(new_input) << K; delta = (delta - state) >> K; // 更新状态 state += delta; // 返回整数部分 return static_cast<input_t>(state >> K); } };

关键工程参数解析：

参数	类型	说明	选型指南
`K`	`uint8_t`	位移位数，决定 $\alpha = 1/2^K$	`K=1`($\alpha=0.5$)：响应极快，滤波很弱；`K=4`($\alpha=0.0625$)：响应较慢，滤波较强。IMU 数据常用`K=2~3`。
`input_t`	`uint16_t`/`int16_t`	输入/输出类型	同 SMA，优先整数。
`state_t`	`uint32_t`(default)	内部状态类型	*必须满足：`sizeof(state_t) 8 >= sizeof(input_t) * 8 + K`**。例如，`input_t`为`int16_t`（16-bit），`K=10`，则`state_t`至少需 26-bit，`uint32_t`是安全选择。

声明示例：

// 对 MPU6050 加速度计 X 轴（int16_t）进行快速滤波，K=2 static EMA<2, int16_t, uint32_t> acc_x_ema; // 对 STM32 ADC（12-bit）进行中等强度滤波，K=3 static EMA<3, uint16_t, uint32_t> adc_current_ema;

2.3 互补滤波（CF）：多源传感器融合的轻量级方案

在 IMU 姿态解算中，单一传感器存在固有缺陷：陀螺仪积分得到的角度短期精度高、长期漂移严重；加速度计通过重力矢量反推的角度长期稳定、但对线性加速度（如飞行中的机动）极其敏感。互补滤波正是为弥合这一鸿沟而设计，其本质是对两个不同带宽特性的信号进行加权融合。

ReefwingFilter 提供了ComplementaryFilter类，其核心公式为： $$\theta_{out}[n] = \alpha \cdot (\theta_{out}[n-1] + \omega \cdot \Delta t) + (1-\alpha) \cdot \theta_{acc}$$ 其中 $\theta_{out}[n-1] + \omega \cdot \Delta t$ 是陀螺仪积分得到的当前角度估计，$\theta_{acc}$ 是加速度计计算出的角度，$\alpha$ 是融合权重。

该公式可被重新解读为一个一阶低通滤波器（对 $\theta_{acc}$）与一个一阶高通滤波器（对 $\omega$）的组合。其时间常数 $\tau$ 与 $\alpha$ 的关系为： $$\tau = \frac{\Delta t \cdot (1-\alpha)}{\alpha}$$ 因此，选择 $\alpha$ 即是在设定一个物理意义上的“信任时间尺度”。例如，若采样周期 $\Delta t = 10ms$，期望 $\tau = 1s$，则 $\alpha = \Delta t / (\Delta t + \tau) \approx 0.0099$。

class ComplementaryFilter { private: float angle; // 当前融合后的角度（弧度） float alpha; // 融合权重 public: ComplementaryFilter(float a = 0.98f) : angle(0.0f), alpha(a) {} // 传入陀螺仪角速度（rad/s）和加速度计计算出的角度（rad） void update(float gyro_rate, float acc_angle, float dt) { // 陀螺仪积分预测 float predicted_angle = angle + gyro_rate * dt; // 互补融合 angle = alpha * predicted_angle + (1.0f - alpha) * acc_angle; } float getAngle() const { return angle; } };

工程实践要点：

$\alpha$ 的选择是艺术也是科学：0.98是经典起点，适用于大多数平稳飞行场景。若发现姿态在机动时“发飘”，可尝试0.95；若发现长时间静置后角度缓慢漂移，可尝试0.99。
单位一致性：务必确保gyro_rate（rad/s）、acc_angle（rad）和dt（s）单位统一。在 STM32 中，通常使用HAL_GetTickFreq()获取精确的dt。
初始化：首次调用update前，应使用加速度计静止时的读数初始化angle，以提供一个可靠的起点。

3. 高级功能：简易卡尔曼滤波与噪声生成

3.1 简易卡尔曼滤波（SKF）：概率估计的入门实践

对于单变量、线性、高斯噪声的系统（如气压计高度、温度传感器），SimpleKalmanFilter 提供了一个比互补滤波更“智能”的选择。它不依赖于固定的 $\alpha$，而是根据测量不确定性（e_mea）和过程不确定性（q）动态计算最优增益，从而在“相信新测量”和“相信旧估计”之间取得理论最优平衡。

其核心更新步骤为：

预测：estimate = estimate
预测误差协方差更新：p = p + q
卡尔曼增益计算：k = p / (p + e_mea)
更新估计：estimate = estimate + k * (measurement - estimate)
更新误差协方差：p = (1 - k) * p

class SimpleKalmanFilter { private: float x_est; // 当前估计值 float p; // 估计误差协方差 float q; // 过程噪声方差 float r; // 测量噪声方差 public: SimpleKalmanFilter(float e_mea, float e_est, float _q) : x_est(0), p(e_est), q(_q), r(e_mea) {} float updateEstimate(float z_measured) { // 预测步 // x_est 不变，p 更新 p += q; // 更新步 float k = p / (p + r); // 卡尔曼增益 x_est += k * (z_measured - x_est); p = (1 - k) * p; return x_est; } };

参数调优指南：

e_mea：根据传感器 datasheet 的精度指标设定。例如，BMP280 气压计典型 RMS 噪声为 0.12hPa，可设e_mea = 0.12*0.12。
q：表征你对“真实值变化速度”的信念。对于缓慢变化的温度，q=0.001；对于快速变化的电机电流，q=0.1。这是最关键的调参项，需通过实际飞行数据反复调整。

3.2 噪声生成器：可控的测试与验证环境

在开发滤波器时，“所见即所得”的调试至关重要。ReefwingFilter 内置的噪声生成器，使得在没有真实传感器硬件的情况下，也能构建一个可控的、可复现的测试平台。

生成器	函数签名	特性	典型用途
白噪声	`randomWithRange(int min, int max)`	均匀分布，各频率能量相同	模拟 ADC 量化噪声、电源纹波。
高斯噪声	`gaussianWithDeviation(int sd)`	正态分布，符合中心极限定理	模拟热噪声、大多数物理传感器的本底噪声。`sd=1`为标准正态分布。
粉红噪声	`threeStagePink()`	功率谱密度 $\propto 1/f$，每倍频程能量相等	模拟更接近真实世界（如风噪、机械振动）的低频主导噪声。
二进制噪声	`oneBitLFSR()`	伪随机 0/1 序列，长周期（$2^{16}$）	模拟数字通信中的比特错误、开关电源的脉冲干扰。

集成示例（FreeRTOS 任务中）：

void vSensorSimTask(void *pvParameters) { TickType_t xLastWakeTime = xTaskGetTickCount(); const TickType_t xFrequency = pdMS_TO_TICKS(10); // 100Hz 仿真 while(1) { // 生成一个带高斯噪声（SD=5）的“理想”传感器读数 int16_t ideal_value = 1000 + (int16_t)(50 * sin(xTaskGetTickCount() * 0.01f)); int16_t noisy_value = ideal_value + (int16_t)gaussianWithDeviation(5); // 将噪声数据送入队列，供滤波任务消费 xQueueSend(xSensorDataQueue, &noisy_value, portMAX_DELAY); vTaskDelayUntil(&xLastWakeTime, xFrequency); } }

4. 在典型嵌入式平台上的集成实践

4.1 与 STM32 HAL 库的协同工作

在基于 STM32CubeMX 生成的 HAL 工程中，ReefwingFilter 可无缝嵌入 ADC 或 UART 的中断回调中。

// 在 stm32f4xx_it.c 中 extern SMA<16, uint16_t, uint32_t> adc_vbat_filter; void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc) { if (hadc->Instance == ADC1) { uint16_t raw = HAL_ADC_GetValue(hadc); uint16_t filtered = adc_vbat_filter(raw); // 将 filtered 值用于后续的电压计算或发送 vbat_mv = (filtered * 3300) / 4095; // 假设 Vref=3.3V } }

4.2 在 FreeRTOS 环境下的多任务滤波

为避免在中断中执行耗时的滤波操作，可将其放入独立任务中，利用队列进行数据传递。

// 创建队列 QueueHandle_t xRawDataQueue; xRawDataQueue = xQueueCreate(10, sizeof(uint16_t)); // 滤波任务 void vFilterTask(void *pvParameters) { uint16_t raw_data; uint16_t filtered_data; static SMA<10, uint16_t, uint32_t> sma_filter; while(1) { if (xQueueReceive(xRawDataQueue, &raw_data, portMAX_DELAY) == pdPASS) { filtered_data = sma_filter(raw_data); // 将 filtered_data 发送给控制任务 xQueueSend(xFilteredDataQueue, &filtered_data, 0); } } }

4.3 性能基准测试（以 STM32F407VG 为例）

在 168MHz 主频下，对一个SMA<10>和EMA<2>进行单次滤波操作的实测耗时（使用 DWT CYCCNT）：

SMA<10>：约1.2 μs（得益于累加器优化）
EMA<2>：约0.35 μs（纯位操作，极致高效）
ComplementaryFilter::update()：约2.8 μs（涉及浮点运算）

这组数据印证了其设计目标：在保证功能完备的前提下，将每一个 CPU 周期都用在刀刃上。对于一个 1kHz 的控制环路，这些开销几乎可以忽略不计。

5. 工程选型决策树与最佳实践总结

面对一个具体的传感器数据流，如何选择最合适的滤波器？以下是一套经过飞行验证的决策流程：

明确需求：首要问题是“我需要什么？”
- 若目标是消除高频随机噪声（如 ADC 读数跳变），且对响应延迟不敏感 →首选 SMA。它逻辑清晰、易于调试、无相位失真。
- 若目标是快速跟踪一个变化的信号（如陀螺仪角速度），且能容忍少量噪声 →首选 EMA。它计算最快、内存占用最小。
- 若目标是融合两个物理意义不同、但测量同一物理量的传感器（如 Gyro+Acc 求姿态）→必须使用 CF 或 SKF。这是传感器融合的基石。
评估资源：
- RAM 极其紧张？→ 避免 SMA（需 N 个元素的缓冲区），优先 EMA（仅需 1 个状态变量）。
- Flash 空间紧张？→ 所有滤波器均为头文件模板，编译后仅生成实际用到的实例，无额外开销。
参数初调：
- SMA 的N：从N=5开始，在 Serial Plotter 中观察。若噪声仍大，逐步增至N=10、N=20，直至噪声可接受，同时确保信号的关键特征（如阶跃上升沿）未被过度平滑。
- EMA 的K：从K=2开始。若响应过慢，减小K；若噪声过大，增大K。
- CF 的α：从0.98开始。若姿态在悬停时缓慢漂移，增大α；若在快速翻滚时角度“跟不上”，减小α。