ArduPilot LowPassFilter 深度解析

一阶低通滤波器的类结构、算法推导、用法示例与工程实践

一、引言：为什么从 LowPassFilter 开始

在 ArduPilot（APM）庞大的滤波器体系中，LowPassFilter 是最基础、也是使用最广泛的一个。它的实现极其精简------核心算法只有一行代码，但却承担了飞控中大量传感器信号的初级平滑任务。理解 LowPassFilter，是理解整个 APM 滤波链路的起点。

本文将从类继承体系出发，逐层深入：先看两种使用模式的区别，再推导核心算法的数学原理，然后逐行解析示例代码，最后给出截止频率选型建议与工程实践要点。无论你是在调参、排查抖动问题，还是在做二次开发，这篇文章都能帮你彻底搞懂这个"最简单也最常用"的滤波器。

二、类继承体系

LowPassFilter 库采用模板类设计，核心逻辑封装在 DigitalLPF 基类中，向外暴露两个具体子类，分别应对固定采样率和可变采样率两种场景：

DigitalLPF<T> // 基类：封装核心 EMA 算法
│
├── LowPassFilterConstDt<T> // 固定时间步长（采样率恒定）
│
└── LowPassFilter<T> // 可变时间步长（每次传入 dt）

这种设计的好处是：核心滤波算法只写一份，两个子类只负责 alpha 系数的计算方式不同。基类维护输出状态（output）和初始化标志（initialised），子类根据各自的时间步长策略计算出 alpha 后，调用基类的 _apply() 完成实际滤波。

三、两种使用模式对比

3.1 可变时间步长：LowPassFilter

适用于采样间隔不固定的场景，每次调用 apply() 时显式传入本次的时间间隔 dt：

// 创建滤波器实例
LowPassFilterFloat filter;

// 设置截止频率（只需设置一次）
filter.set_cutoff_frequency(1.0f); // 截止频率 1 Hz

// 每次采样时传入实际 dt
float output = filter.apply(new_value, 0.02f); // dt = 20 ms

3.2 固定时间步长：LowPassFilterConstDt

适用于采样率恒定的场景，在构造时一次性指定采样频率和截止频率，后续 apply() 无需传 dt：

// 创建时指定：采样率 50 Hz，截止频率 1 Hz
LowPassFilterConstDtFloat filter(50.0f, 1.0f);

// 每次采样只需传入新值，无需传 dt
float output = filter.apply(new_value);

3.3 如何选择

|----------|--------------------------|---------------------------------|
| 对比维度 | LowPassFilter（可变 dt） | LowPassFilterConstDt（固定 dt） |
| 适用场景 | 采样间隔不稳定、多任务调度 | 定时器中断、固定频率循环 |
| alpha 计算 | 每次 apply 时根据传入 dt 实时计算 | 构造时预计算一次，后续直接使用 |
| 计算开销 | 略高（每次需算 alpha） | 极低（alpha 为常量） |
| 灵活性 | 高，可适应任意 dt | 低，dt 必须恒定 |
| APM 典型用法 | 主循环中传感器读取 | 高速 IMU 采样中断 |

四、核心算法深度解析

4.1 核心实现

所有滤波逻辑都在 DigitalLPF 基类的 _apply() 方法中，代码精简到令人惊讶：

T DigitalLPF<T>::_apply(const T &sample, const float &alpha) {
output += (sample - output) * alpha;
if (!initialised) {
initialised = true;
output = sample; // 首次采样直接输出，不做平滑
}
return output;
}

4.2 滤波公式

这是典型的一阶低通滤波器，也称为指数移动平均（Exponential Moving Average, EMA）：

y $n$ = y $n-1$ + α × (x $n$ - y $n-1$ )

等价展开：
y $n$ = α × x $n$ + (1 - α) × y $n-1$

从展开式可以直观理解：当前输出 = 新样本的 α 倍 + 上一次输出的 (1-α) 倍。α 越小，历史输出的权重越大，滤波越"重"，输出越平滑，但延迟也越大；α 越大，新样本的权重越大，滤波越"轻"，响应越快但平滑效果越弱。

4.3 alpha 系数的计算与推导

alpha 并非随意设定，而是由截止频率和采样间隔共同决定：

alpha = 2π × fc × dt

其中：

fc = 截止频率（Hz），即滤波器 -3 dB 衰减点对应的频率
dt = 采样时间间隔（秒），即两次采样之间的时间

推导过程如下：

// 连续域 RC 低通滤波器的 -3dB 截止频率：
fc = 1 / (2π × RC)

// 离散化（双线性变换 / 前向欧拉近似）：
α = 1 - e^(-dt/RC)

// 当 dt << RC 时（即采样率远高于截止频率），一阶泰勒展开近似：
α ≈ dt / RC = 2π × fc × dt

需要注意的是，alpha = 2π·fc·dt 是一个近似公式，成立条件是采样率远高于截止频率（通常要求采样率 ≥ 10× 截止频率）。如果截止频率接近奈奎斯特频率（采样率的一半），这个近似会产生较大误差，此时应使用精确公式 α = 1 - e^(-dt/RC)。

4.4 首次采样的冷启动处理

_apply() 中的 initialised 标志值得特别关注。第一次调用时，output 的初始值是未定义的（可能是 0，也可能是随机值），如果直接做 EMA 计算，输出会从初始值缓慢爬升到真实信号，产生一个明显的过渡过程。

APM 的处理方式是：首次采样直接将 output 设为 sample，跳过滤波计算。这样滤波器从第一个真实值开始工作，避免了冷启动瞬态。在飞行模式切换、传感器重新初始化等场景下，也可以通过 reset() 方法手动触发这个"重新初始化"的过程。

五、示例代码逐行解析

// 全局滤波器实例
LowPassFilterFloat low_pass_filter;

void setup() {
// 设置截止频率为 1 Hz
low_pass_filter.set_cutoff_frequency(1.0f);
}

void loop() {
// 重置滤波器到初始值 0
low_pass_filter.reset(0);

// 模拟输入：5 Hz 正弦波，50 Hz 采样率
for (int16_t i = 0; i < 300; i++) {
float new_value = sinf(i * 2 * M_PI * 5 / 50.0f); // 5 Hz
float filtered_value = low_pass_filter.apply(new_value, 0.02f);
}
}

5.1 参数配置

|--------|-------|-------------------------|
| 参数 | 值 | 含义 |
| 输入信号频率 | 5 Hz | 正弦波的频率 |
| 采样频率 | 50 Hz | 采样率（dt = 1/50 = 0.02 s） |
| 截止频率 | 1 Hz | 滤波器 -3 dB 截止点 |
| 奈奎斯特频率 | 25 Hz | 采样频率的一半，理论可恢复最高频率 |

5.2 频率响应分析

在这个示例中，截止频率（1 Hz）远低于输入信号频率（5 Hz），因此 5 Hz 的正弦波会被显著衰减。我们可以用一阶低通的幅频响应公式估算衰减量：

// 一阶低通幅频响应：
|H(f)| = 1 / sqrt(1 + (f/fc)²)

// 代入 f = 5 Hz, fc = 1 Hz：
|H(5)| = 1 / sqrt(1 + 25) = 1 / sqrt(26) ≈ 0.196

// 换算成分贝：
衰减 = 20 × log10(0.196) ≈ -14.2 dB

也就是说，5 Hz 的输入信号经过这个 1 Hz 截止的一阶低通后，幅度会衰减到原来的约 19.6%（-14.2 dB）。如果输入中混杂着更高频的噪声（如 50 Hz 的电机振动），衰减会更加强烈（|H(50)| ≈ 0.02，即 -34 dB）。这正是低通滤波器去除高频噪声的原理。

六、截止频率怎么选

截止频率是 LowPassFilter 唯一需要调的参数，它直接决定了滤波器的"通带边界"：

|----------------|-------------|----------|------------|
| 截止频率 | 滤波效果 | 相位延迟 | 适用场景 |
| 高（如 10~20 Hz） | 通过更多高频，滤波较弱 | 小 | 需要快速响应的信号 |
| 中（如 3~5 Hz） | 平衡平滑与响应 | 中等 | 通用姿态信号 |
| 低（如 0.5~1 Hz） | 过滤更多高频，滤波很强 | 大 | 慢变信号、高度/位置 |

6.1 选型原则

保留信号中需要的最高频率分量：先确定你的有效信号带宽，截止频率应高于这个带宽。
滤除不需要的高频噪声：通过 FFT 分析确认噪声主要分布在哪个频段，截止频率应低于噪声频段。
经验法则：截止频率通常设为信号最高有效频率的 1~2 倍，同时远低于噪声起始频率。
注意相位裕度：截止频率越低，相位延迟越大，可能影响闭环稳定性。调参后需验证环路相位裕度。

6.2 APM 中的典型取值参考

|-------------|------------|-------------------------------|
| 信号类型 | 典型截止频率 | 说明 |
| 陀螺仪（Gyro） | 20~40 Hz | 姿态内环，需要较快响应，通常配合谐波陷波使用 |
| 加速度计（Accel） | 5~10 Hz | 水平加速度用于位置估计，需适度平滑 |
| 气压计高度 | 0.5~2 Hz | 高度变化慢，气流噪声大，需要较强滤波 |
| GPS 位置/速度 | 1~3 Hz | GPS 更新率低（5~10 Hz），噪声主要来自多径效应 |
| 电池电压/电流 | 0.1~1 Hz | 慢变信号，主要滤除开关电源纹波 |

注意：以上仅为经验参考值，实际取值需根据具体机型、传感器、振动环境通过 FFT 分析和飞行测试确定。APM 中很多滤波器的截止频率是可通过参数配置的，调参时建议从保守值（偏低）开始，逐步提高直到出现噪声或抖动，再回退一档。

七、完整 API 方法一览

|-----------------------------------------|-------------------------|----------------------|
| 方法 | 功能 | 适用类 |
| set_cutoff_frequency(freq) | 设置截止频率（使用默认或已设定的采样率） | 两者皆可 |
| set_cutoff_frequency(sample_freq, freq) | 同时设置采样率和截止频率 | 两者皆可 |
| apply(sample) | 应用滤波（固定 dt 模式，无需传时间间隔） | LowPassFilterConstDt |
| apply(sample, dt) | 应用滤波（可变 dt 模式，传入本次时间间隔） | LowPassFilter |
| get() | 获取当前滤波输出值（不输入新样本） | 两者皆可 |
| reset(value) | 重置滤波器到指定值（相当于重新初始化） | 两者皆可 |
| get_cutoff_freq() | 获取当前设定的截止频率 | 两者皆可 |

八、支持的数据类型

得益于模板设计，LowPassFilter 不仅支持标量，还支持向量类型。APM 预定义了以下常用类型别名：

// ===== 标量类型 =====
typedef LowPassFilterConstDt<float> LowPassFilterConstDtFloat;
typedef LowPassFilter<float> LowPassFilterFloat;

// ===== 二维向量 =====
typedef LowPassFilterConstDt<Vector2f> LowPassFilterConstDtVector2f;
typedef LowPassFilter<Vector2f> LowPassFilterVector2f;

// ===== 三维向量 =====
typedef LowPassFilterConstDt<Vector3f> LowPassFilterConstDtVector3f;
typedef LowPassFilter<Vector3f> LowPassFilterVector3f;

向量版本的滤波器会对每个分量独立做 EMA 运算，使用方式与标量完全一致。例如对三维加速度向量滤波时，X、Y、Z 三个轴各自维护独立的 output 状态，但共享同一个截止频率和 alpha 系数。这在处理 IMU 数据时非常方便------一个 Vector3f 滤波器实例即可同时完成三个轴的平滑。

九、性能特点总结

|--------|--------------------------------------|
| 特性 | 说明 |
| 阶数 | 一阶（单极点） |
| 算法 | 指数移动平均（EMA） |
| 核心运算量 | 极低------仅 1 次乘法 + 1 次加法（不含 alpha 计算） |
| 内存占用 | 极小------仅需维护 1 个输出变量 + 1 个初始化标志 |
| 衰减斜率 | -20 dB / 十倍频程 |
| 相位延迟 | 较小（一阶系统，相位滞后最大 90°） |
| 通带纹波 | 无（单调衰减） |
| 数值稳定性 | 极好（一阶 IIR，无内部溢出风险） |
| 适用场景 | 实时性要求高、计算资源受限的嵌入式系统 |

十、一阶 vs 二阶低通滤波器对比

LowPassFilter 是一阶低通，APM 中还有一个 LowPassFilter2p 是二阶低通。两者在算法复杂度、滤波效果和适用场景上有明显区别：

|------------|-----------------------|--------------------------------------------------|
| 对比维度 | LowPassFilter（一阶） | LowPassFilter2p（二阶） |
| 极点数量 | 1 个 | 2 个 |
| 衰减斜率 | -20 dB / 十倍频程 | -40 dB / 十倍频程 |
| 过渡带 | 较平缓 | 更陡峭 |
| 通带纹波 | 无（天然单调） | 无（巴特沃斯设计） |
| 相位延迟 | 较小（最大 90°） | 较大（最大 180°） |
| 计算量 | 极低（1 乘 1 加） | 中等（5 乘 4 加 + 4 个状态变量） |
| 内存占用 | 极小（1 个输出） | 较小（4 个状态：x $n-1$ , x $n-2$ , y $n-1$ , y $n-2$ ） |
| alpha/系数计算 | 简单（2π·fc·dt） | 复杂（Biquad 系数，含三角函数） |
| 适用场景 | 简单平滑、资源极度受限 | 精密滤波、需要陡峭过渡带 |
| APM 典型用途 | 气压计、电池电压、GPS 等慢变信号 | 陀螺仪、加速度计等 IMU 主力滤波 |

简单来说：如果只是做基础平滑、对滤波陡峭度要求不高，一阶低通就够了，而且更省资源；如果需要在保留有效信号的同时强力滤除紧邻的高频噪声（如陀螺仪信号中紧邻的电机振动），则需要二阶低通提供更陡峭的过渡带。在 APM 的实际滤波链路中，两者经常配合使用------一阶低通处理慢变信号，二阶低通处理 IMU 等对精度要求高的信号。

十一、工程实践要点

11.1 采样率与截止频率的约束关系

alpha = 2π·fc·dt 这个近似公式有一个隐含约束：alpha 必须小于 1（否则滤波器不稳定）。代入公式可得 2π·fc·dt < 1，即 fc < 1/(2π·dt) ≈ 0.159 × 采样率。也就是说，截止频率不能超过采样率的约 16%。实际使用中建议截止频率不超过采样率的 10%，以保证近似精度和滤波器稳定性。

11.2 避免在中断中动态计算 alpha

LowPassFilter（可变 dt）每次 apply 都要计算 alpha = 2π·fc·dt，涉及浮点乘法。在高频中断（如 8 kHz 或 32 kHz 的 IMU 采样）中，这个额外的浮点运算可能成为性能瓶颈。如果采样率是固定的，优先使用 LowPassFilterConstDt，让 alpha 在构造时预计算一次，中断中只做最核心的 EMA 运算。

11.3 reset() 的正确使用时机

传感器初始化或重新校准后：调用 reset(当前读数)，避免从 0 开始的过渡过程。
飞行模式切换时：如果新模式下信号特性差异大（如手动切自动），可考虑 reset。
长时间停滞后恢复：如果滤波器长时间没有新输入，output 可能停留在旧值，reset 可避免跳变。
不要频繁 reset：每次 reset 相当于丢弃历史信息，滤波器需要重新"热身"，频繁 reset 会降低平滑效果。

11.4 级联使用时的顺序

在 APM 的实际滤波链路中，LowPassFilter 通常不是单独使用的，而是与其他滤波器级联。典型的陀螺仪信号处理链路是：

原始信号 → HarmonicNotchFilter（谐波陷波）→ LowPassFilter2p（二阶低通）→ 输出

一阶 LowPassFilter 更多用于气压计、电池电压、GPS 等慢变信号的独立平滑。如果需要将一阶低通与其他滤波器级联，建议将一阶低通放在最后一级，作为"收尾平滑"，因为它的计算成本最低，放在最后不会增加前级信号的动态范围。

十二、结语

LowPassFilter 是 APM 滤波器体系中最简单的一个，但简单不等于不重要。它用一行代码实现了指数移动平均，用极小的计算和内存开销完成了大量信号的基础平滑任务。理解它的类结构设计（基类 + 固定/可变 dt 子类）、alpha 系数的数学推导、首次采样的冷启动处理，以及截止频率的选型原则，能帮助你在调参和开发中做出更合理的决策。

当然，一阶低通也有其局限性-------20 dB/十倍频程的衰减斜率在面对紧邻噪声时显得力不从心，这也是为什么 APM 还提供了二阶低通（LowPassFilter2p）、陷波（NotchFilter）、谐波陷波（HarmonicNotchFilter）等更强大的滤波器。在后续文章中，我们将继续深入解析这些滤波器。