CycleUser

本系列文章从移动平均到粒子滤波，逐步介绍七种经典的和现代的滤波方法。在进入正题之前，先把全系列用到的数学概念和 Python 工具集中梳理一遍。已经熟悉这些内容的读者可以跳过本篇。

随机变量与期望

随机变量 $X$ 的取值不确定，但遵循某种概率分布。其期望值（均值）定义为

$$E[X] = \int_{-\infty}^{\infty} x \, p(x) \, dx$$

期望刻画随机变量的"平均位置"。在离散情况下，积分变为求和。当样本量增大时，样本均值会收敛到期望值——这就是大数定律的内容。

图 1. 随机变量与期望：样本均值随采样数增加收敛到真实均值

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

方差与协方差

方差度量单个随机变量的离散程度：

$$\text{Var}(X) = E\left[(X - E[X])^2\right]$$

在实际计算中，我们手上有的不是概率分布，而是一组样本 ${x_1, x_2, \dots, x_N}$。样本方差的计算公式为

$$\hat{\text{Var}}(X) = \frac{1}{N-1} \sum_{i=1}^{N} (x_i - \bar{x})^2$$

其中 $\bar{x} = \frac{1}{N}\sum_i x_i$ 是样本均值。分母用 $N-1$ 而非 $N$ 是为了保证无偏估计（贝塞尔校正）。

协方差度量两个随机变量之间的线性相关性。关键在于：协方差要求 $X$ 和 $Y$ 是成对观测的——即对于每个索引 $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)$，计算 $x_i$ 偏离 $\bar{x}$ 的程度和 $y_i$ 偏离 $\bar{y}$ 的程度，把两者相乘后求和。如果 $x_i$ 大于均值时 $y_i$ 也倾向于大于均值，乘积为正，协方差为正——正相关。反之若 $x_i$ 大时 $y_i$ 倾向于小，乘积为负，协方差为负——负相关。若两者没有同步变化的趋势，正负乘积相互抵消，协方差接近零——不相关。

注意：协方差为零只意味着没有线性相关性，不代表完全独立（可能存在非线性关系）。

当 $X$ 和 $Y$ 独立时，$\text{Cov}(X, Y) = 0$。协方差为正表示正相关（$X$ 大时 $Y$ 倾向于大），为负表示负相关（$X$ 大时 $Y$ 倾向于小）。

图 2. 协方差三维总览：X、Y、Z三个变量在三维空间中的分布

图 2 展示了三个变量在三维空间中的联合分布。X 轴是共享的自变量，Y 轴是与 X 负相关的变量，Z 轴是与 X 正相关的变量。蓝色点是三维散点 ${(x_i, y_i, z_i)}_{i=1}^N$，红色点投影到 X-Y 平面，绿色点投影到 X-Z 平面，橙色点投影到 Y-Z 平面。摄像机旋转让我们从不同角度观察三维结构和投影面的关系。

图 3. X-Y平面：负协方差的逐步构建

图 3 展示了协方差的具体计算过程。每个红点是一对样本 $(x_i, y_i)$——它们在同一个索引 $i$ 处被同时观测。随着样本数增加，趋势线（黑色）逐步拟合出 $Y$ 随 $X$ 增大而减小的负相关趋势。标题中实时显示的 Cov 值就是 $\frac{1}{N-1}\sum_i (x_i - \bar{x})(y_i - \bar{y})$ 的计算结果，随着样本增多趋于稳定的负值。

图 4. X-Z平面：正协方差的逐步构建

图 4 展示了正协方差的构建过程。绿色点是成对样本 $(x_i, z_i)$。可以看到 $X$ 增大时 $Z$ 也倾向于增大，趋势线斜率为正。Cov 值逐步收敛到稳定的正值。

图 5. Y-Z平面：协方差接近零的逐步构建

图 5 展示了近零协方差的情况。橙色点是成对样本 $(y_i, z_i)$。虽然 $Y$ 与 $X$ 负相关、$Z$ 与 $X$ 正相关，但 $Y$ 和 $Z$ 之间没有直接的线性同步关系——趋势线几乎水平，Cov 值在零附近波动。这说明：两个变量可以分别与第三个变量相关，但彼此之间不一定相关。

协方差矩阵

对于随机向量 $x = [x_1, x_2, \dots, x_n]^T$，协方差矩阵 $P$ 是一个 $n \times n$ 矩阵，其第 $(i, j)$ 元素为 $\text{Cov}(x_i, x_j)$。对角元素是各分量的方差，非对角元素是分量间的协方差。协方差矩阵是对称半正定的，它完整描述了随机向量的二阶统计特性。

在本系列中，卡尔曼滤波器的状态估计 $\hat{x}_k$ 总是伴随着一个误差协方差矩阵 $P_k$，后者量化了估计的不确定性。

图 6. 协方差矩阵：不同协方差矩阵对应的二维分布与不确定性椭圆

图 6 展示了五种不同协方差矩阵对应的二维高斯分布。灰色点是采样点，红色椭圆是协方差矩阵的 2σ 置信区域。可以看到：对角矩阵对应圆形分布，非对角元素非零时椭圆倾斜，方差大的方向椭圆更长。

高斯分布

高斯（正态）分布的概率密度函数为

$$p(x) = \frac{1}{\sqrt{2\pi\sigma^2}} \exp\left(-\frac{(x - \mu)^2}{2\sigma^2}\right)$$

其中 $\mu$ 是均值，$\sigma^2$ 是方差。记作 $x \sim \mathcal{N}(\mu, \sigma^2)$。

多维情况下，$x \sim \mathcal{N}(\mu, \Sigma)$，其中 $\mu$ 是均值向量，$\Sigma$ 是协方差矩阵。高斯分布的重要性在于两条性质：第一，高斯分布经过线性变换后仍是高斯分布——如果 $x \sim \mathcal{N}(\mu, \sigma^2)$，则 $y = ax + b \sim \mathcal{N}(a\mu+b, a^2\sigma^2)$；第二，两个高斯分布的乘积归一化后仍是高斯分布。这两条性质是卡尔曼滤波可行性的数学基础：预测步是线性变换（保持高斯），更新步是先验与似然的乘积（保持高斯）。

图 7. 高斯分布：均值μ和标准差σ变化对分布形状的影响

图 7 展示了高斯分布的形状随均值 $\mu$ 和标准差 $\sigma$ 变化的过程。$\mu$ 控制分布的中心位置（左右移动），$\sigma$ 控制分布的宽度（胖瘦）。红色填充区域代表概率密度，曲线下的总面积始终为 1。

高斯分布经过线性变换后仍是高斯分布——如果 $x \sim \mathcal{N}(\mu, \sigma^2)$，则 $y = ax + b \sim \mathcal{N}(a\mu+b, a^2\sigma^2)$。

图 8. 高斯分布：线性变换保持高斯性质

图 8 展示了线性变换 $y = ax + b$ 对高斯分布的影响。左图是原始分布 $\mathcal{N}(0, 1)$（蓝色），右图是变换后的分布（红色）。随着参数 $a$ 从 1 增大到 2.5（拉伸），$b$ 从 0 增大到 2（平移），分布的钟形形状保持不变——只是变宽了、移动了。这就是"线性变换保持高斯"的含义。

两个高斯分布的乘积归一化后仍是高斯分布。这是卡尔曼滤波更新步的数学基础。

图 9. 高斯分布：两个高斯分布的乘积仍是高斯

图 9 展示了两个高斯分布相乘的过程。蓝色是先验 $\mathcal{N}(1, 4)$，绿色是似然 $\mathcal{N}(4, 1)$。红色实线是它们的逐点乘积（归一化后），黑色点线是与乘积完美重合的高斯拟合——证明乘积仍是高斯分布。乘积分布的均值位于两个分布均值之间，方差比两者都小——这意味着融合两个信息源后，估计的不确定性减小了。这正是卡尔曼滤波每次更新时发生的事情。

傅里叶变换

傅里叶变换的核心思想是：任何信号都可以分解为一系列正弦波的叠加。每个正弦波有固定的频率和振幅。傅里叶变换就是把时域信号"拆"成这些正弦波分量，告诉我们信号里含有哪些频率、每个频率有多强。

离散傅里叶变换（DFT）的公式为：

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

直觉上，这个公式在做一件事：对每个频率 $k$，把信号 $x[n]$ 和该频率的正弦波做"对比"（内积），得到的结果 $X[k]$ 就是这个频率分量的强度。

在 Python 中用 np.fft.rfft(x) 计算实数信号的频域系数，用 np.fft.irfft(X, n=N) 做逆变换。维纳滤波器在频域中设计传递函数，需要用到傅里叶变换。

功率谱密度 $S(\omega)$ 描述信号功率在各频率上的分布，是信号自相关函数的傅里叶变换。

图 10. 傅里叶变换：两个独立的正弦波

图 10 展示了两个独立的正弦波。蓝色是一个低频波（4 个周期，振幅 1.0），绿色是一个高频波（16 个周期，振幅 0.4）。它们看起来完全不同——一个缓慢起伏，一个快速振荡。

图 11. 傅里叶变换：两个正弦波叠加成合成信号

图 11 展示了把两个正弦波逐点相加的过程。蓝色和绿色是原始的两个波（半透明），红色是它们的和——一个看起来不太规则的合成信号。这个信号同时包含了低频和高频两种成分，但在时域上很难直接看出它们各自是什么。

图 12. 傅里叶变换：频谱精确分解出原始频率

图 12 展示了对合成信号做傅里叶变换后的频谱。随着数据长度增加，频谱在两个对应频率处出现越来越清晰的峰值——低频峰值高（对应振幅 1.0），高频峰值低（对应振幅 0.4）。傅里叶变换精确地把混合信号"拆"回了原始的两个频率分量。这就是傅里叶变换的核心能力：从时域的混合信号中提取出各频率分量的强度。

Python 工具

NumPy

NumPy 是 Python 数值计算的基础库。本系列用到的核心功能：

import numpy as np

# 创建数组
x = np.zeros(4)              # 零向量
P = np.eye(4) * 50           # 对角矩阵
A = np.array([[1, 2], [3, 4]])

# 矩阵运算
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)  # 从多元高斯分布采样
noise = np.random.randn(N)                    # 标准正态随机数

# 信号处理
est = np.convolve(obs, np.ones(W)/W, mode='same')  # 卷积（移动平均）
X = np.fft.rfft(x)            # 实数傅里叶变换
x = np.fft.irfft(X, n=N)     # 逆傅里叶变换

# 统计
mean = np.sum(weights[:, None] * particles, axis=0)  # 加权平均

Matplotlib

Matplotlib 是 Python 的绘图库。本系列用到的核心功能：

import matplotlib.pyplot as plt

# 静态图
fig, ax = plt.subplots(figsize=(8, 6))
ax.plot(t, true, 'k-', lw=1.0, label='True')
ax.scatter(x, y, s=10, c='gray', alpha=0.3)
ax.set_xlabel('x'); ax.set_ylabel('y')
ax.set_title('Title'); ax.legend(); ax.grid(alpha=0.3)

# 保存
plt.savefig('images/output.svg', format='svg', bbox_inches='tight')
plt.savefig('images/output.png', dpi=150, bbox_inches='tight')

FuncAnimation（GIF 动画）

from matplotlib.animation import FuncAnimation

fig, ax = plt.subplots(figsize=(8, 6))
line, = ax.plot([], [], 'r-', lw=1.5)

def update(frame):
    line.set_data(t[:frame], est[:frame])
    ax.set_title(f'Frame {frame}')
    return line,

ani = FuncAnimation(fig, update, frames=range(0, N, 2),
                    interval=120, blit=False)
ani.save('images/output.gif', writer='pillow', fps=8, dpi=72)

每帧调用 update 函数，返回需要重绘的艺术家对象。blit=False 表示每帧重绘整个画布（简单但稍慢）。

Ellipse（不确定性椭圆）

卡尔曼滤波器的状态估计伴随协方差矩阵，其二维投影可用椭圆可视化：

from matplotlib.patches import Ellipse

cov_xy = P[[0, 2]][:, [0, 2]]       # 提取 x,y 子块
vals, vecs = np.linalg.eigh(cov_xy)  # 特征值分解
angle = np.degrees(np.arctan2(vecs[1, 0], vecs[0, 0]))
ell = Ellipse((x_est, y_est),
              2*np.sqrt(vals[0]), 2*np.sqrt(vals[1]),
              angle=angle, fc='red', alpha=0.1, ec='red')
ax.add_patch(ell)

椭圆的两个半轴长度等于协方差矩阵特征值的平方根的两倍（对应 2σ 置信区域），旋转角度由特征向量决定。

CSV 输出

import csv

with open('images/output_data.csv', 'w', newline='') as f:
    w = csv.writer(f)
    w.writerow(['k', 'true', 'obs', 'estimate'])  # 表头
    for k in range(N):
        w.writerow([k, f'{true[k]:.4f}', f'{obs[k]:.4f}', f'{est[k]:.4f}'])

图 13. Python工具综合演示：轨迹追踪与不确定性椭圆动画

图 13 综合演示了本系列用到的 Python 工具：NumPy 生成模拟曲线数据、矩阵运算计算动态协方差、Matplotlib 绘制轨迹和散点、Ellipse 绘制不确定性椭圆。红色椭圆的大小、形状和旋转角度随位置和时间不断变化——在轨迹弯曲处，不确定性在横向拉长；在初始阶段，椭圆较大表示估计尚不精确。这正是卡尔曼滤波的核心输出：每个时刻不仅有状态的最佳估计，还有随时间动态变化的误差协方差。