滤波器系列1：概述

Merlin0504

550人浏览 · 2026-06-15 17:16:07

Merlin0504 · 2026-06-15 17:16:07 发布

第 1 篇：引子 — 为什么要滤波？

设想一个场景：你拿着一把卷尺去量一张桌子的长度。量了 10 次，得到的结果分别是：

120.1cm, 119.8cm, 120.3cm, 119.9cm, 120.2cm,
120.0cm, 119.7cm, 120.1cm, 119.8cm, 120.2cm

没有一次完全一样，但都在 120cm 附近。你的第一反应是：取个平均吧。

$\bar{x} = \frac{1}{N}\sum_{i=1}^{N} x_i = \frac{120.1 + 119.8 + \cdots + 120.2}{10} = 120.01 \text{ cm}$

这个直觉就是滤波的雏形：多个带噪声的观测 $→\rightarrow$ 一个更可靠的估计。

为什么单次测量不够？

传感器的测量永远有噪声。不管是卷尺（人眼读数误差）、激光雷达（光子散粒噪声）、还是 GPS（多径效应），噪声是物理世界的固有属性。

我们可以把一个带噪声的测量建模为：

$z_k = x_k + v_k, \quad v_k \sim \mathcal{N}(0, \sigma^2)$

其中 $x_k$ 是真实值， $v_k$ 是高斯白噪声。

传感器	典型噪声来源	噪声量级（ $σ\sigma$ ）
卷尺	读数误差、拉伸松紧	$±1mm∼±5mm\pm 1\text{mm} \sim \pm 5\text{mm}$
毫米波雷达	热噪声、多径反射	$±0.1m∼±1m\pm 0.1\text{m} \sim \pm 1\text{m}$
GPS	电离层延迟、卫星钟差	$±1m∼±10m\pm 1\text{m} \sim \pm 10\text{m}$
惯性测量单元（IMU）	零偏、随机游走	随时间累积

// 模拟一个带噪声的雷达测距
double trueRange = 100.0;                    // 真实距离 100m
double noiseStd  = 2.0;                      // 噪声标准差 2m
double measured  = trueRange + randn() * noiseStd;  // 比如 101.3m 或 98.7m

如果你只靠"这一帧"的测量值做决策，那么误差可能在 $±2σ\pm 2\sigma$ 甚至更大。但如果你把连续多帧的测量串联起来，就能做出比任何单帧都好的估计。

什么是"状态"？

在跟踪问题中，我们关心的是目标的状态（state）。状态是一个数学抽象，它包含所有我们需要知道的信息。

最简单的跟踪场景：一辆车在笔直的公路上行驶。

t=0s     t=1s     t=2s     t=3s     t=4s
|--------|--------|--------|--------|-------->
x=0      x=10     x=20     x=30     x=40    位置 (m)
v=10     v=10     v=10     v=10     v=10    速度 (m/s)

如果我们用位置和速度来描述这辆车：

$\mathbf{x}_k = \begin{bmatrix} p_k \\ v_k \end{bmatrix}$

那么"跟踪"的本质就是：每一时刻 $k$ ，我们都想知道 $xk\mathbf{x}_k$ 。但我们往往不能直接测量所有状态分量（比如速度测量成本很高），我们只能测量位置，而且测量还有噪声。

这就是滤波器发挥作用的地方：从带噪声的位置观测中，推断出位置和速度两个状态。

滤波的两大基本操作

预测（Predict）

假设我们知道上一时刻的状态 $xk−1\mathbf{x}_{k-1}$ ，我们可以根据运动模型推测当前时刻的状态。

匀速运动模型：

$\mathbf{x}_k^- = \mathbf{F} \cdot \mathbf{x}_{k-1}$

展开写：

$\begin{bmatrix} p_k^- \\ v_k^- \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_{k-1} \\ v_{k-1} \end{bmatrix}$

即：

$\begin{aligned} p_k^- &= p_{k-1} + v_{k-1} \cdot \Delta t \\ v_k^- &= v_{k-1} \end{aligned}$

上标 $-$ 表示"预测值"（也称为先验估计），尚未用测量校正。

用代码表达就是：

// 匀速模型：新位置 = 旧位置 + 速度 × Δt
void predict(double dt) {
    state_(0) += state_(1) * dt;  // 位置外推: p = p + v·dt
    // state_(1) 速度不变
}

这是 AlphaBetaFilter::predict() 的核心逻辑，一共两行。

更新（Update）

现在我们拿到了一个新的测量值 $z_k$ （带噪声）。我们需要在"预测值"和"测量值"之间做一个折中。

定义新息（innovation）——测量值与预测值的差异：

$\tilde{y}_k = z_k - p_k^-$

然后用一个比例系数 $α\alpha$ 来控制校正的幅度：

$\begin{aligned} p_k &= p_k^- + \alpha \cdot \tilde{y}_k \\ v_k &= v_{k-1} + \beta \cdot \tilde{y}_k / \Delta t \end{aligned}$

这里 $α\alpha$ 和 $β\beta$ 就是权重（增益）：

$α=0.8\alpha = 0.8$ ：位置更新 80% 相信新测量，20% 保留预测
$α=0.2\alpha = 0.2$ ：位置更新 20% 相信新测量，80% 保留预测

// 更新：在预测和测量之间折中
void update(double z) {
    double residual = z - state_(0);       // 新息: z - p⁻
    state_(0) += alpha_ * residual;         // 位置校正: p = p⁻ + α·(z - p⁻)
    state_(1) += beta_ * residual / dt_;    // 速度校正: v = v + β·(z - p⁻)/Δt
}

这就是本系列第二篇要详细讲的 $α\alpha$ - $β\beta$ 滤波器。

预测-更新循环

跟踪不是一个一次性的操作，而是一个持续的过程：

时间轴:

t₀:   [初始化] ← 拿到第一次测量，设置初始状态
       │
       │  预测 (模型)        更新 (测量)
       ▼
t₁:   Predict(Δt) ──────→ Update(z₁) ──────→ 状态 x₁
       │
t₂:   Predict(Δt) ──────→ Update(z₂) ──────→ 状态 x₂
       │
t₃:   Predict(Δt) ──────→ Update(z₃) ──────→ 状态 x₃
       │
      ...

每一步，滤波器都在做同一件事：

用模型走向未来，用测量校正现在。

为什么不用简单平均？

你可能会问：既然测量有噪声，那我连续取 $N$ 帧测量值做个滑动平均不就行了？

double simpleAverage(std::deque<double>& window, double newMeas) {
    window.push_back(newMeas);
    if (window.size() > 10) window.pop_front();
    double sum = std::accumulate(window.begin(), window.end(), 0.0);
    return sum / window.size();
}

这个方法的问题是：

它对"目标在运动"这件事毫无感知。

假设目标在匀速运动，真实位置 $p_k = 10k$ （每帧走 10m），测量噪声 $σ=2m\sigma = 2\text{m}$ ：

帧数 k:    0     1     2     3     4     5    ...
真实位置:   0    10    20    30    40    50    ...
测量值:     1     9    22    28    42    48    ...  (带 ±2m 噪声)
滑动平均:   1     5    11    15    20    25    ...  (严重滞后！)

定量来看，滑动平均的滞后误差为：

$\text{lag} = \frac{N-1}{2} \cdot v \cdot \Delta t$

对于 $N = 10$ , $v=10m/sv=10\text{m/s}$ , $Δt=1s\Delta t=1\text{s}$ ：

$\text{lag} = \frac{9}{2} \times 10 = 45 \text{ m}$

这是一个系统性的滞后误差，增大窗口 $N$ 只会让滞后更严重，减少 $N$ 则噪声抑制效果变差。

而滤波器知道："根据我的速度估计，下一时刻目标应该往前走大约 $vΔtv\Delta t$ 。"所以即使新测量没到，滤波器也能给出一个合理的预测。这就是有模型方法和无模型方法的分水岭。

滤波器的核心：权重的艺术

每个滤波器本质上都在回答同一个问题：

当预测 $xk−\mathbf{x}_k^-$ 和测量 $z_k$ 不一致时，你更相信谁？

$x^k=xk−+Kk⋅(zk−Hxk−) \boxed{\hat{\mathbf{x}}_k = \mathbf{x}_k^- + \mathbf{K}_k \cdot (z_k - \mathbf{H}\mathbf{x}_k^-)}$

所有线性滤波器都可以写成这个形式，区别只在于增益 $Kk\mathbf{K}_k$ 如何计算：

$α\alpha$ - $β\beta$ 滤波器： $β/Δt]T\mathbf{K} = [\alpha,\ \beta/\Delta t]^\mathsf{T}$ —— 固定常数，简单粗暴
Kalman Filter： $Kk=Pk−HT(HPk−HT+R)−1\mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}^\mathsf{T} (\mathbf{H} \mathbf{P}_k^- \mathbf{H}^\mathsf{T} + \mathbf{R})^{-1}$ —— 由协方差动态计算
EKF/UKF：在非线性系统中用不同的方式近似 $Kk\mathbf{K}_k$
Sliding Window Filter：不要模型，全靠历史数据拟合

这就像是你在开车时用导航：

导航说"前方 300m 右转"，但你的眼睛看到路口只有 200m $→\rightarrow$ 相信眼睛（测量）还是相信导航（模型）？
如果导航一直很准（ $Q$ 小，过程噪声低），你倾向于相信它
如果 GPS 信号不好（ $R$ 大，测量噪声高），你倾向于相信自己的判断

滤波器就是用数学语言来表达这个权衡过程。

本系列的路线图

篇次	滤波器	核心思想	难度
第 2 篇	$α\alpha$ - $β\beta$	固定增益的匀速跟踪	⭐
第 3 篇	$α\alpha$ - $β\beta$ - $γ\gamma$	加入加速度估计	⭐⭐
第 4 篇	Kalman Filter	协方差驱动的自适应增益	⭐⭐⭐
第 5 篇	EKF & UKF	非线性扩展	⭐⭐⭐⭐
第 6 篇	Sliding Window	无模型多项式拟合	⭐⭐
第 7 篇	Quality Estimator	NIS / DRMS / 跟踪质量评估	⭐⭐

每篇结构相同：

算法原理 — 公式推导结合直觉解释
代码剖析 — 直接读项目中的滤波器源码
动手实验 — 运行 demo.cpp 中的对应示例，观察效果
总结 — 优缺点和适用场景

代码准备

本系列全部代码基于项目 filters/ 目录下的滤波器库，所有滤波器都是 header-only，只需 Eigen 头文件即可编译。

cd filters
cmake -B build
cmake --build build
./build/filter_demo

推荐在阅读过程中：

打开 demo.cpp，找到对应的 demo 函数
修改参数（ $α,β,Q,R\alpha, \beta, \mathbf{Q}, \mathbf{R}$ 、窗口大小等）
观察结果变化
最好能画图（用 Python matplotlib 或直接看命令行输出的数值）

本篇小结

传感器测量总带噪声 $z_k = x_k + v_k$ ，单次测量不可靠
状态 $xk\mathbf{x}_k$ 是描述目标运动的一组数值（位置、速度等）
滤波 = 预测（Predict）+ 更新（Update）的循环：

$Predict:xk−=Fxk−1Update:x^k=xk−+Kk(zk−Hxk−) \begin{aligned} \text{Predict:}&\quad \mathbf{x}_k^- = \mathbf{F}\mathbf{x}_{k-1} \\ \text{Update:}&\quad \hat{\mathbf{x}}_k = \mathbf{x}_k^- + \mathbf{K}_k(z_k - \mathbf{H}\mathbf{x}_k^-) \end{aligned}$