CycleUser

数学基础

滤波，在最广泛的意义上，是从含噪声的观测序列中估计感兴趣信号的过程。这个概念横跨信号处理、控制理论、时间序列分析和统计学，但核心目标高度统一：从杂乱无章的测量值中还原出隐藏其后的真实状态或趋势。

在进入具体的滤波方法之前，需要先建立几个基本概念。

随机变量与期望

随机变量 $X$ 的取值不确定，但遵循某种概率分布。期望值 $E[X] = \int x \, p(x) \, dx$ 刻画随机变量的"平均位置"。当样本量增大时，样本均值会收敛到期望值——这是大数定律的内容。

图 1 展示了从高斯分布 $\mathcal{N}(5, 4)$ 中逐步采样的过程。灰色点是每次抽到的样本值，红色线是样本均值的累计值，逐步收敛到真实期望 $\mu = 5$。

方差与协方差

方差度量单个随机变量的离散程度。样本方差 $\hat{\text{Var}}(X) = \frac{1}{N-1} \sum_i (x_i - \bar{x})^2$。协方差度量两个变量之间的线性相关性，要求两个变量是成对观测的——对于每个索引 $i$，同时观测到 $x_i$ 和 $y_i$：

$$\hat{\text{Cov}}(X, Y) = \frac{1}{N-1} \sum_{i=1}^{N} (x_i - \bar{x})(y_i - \bar{y})$$

若 $x_i$ 大于均值时 $y_i$ 也倾向于大于均值，则协方差为正——正相关。反之若方向相反则为负相关。无同步变化趋势时协方差接近零。

图 2 在三维空间中展示三个变量的联合分布。图 3-5 分别在 X-Y、X-Z、Y-Z 平面上展示负协方差、正协方差和近零协方差的逐步构建过程。

协方差矩阵

对于随机向量 $x = [x_1, \dots, x_n]^T$，协方差矩阵 $P$ 是一个 $n \times n$ 矩阵，其第 $(i, j)$ 元素为 $\text{Cov}(x_i, x_j)$。对角元素是方差，非对角元素是协方差。协方差矩阵是对称半正定的，完整描述了随机向量的二阶统计特性。卡尔曼滤波器的每个状态估计 $\hat{x}_k$ 都伴随一个误差协方差矩阵 $P_k$。

图 6 展示了五种协方差矩阵对应的二维分布与 2σ 置信椭圆。

高斯分布

高斯分布 $p(x) = \frac{1}{\sqrt{2\pi\sigma^2}} e^{-(x-\mu)^2/(2\sigma^2)}$。多维情况下 $x \sim \mathcal{N}(\mu, \Sigma)$。高斯分布的两条关键性质是卡尔曼滤波的数学基础：线性变换下的高斯分布仍是高斯分布；两个高斯分布的乘积归一化后仍是高斯分布。

图 7 展示 $\mu$ 和 $\sigma$ 对高斯分布的影响。图 8 展示线性变换 $y = ax + b$ 保持高斯性质。图 9 展示两个高斯分布 $\mathcal{N}(1,4)$ 与 $\mathcal{N}(4,1)$ 的乘积——红色是结果，黑色是完美重合的高斯拟合。

傅里叶变换

傅里叶变换的核心思想是：任何信号可分解为一系列正弦波的叠加。每个正弦波有固定频率和振幅：

$$X[k] = \sum_{n=0}^{N-1} x[n] \, e^{-j 2\pi k n / N}$$

逆变换将频域信息还原为时域信号。维纳滤波器在频域中设计传递函数，需要用到傅里叶变换。

图 10 展示两个独立正弦波。图 11 展示它们叠加成合成信号。图 12 展示傅里叶频谱——两个峰精确对应两个原始频率和振幅。

Python 工具概述

import numpy as np
from scipy import linalg
import sympy as sp

# 矩阵运算
C = A @ B; At = A.T; Ainv = np.linalg.inv(A)
vals, vecs = np.linalg.eigh(P)  # 对称矩阵特征值分解
L = np.linalg.cholesky(P)       # Cholesky 分解
w = np.random.multivariate_normal(mean, cov)  # 多元高斯采样

# 信号处理
est = np.convolve(obs, np.ones(W)/W, mode='same')  # 卷积
X = np.fft.rfft(x);  x = np.fft.irfft(X, n=N)     # 傅里叶变换

# 符号计算 (sympy)
x_sym = sp.symbols('x')
sp.diff(x_sym**2 + 1, x_sym)  # 求导 → 2*x
sp.integrate(sp.exp(-x_sym**2), x_sym)  # 符号积分

图 13 综合演示了本系列用到的 Python 工具：NumPy 生成模拟数据、矩阵运算计算动态协方差、Matplotlib 绘制轨迹和散点、Ellipse 绘制不确定性椭圆。椭圆的大小、形状和旋转角度随位置和时间动态变化。

一、移动平均滤波

与基本概念的衔接

有了期望和方差的概念之后，最简单的滤波直觉就是取平均——用多个观测的均值代替单个含噪声的观测，来逼近真实值。

定义

移动平均（Moving Average）是最古老的滤波器。第 $k$ 时刻的估计定义为最近 $W$ 个观测值的算术平均：

$$\hat{x}k = \frac{1}{W} \sum$$}^{W-1} z_{k-j

唯一的参数是窗口宽度 $W$。$W$ 越大，输出越平滑，但对真实变化的响应越迟缓——这是平滑度与响应速度的基本权衡。

移动平均本质上是有限脉冲响应（FIR）滤波器，频率响应为

$$H(\omega) = \frac{1}{W} \cdot \frac{\sin(\omega W/2)}{\sin(\omega/2)} \cdot e^{-j\omega (W-1)/2}$$

$\sin(\omega W/2)/\sin(\omega/2)$ 是 Dirichlet 核，在主瓣外有衰减旁瓣，会引入振铃效应。

可视化

图 14 展示了窗口宽度从 1 逐步增加到 30 的过程中，滤波输出如何在平滑与滞后之间变化。窗口越宽，曲线越平滑但越滞后于真实变化。

Python 实现

import numpy as np

def moving_average(obs, window):
    """移动平均滤波：obs 为观测序列，window 为窗口宽度"""
    kernel = np.ones(window) / window
    return np.convolve(obs, kernel, mode='same')

# 示例
obs = np.sin(np.linspace(0, 4*np.pi, 100)) + np.random.randn(100) * 0.3
est_w3  = moving_average(obs, 3)
est_w10 = moving_average(obs, 10)

局限与演化

移动平均有三个内在局限：窗口内所有观测权重相等，不给近期数据更高权重；需要存储整段窗口历史数据；对趋势信号的跟踪天然滞后。这直接导向了指数平滑——用一条递推公式同时解决前两个问题。

二、指数平滑滤波

与移动平均的对比

移动平均给窗口内每个观测权重 $1/W$，窗口外的权重为零——是一个矩形窗。指数平滑把这个权重函数换成指数衰减：越近的观测权重越大，且只需存储上一个估计值。

递推公式

$$\hat{x}k = \alpha \cdot z_k + (1 - \alpha) \cdot \hat{x}$$

参数 $\alpha \in (0, 1)$ 是平滑因子。展开可看到权重呈几何衰减：$\hat{x}k = \alpha \sum$。}^{\infty} (1 - \alpha)^j z_{k-j

指数平滑是控制理论中一阶低通滤波器的离散等价。其幅频响应为

$$|H(\omega)| = \frac{\alpha}{\sqrt{1 + (1 - \alpha)^2 - 2(1 - \alpha) \cos \omega}}$$

与移动平均的多旁瓣结构不同，指数平滑呈现平滑衰减的单峰特性，没有振铃。

可视化

图 15 演示 $\alpha$ 从 0.02 连续变化到 0.98 的过程中，滤波器从极度平滑到紧密追踪的转变。

Python 实现

import numpy as np

def exponential_smoothing(obs, alpha):
    """指数平滑滤波：递推实现"""
    est = np.zeros_like(obs)
    est[0] = obs[0]
    for k in range(1, len(obs)):
        est[k] = alpha * obs[k] + (1 - alpha) * est[k-1]
    return est

# 示例
est_01 = exponential_smoothing(obs, 0.1)  # 强平滑
est_07 = exponential_smoothing(obs, 0.7)  # 紧密追踪

局限与演化

与移动平均相比，指数平滑在两个维度上有了实质进步：权重分配更合理，计算和存储更经济。但两者共享同一个根本局限：它们都只能做单变量滤波，无法处理具有内部动力学的多维系统，也没有使用信号和噪声的统计特性。这导向了维纳滤波——在频域中利用统计特性设计全局最优滤波器。

三、维纳滤波

与指数平滑的对比

移动平均和指数平滑的滤波器系数完全靠经验调参（$W$ 或 $\alpha$），没有任何"最优"保证。维纳滤波第一次引入了统计最优性：它需要知道信号和噪声的功率谱密度，然后直接算出使均方误差最小的传递函数。

频域中的最优解

给定含噪观测 $z(t) = s(t) + n(t)$，维纳滤波的设计目标是使均方误差最小：

$$\min_h \; E\left[ (s(t) - h(t) * z(t))^2 \right]$$

正交性原理导出了 Wiener–Hopf 积分方程。在频域中，非因果维纳滤波器的传递函数为

$$H(\omega) = \frac{S_{ss}(\omega)}{S_{ss}(\omega) + S_{nn}(\omega)}$$

其中 $S_{ss}(\omega)$ 和 $S_{nn}(\omega)$ 分别是信号和噪声的功率谱密度。直觉：信噪比高的频率分量上 $H \approx 1$（通过），信噪比低的频率分量上 $H \approx 0$（抑制）。

可视化

图 16 展示了维纳滤波的工作过程：在频域中根据功率谱计算最优传递函数 $H(\omega)$，应用后逆变换回时域，低频信号保留、高频噪声被压制。

Python 实现

import numpy as np

def wiener_filter(obs, signal_psd, noise_psd):
    """频域维纳滤波：signal_psd 和 noise_psd 为功率谱密度数组"""
    X = np.fft.rfft(obs)
    H = signal_psd / (signal_psd + noise_psd + 1e-10)
    return np.fft.irfft(X * H, n=len(obs))

# 示例：假设已知信号和噪声的功率谱
freqs = np.fft.rfftfreq(len(obs))
S_signal = 1.0 / (1.0 + (freqs * 80)**2)  # 低频信号功率高
S_noise  = np.full_like(S_signal, 0.5)     # 白噪声
filtered = wiener_filter(obs, S_signal, S_noise)

局限与演化

维纳滤波相比前两种方法有了质的飞跃：第一次引入了统计最优性。但它的三个致命局限阻碍了实际应用：要求信号和噪声平稳、需要离线批量处理整段数据、无法处理多维耦合状态。这直接导向了卡尔曼滤波——用状态空间方法替代频域方法，实现实时递推的多维最优估计。

四、卡尔曼滤波

与维纳滤波的对比

维纳滤波在频域中设计最优滤波器，需要完整的功率谱密度，只能离线处理。卡尔曼滤波（1960）把问题转换到时域：用状态方程描述系统动力学，用观测方程描述测量过程，以递推方式在每个时刻仅用当前观测更新估计。两者在平稳线性高斯的特殊情况下等价，但卡尔曼滤波的适用范围远大于维纳滤波——它支持非平稳信号、多维耦合状态、实时在线计算。

状态空间模型

卡尔曼滤波将问题建模为一个离散线性动态系统：

$$x_k = F_k x_{k-1} + w_k, \quad w_k \sim \mathcal{N}(0, Q_k)$$ $$z_k = H_k x_k + v_k, \quad v_k \sim \mathcal{N}(0, R_k)$$

$F_k$ 是状态转移矩阵，$H_k$ 是观测矩阵。过程噪声 $w_k$ 和观测噪声 $v_k$ 为独立零均值高斯白噪声。

预测-更新递推

预测步——按系统模型推演状态和不确定性的先验估计：

$$\hat{x}{k|k-1} = F_k \hat{x}$$ $$P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k$$

更新步——用实际观测修正先验估计得到后验估计：

$$K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}$$ $$\hat{x}{k|k} = \hat{x}} + K_k (z_k - H_k \hat{x{k|k-1})$$ $$P$$} = (I - K_k H_k) P_{k|k-1

$K_k$ 是卡尔曼增益矩阵。以标量情况为例，$K_k = p / (p + R)$。当观测噪声 $R$ 很大时 $K \approx 0$（忽略观测），当 $R$ 很小时 $K \approx 1$（信任观测）。在贝叶斯视角下，预测步给出先验分布，更新步将先验与似然相乘得到后验——这是线性高斯系统下的精确贝叶斯递推。

可视化

图 17 展示了二维匀速运动目标的卡尔曼滤波追踪过程。红线是滤波估计，黑线是真实轨迹，红色椭圆是误差协方差矩阵的 2σ 置信区域，随时间逐渐缩小。

Python 实现

import numpy as np

def kalman_filter(z_obs, F, H, Q, R, x0, P0):
    """卡尔曼滤波：z_obs (N,m), F (n,n), H (m,n), Q (n,n), R (m,m)"""
    N = len(z_obs)
    n = F.shape[0]
    x_est = np.zeros((N, n))
    x, P = x0, P0
    for k in range(N):
        x_pred = F @ x
        P_pred = F @ P @ F.T + Q
        S = H @ P_pred @ H.T + R
        K = P_pred @ H.T @ np.linalg.inv(S)
        x = x_pred + K @ (z_obs[k] - H @ x_pred)
        P = (np.eye(n) - K @ H) @ P_pred
        x_est[k] = x
    return x_est

# 示例：二维匀速运动追踪
dt = 1.0
F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]])
H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
Q = np.eye(4) * 0.05
R = np.eye(2) * 10
x0, P0 = np.zeros(4), np.eye(4) * 50
est = kalman_filter(z_obs, F, H, Q, R, x0, P0)

局限与演化

卡尔曼滤波在线性高斯假设下是最优递推估计器。但实际系统中 $f$ 和 $h$ 往往是非线性函数——雷达测量的是距离和方位角，飞行器的动力学涉及姿态旋转。一旦非线性，标准卡尔曼方程中的矩阵乘法不再适用。最直接的推广思路是将非线性函数在当前估计点做一阶泰勒展开，用雅可比矩阵替代 $F_k$ 和 $H_k$——这就是扩展卡尔曼滤波（EKF）。

五、扩展卡尔曼滤波（EKF）

与卡尔曼滤波的对比

标准卡尔曼滤波的预测步 $\hat{x}{k|k-1} = F_k \hat{x}}$ 是矩阵乘法。EKF 将其替换为非线性函数的直接求值 $\hat{x{k|k-1} = f(\hat{x})$。雅可比矩阵仅在协方差传播中使用——近似描述不确定性如何通过非线性函数变换。})$，残差变为 $z_k - h(\hat{x}_{k|k-1

雅可比矩阵

设非线性系统为 $x_k = f(x_{k-1}) + w_k$，$z_k = h(x_k) + v_k$。EKF 在每一时刻计算雅可比矩阵：

$$F_k = \left. \frac{\partial f}{\partial x} \right|{\hat{x} \right|}}, \quad H_k = \left. \frac{\partial h}{\partial x{\hat{x}$$}

雅可比矩阵是多元函数的导数推广——其第 $(i,j)$ 元素为 $f_i$ 对 $x_j$ 的偏导数，代表非线性函数在某一点附近的局部线性近似。

预测步 $\hat{x}{k|k-1} = f(\hat{x})$，$H_k$ 替换为观测函数的雅可比矩阵。})$，$P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k$。更新步与标准卡尔曼相同，但残差为 $z_k - h(\hat{x}_{k|k-1

可视化

图 18 对比了线性 KF（匀速直线模型）和 EKF（非线性转弯模型 + 雅可比矩阵）对圆周运动目标的追踪。EKF 的轨迹更紧密贴合真实圆周路径。

Python 实现

import numpy as np
from scipy.linalg import inv

def ekf(z_obs, f, h, F_jac, H_jac, Q, R, x0, P0):
    """扩展卡尔曼滤波：f/h 为非线性函数，F_jac/H_jac 返回雅可比矩阵"""
    N, n = len(z_obs), len(x0)
    x_est = np.zeros((N, n))
    x, P = x0, P0
    for k in range(N):
        Fk = F_jac(x)
        x_pred = f(x)
        P_pred = Fk @ P @ Fk.T + Q
        Hk = H_jac(x_pred)
        S = Hk @ P_pred @ Hk.T + R
        K = P_pred @ Hk.T @ inv(S)
        x = x_pred + K @ (z_obs[k] - h(x_pred))
        P = (np.eye(n) - K @ Hk) @ P_pred
        x_est[k] = x
    return x_est

# 示例：雷达追踪（观测 = 距离 + 方位角）
def h_radar(x):
    px, py = x[0], x[1]
    return np.array([np.hypot(px, py), np.arctan2(py, px)])

def H_jac_radar(x):
    px, py = x[0], x[1]
    r = np.hypot(px, py) + 1e-10
    H = np.zeros((2, 4))
    H[0, 0] = px / r;  H[0, 1] = py / r
    H[1, 0] = -py / r**2;  H[1, 1] = px / r**2
    return H

局限与演化

EKF 把卡尔曼滤波从线性推广到了非线性，是工程实践中最广泛使用的非线性滤波器。但它有两个根本缺陷：一阶泰勒展开在强非线性下截断误差大，且要求函数处处可微。如果不去近似函数本身，而是用一组确定性的采样点通过非线性函数后重建分布的均值和协方差，就避开了雅可比计算——这就是无迹卡尔曼滤波（UKF）。

六、无迹卡尔曼滤波（UKF）

与 EKF 的对比

EKF 近似的是函数——用切线代替曲线。UKF 近似的是分布——用一组采样点（西格玛点）代替高斯分布，让这些点各自通过真实的非线性函数，再用变换后的点集重建新的均值和协方差。对高斯分布而言，UKF 的精度达到三阶泰勒展开等价，而 EKF 仅为一阶。

西格玛点采样

对于 $n$ 维状态，在均值和协方差周围选取 $2n+1$ 个西格玛点：

$$\mathcal{X}_0 = \hat{x}, \quad \mathcal{X}_i = \hat{x} \pm \left( \sqrt{(n+\lambda)P} \right)_i$$

尺度参数 $\lambda = \alpha^2(n+\kappa) - n$，$\sqrt{(n+\lambda)P}$ 通过 Cholesky 分解 $P = L L^T$ 后取列得到。权重为 $W_0^{(m)} = \lambda/(n+\lambda)$，$W_i^{(m)} = 1/[2(n+\lambda)]$。

预测步：将西格玛点通过 $f$，加权重建 $\hat{x}{k|k-1} = \sum_i W_i^{(m)} f(\mathcal{X}_i)$ 和 $P][\cdots]^T + Q_k$。} = \sum_i W_i^{(c)} [f(\mathcal{X}_i) - \hat{x

更新步：将西格玛点通过 $h$，计算 $\hat{z}{k|k-1}$、$P}$、$P_{xz}$，然后 $K_k = P_{xz} P_{zz}^{-1}$，$\hat{x{k|k} = \hat{x})$。} + K_k (z_k - \hat{z}_{k|k-1

可视化

图 19 在同一个非线性追踪场景中对比了 EKF 和 UKF 的表现。UKF 通过西格玛点传播分布，不需要计算雅可比矩阵，在转弯模型下的估计精度明显优于 EKF。

Python 实现

import numpy as np
from scipy.linalg import inv, cholesky

def ukf(z_obs, f, h, Q, R, x0, P0, alpha=1e-3, kappa=0):
    """无迹卡尔曼滤波"""
    n = len(x0); lam = alpha**2 * (n + kappa) - n
    wm = np.full(2*n+1, 1/(2*(n+lam))); wm[0] = lam/(n+lam)
    wc = wm.copy(); wc[0] += 1 - alpha**2 + 2  # beta=2 for Gaussian
    N = len(z_obs)
    x_est = np.zeros((N, n)); x, P = x0, P0
    for k in range(N):
        L = cholesky((n + lam) * P + 1e-6*np.eye(n), lower=True)
        sig = np.zeros((2*n+1, n)); sig[0] = x
        for i in range(n):
            sig[i+1] = x + L[:, i]; sig[i+1+n] = x - L[:, i]
        f_sig = np.array([f(s) for s in sig])
        x_pred = np.sum(wm[:, None] * f_sig, axis=0)
        P_pred = Q + sum(wc[i] * np.outer(f_sig[i]-x_pred, f_sig[i]-x_pred) for i in range(2*n+1))
        h_sig = np.array([h(s) for s in sig])
        z_pred = np.sum(wm[:, None] * h_sig, axis=0)
        Pzz = R + sum(wc[i] * np.outer(h_sig[i]-z_pred, h_sig[i]-z_pred) for i in range(2*n+1))
        Pxz = sum(wc[i] * np.outer(f_sig[i]-x_pred, h_sig[i]-z_pred) for i in range(2*n+1))
        K = Pxz @ inv(Pzz)
        x = x_pred + K @ (z_obs[k] - z_pred)
        P = P_pred - K @ Pzz @ K.T
        x_est[k] = x
    return x_est

局限与演化

UKF 相比 EKF 是显著进步：不需要计算雅可比矩阵、精度更高、可处理不可微函数。但 EKF 和 UKF 共享一个根本假设——后验分布可以用高斯分布近似表示。当噪声严重非高斯（多峰、重尾）或后验本身高度非高斯时，两者都会失效。要彻底放弃高斯假设，需要用大量随机样本直接表示任意分布——这就是粒子滤波。

七、粒子滤波

与 UKF 的对比

EKF 用雅可比矩阵近似非线性函数，UKF 用西格玛点近似高斯分布通过非线性函数的变换。两者都假设后验是高斯的。粒子滤波不做任何参数化假设：它用 $N$ 个带权重的粒子 ${x_k^{(i)}, w_k^{(i)}}$ 直接近似后验分布，无论分布形状如何——单峰、多峰、重尾、偏斜——都能表示。

$$p(x_k \mid z_{1:k}) \approx \sum_{i=1}^N w_k^{(i)} \delta(x_k - x_k^{(i)})$$

三步递推

粒子滤波的每步递推包含三个基本操作。

重要性采样：从提议分布中抽取新粒子。最简单的选择是使用状态转移分布 $x_k^{(i)} \sim p(x_k \mid x_{k-1}^{(i)})$——按系统模型将每个粒子推进一步并加入过程噪声。

权重更新：根据观测似然赋权 $w_k^{(i)} \propto w_{k-1}^{(i)} \cdot p(z_k \mid x_k^{(i)})$，归一化后 $\sum_i w_k^{(i)} = 1$。

重采样：依权重复制高权重粒子、淘汰低权重粒子，以避免粒子退化——少数粒子占据绝大部分权重。

可视化

图 20 展示了 500 个粒子在方位角观测下的追踪过程。蓝色点代表粒子集合，红色线代表加权估计。粒子云从发散的初始分布逐步收敛到真实轨迹附近。

Python 实现

import numpy as np

def particle_filter(z_obs, f, h, Q, R_std, n_particles, state_dim):
    """粒子滤波：f 为状态转移，h 为观测函数，R_std 为观测噪声标准差"""
    N = len(z_obs)
    particles = np.random.randn(n_particles, state_dim) * 5  # 初始散布
    weights = np.ones(n_particles) / n_particles
    x_est = np.zeros((N, state_dim))
    for k in range(N):
        # 重要性采样
        particles = np.array([f(p) + np.random.multivariate_normal(np.zeros(state_dim), Q)
                              for p in particles])
        # 权重更新
        z_pred = np.array([h(p) for p in particles])
        innov = z_obs[k] - z_pred
        weights = np.exp(-0.5 * np.sum((innov / R_std)**2, axis=1 if innov.ndim>1 else 0))
        weights /= np.sum(weights) + 1e-300
        # 状态估计
        x_est[k] = np.sum(weights[:, None] * particles, axis=0)
        # 系统重采样
        cdf = np.cumsum(weights); cdf[-1] = 1.0
        u = np.random.uniform(0, 1/n_particles)
        idx = np.searchsorted(cdf, u + np.arange(n_particles)/n_particles)
        particles = particles[idx]
        weights = np.ones(n_particles) / n_particles
    return x_est

各方法总结

从移动平均到粒子滤波，是统计学和控制理论近一个世纪的主线：逐步放宽假设、提升精度、增加计算代价。

在实践中选择何种方法，取决于问题的线性程度、噪声分布、实时性要求和可用计算资源的综合权衡。