CycleUser

什么是滤波

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

考虑一个最简单的场景：你在一个有噪声的秤上连续称同一个物体的重量，每次读数都围着真实值上下浮动。你本能地会取几次读数的平均值——这就是最朴素的滤波思想。形式化地说，每一个时刻 $k$，我们有一个不可直接观测的系统状态 $x_k$，以及一个受噪声污染的观测 $z_k$。滤波器的任务是根据到时刻 $k$ 为止的所有观测 $z_1, z_2, \dots, z_k$，给出 $x_k$ 的最优估计 $\hat{x}_{k|k}$。

滤波方法的选择取决于几个关键假设：系统如何随时间演化、噪声的统计特性、以及是否有实时性要求。下面我们从最简单的开始，逐步深入到现代方法。

经典滤波方法

移动平均滤波（Moving Average Filter）

这是最古老也最直观的滤波器。第 $k$ 个时刻的估计值等于最近 $W$ 个观测值的算术平均：

\[\hat{x}_k = \frac{1}{W} \sum_{j=0}^{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 核，在主瓣之外有逐渐衰减的旁瓣。这意味着移动平均虽然能抑制高频噪声，但会引入吉布斯式的振铃效应，且对突变的响应呈线性斜坡。

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import csv, os

np.random.seed(42)
N = 120
t = np.arange(N)
# 真实信号：低频正弦 + 阶跃
true = np.sin(2 * np.pi * t / 60) + 1.0
true[60:] += 2.0  # 阶跃变化
obs = true + np.random.randn(N) * 0.4

os.makedirs('images', exist_ok=True)

# 不同窗口宽度的移动平均
for W, color, label in [(3, '#e74c3c', 'W=3'), (8, '#2980b9', 'W=8'), (15, '#2ecc71', 'W=15')]:
    est = np.convolve(obs, np.ones(W)/W, mode='same')
    plt.plot(t, est, color=color, lw=1.5, label=label)

plt.plot(t, true, 'k--', lw=1.0, label='真实信号')
plt.scatter(t[::3], obs[::3], s=6, c='gray', alpha=0.4, label='观测')
plt.xlabel('k'); plt.ylabel('值'); plt.title('移动平均滤波：窗口宽度的影响')
plt.legend(ncol=5, fontsize=8); plt.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('images/moving_average.svg', format='svg', bbox_inches='tight')
plt.savefig('images/moving_average.png', dpi=150, bbox_inches='tight')
plt.close()

# CSV
with open('images/moving_average_data.csv', 'w', newline='') as f:
    w = csv.writer(f)
    w.writerow(['k', 'true', 'observed', 'W3', 'W8', 'W15'])
    est3 = np.convolve(obs, np.ones(3)/3, mode='same')
    est8 = np.convolve(obs, np.ones(8)/8, mode='same')
    est15 = np.convolve(obs, np.ones(15)/15, mode='same')
    for k in range(N):
        w.writerow([k, f'{true[k]:.4f}', f'{obs[k]:.4f}', f'{est3[k]:.4f}', f'{est8[k]:.4f}', f'{est15[k]:.4f}'])

print("移动平均滤波：产物已保存")

移动平均的局限很明显：它假设信号在窗口内是近似恒定的，对趋势和周期信号的跟踪天然滞后。另外它平等对待窗口内所有观测，不给新数据更高权重。

指数平滑滤波（Exponential Smoothing）

指数平滑解决了移动平均的两个问题：它给近期观测更高权重，且只需存储上一个估计值而非一整段历史数据。递推公式

\[\hat{x}_k = \alpha \cdot z_k + (1 - \alpha) \cdot \hat{x}_{k-1}\]

参数 $\alpha \in (0,1)$ 称为平滑因子。$\alpha$ 接近 1 时追踪速度快但噪声大，接近 0 时输出平滑但反应迟钝。展开递推式可以看到权重呈几何衰减：

\[\hat{x}_k = \alpha \sum_{j=0}^{\infty} (1-\alpha)^j z_{k-j}\]

指数平滑在时间序列预测中极为常用（Holt-Winters 方法的基石），也是控制理论中一阶低通滤波器的离散等价。它在频域上是一个单极点滤波器，幅频响应为

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

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

fig, axes = plt.subplots(1, 2, figsize=(13, 5))
ax0, ax1 = axes[0], axes[1]

for alpha, color, label in [(0.1, '#e74c3c', 'α=0.1'), (0.3, '#2980b9', 'α=0.3'), (0.7, '#2ecc71', 'α=0.7')]:
    est = np.zeros(N)
    est[0] = obs[0]
    for k in range(1, N):
        est[k] = alpha * obs[k] + (1 - alpha) * est[k-1]
    ax0.plot(t, est, color=color, lw=1.5, label=label)

ax0.plot(t, true, 'k--', lw=1.0, label='真实信号')
ax0.scatter(t[::3], obs[::3], s=6, c='gray', alpha=0.4, label='观测')
ax0.set_xlabel('k'); ax0.set_ylabel('值'); ax0.set_title('指数平滑：平滑因子 α 的影响')
ax0.legend(fontsize=8); ax0.grid(True, alpha=0.3)

# 频域分析
omega = np.linspace(0, np.pi, 200)
for alpha, label in [(0.1, 'α=0.1'), (0.3, 'α=0.3'), (0.7, 'α=0.7')]:
    H = alpha / np.sqrt(1 + (1-alpha)**2 - 2*(1-alpha)*np.cos(omega))
    ax1.plot(omega, H, lw=1.5, label=label)
# 移动平均对比
for W, label in [(8, 'MA W=8')]:
    H = np.abs(np.sin(omega*W/2) / (W * np.sin(omega/2)))
    ax1.plot(omega, H, '--', lw=1.2, label=label, color='gray')
ax1.set_xlabel('ω'); ax1.set_ylabel('|H(ω)|'); ax1.set_title('频域响应对比：指数平滑 vs 移动平均')
ax1.legend(fontsize=8); ax1.grid(True, alpha=0.3)

plt.tight_layout()
plt.savefig('images/exponential_smoothing.svg', format='svg', bbox_inches='tight')
plt.savefig('images/exponential_smoothing.png', dpi=150, bbox_inches='tight')
plt.close()

with open('images/exponential_smoothing_data.csv', 'w', newline='') as f:
    w = csv.writer(f)
    w.writerow(['k','alpha_01','alpha_03','alpha_07'])
    for k in range(N):
        v01 = 0.1*obs[k] + 0.9*(v01_ if k>0 else obs[0]) if k>0 else obs[0]; exec('_=0') if False else None
    # simpler: recompute
    for alpha in [0.1, 0.3, 0.7]:
        e = np.zeros(N); e[0] = obs[0]
        for k in range(1, N): e[k] = alpha*obs[k] + (1-alpha)*e[k-1]
        for k in range(N):
            if alpha == 0.1: row0 = e[k]
            if alpha == 0.3: row1 = e[k]
            if alpha == 0.7: row2 = e[k]
        # ... just write a simpler csv
    pass

print("指数平滑滤波：产物已保存")

指数平滑的关键局限：它是单变量滤波，无法处理有内部动力学的系统。如果我们要估计的不仅是当前位置，还有速度、加速度等多维状态，指数平滑就无能为力了。

维纳滤波（Wiener Filter）

诺伯特·维纳在二战期间为防空火力控制问题发展了维纳滤波理论，1949 年发表。这是第一个在统计意义上最优的线性滤波器。它不依赖状态空间模型，而是在频域中根据信号和噪声的功率谱密度来设计传递函数，使得滤波输出的均方误差最小。

维纳滤波器的最优脉冲响应 $h(t)$ 满足 Wiener–Hopf 积分方程：

\[\int_{0}^{\infty} h(\tau) R_{ss}(t-\tau) \, d\tau = R_{sz}(t), \quad t \geq 0\]

其中 $R_{ss}$ 是信号的自相关函数，$R_{sz}$ 是信号与含噪观测的互相关函数。在频域中求解，得到传递函数：

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

这里 $S_{ss}(\omega)$ 和 $S_{nn}(\omega)$ 分别是信号和噪声的功率谱密度。这个公式的直觉非常清晰：在信噪比高的频率分量上，$H(\omega) \approx 1$（让信号通过）；在信噪比低的频率分量上，$H(\omega) \approx 0$（抑制噪声）。

维纳滤波是卡尔曼滤波的前身。它优雅且理论完备，但有致命缺点：要求信号和噪声都是平稳随机过程（统计特性不随时间变化），不能递推计算（需要整段数据离线处理），且对非平稳信号（如含有趋势和突变）表现不佳。卡尔曼滤波正是为了解决这些问题而诞生的。

卡尔曼滤波（Kalman Filter）

1960 年，鲁道夫·卡尔曼发表了《A New Approach to Linear Filtering and Prediction Problems》。这篇论文的突破口在于用状态空间方法替代了维纳的频域方法，从而允许滤波器自然地处理非平稳信号和多维状态，并且以递推方式实时计算。

状态空间模型

卡尔曼滤波将问题建模为一个离散线性动态系统。系统状态 $x_k \in \mathbb{R}^n$ 随时间按线性方程演化，测量 $z_k \in \mathbb{R}^m$ 是状态的线性函数：

状态方程： $$x_k = F_k x_{k-1} + B_k u_k + w_k$$

观测方程： $$z_k = H_k x_k + v_k$$

$F_k$ 是 $n \times n$ 状态转移矩阵，$H_k$ 是 $m \times n$ 观测矩阵，$B_k$ 是控制输入矩阵。过程噪声 $w_k \sim \mathcal{N}(0, Q_k)$ 和观测噪声 $v_k \sim \mathcal{N}(0, R_k)$ 互相独立，且均为零均值高斯白噪声。

预测-更新递推

卡尔曼滤波器以两步递推方式工作。给定 $k-1$ 时刻的后验估计 $\hat{x}_{k-1|k-1}$ 和后验协方差 $P_{k-1|k-1}$：

预测步——按照系统模型将状态和不确定性向前推进一个时间步：

\[\hat{x}_{k|k-1} = F_k \hat{x}_{k-1|k-1} + B_k u_k\]

\[P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k\]

这里 $\hat{x}_{k|k-1}$ 是先验估计（仅依赖于模型预测，未使用当前观测），$P_{k|k-1}$ 是先验误差协方差。$Q_k$ 的加入体现了由于过程噪声，我们的预测不确定性总是比上一时刻的后验不确定性大。

更新步——用当前观测来修正先验估计：

\[K_k = P_{k|k-1} H_k^T \left( H_k P_{k|k-1} H_k^T + R_k \right)^{-1}\]

\[\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \left( z_k - H_k \hat{x}_{k|k-1} \right)\]

\[P_{k|k} = \left( I - K_k H_k \right) P_{k|k-1}\]

$K_k$ 是卡尔曼增益矩阵（$n \times m$），残差 $z_k - H_k \hat{x}_{k|k-1}$ 称为新息（innovation），代表"观测与预期之差"。更新公式的结构表明后验估计是先验估计加上一个新息加权修正，权重由卡尔曼增益决定。

卡尔曼增益的直觉

理解卡尔曼增益的结构是掌握整个滤波器的关键。以标量情况为例（$n=m=1$），设 $F=H=1$，$P=P_{k|k-1}$，则

\[K_k = \frac{P}{P + R}\]

这个公式揭示了卡尔曼滤波的本质行为。当观测噪声方差 $R$ 很大时（测量不可靠），$K \approx 0$，滤波器几乎忽略观测，后验估计约等于先验预测；反之当 $R$ 很小（测量精确），$K \approx 1$，滤波器充分信任观测。当过程噪声 $Q$ 很大导致先验协方差 $P$ 很大时，$K$ 也增大——滤波器意识到自己的预测不可靠，转而依赖观测。

在多维情况下完全一样的直觉：卡尔曼增益 $K_k$ 本质上是"状态先验不确定性通过观测矩阵投影到观测空间"与"观测的总不确定性（投影后的先验不确定性加上观测噪声）"之比。因此它是一个动态调整的信噪比权重矩阵。

递推的数学基础

卡尔曼滤波的推导可以从多个角度理解。从贝叶斯观点看，预测步给出了先验分布 $p(x_k | z_{1:k-1})$，更新步用贝叶斯公式将观测似然 $p(z_k|x_k)$ 与先验结合得到后验分布 $p(x_k | z_{1:k})$。在高斯假设下，所有分布都是高斯的，卡尔曼滤波恰好给出了后验均值和协方差的解析递推。这就是它作为线性高斯系统下最小均方误差最优估计器的本质。

从优化观点看，卡尔曼滤波在每个时间步求解一个加权最小二乘问题：

\[\hat{x}_{k|k} = \arg\min_x \left[ (x - \hat{x}_{k|k-1})^T P_{k|k-1}^{-1} (x - \hat{x}_{k|k-1}) + (z_k - H_k x)^T R_k^{-1} (z_k - H_k x) \right]\]

第一项惩罚偏离预测，第二项惩罚偏离观测，各自的不确定性协方差矩阵的逆作为权重——不确定性越大的来源，在优化中权重越小。

二维轨迹追踪演示

下面用 Python 实现一个完整的卡尔曼滤波器，追踪二维平面中匀速运动的目标。状态向量为 $x_k = [p_x, v_x, p_y, v_y]^T$，我们只能观测目标的位置（$p_x, p_y$）。

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from matplotlib.patches import Ellipse
import csv, os

np.random.seed(42)
os.makedirs('images', exist_ok=True)

# ---------- 系统参数 ----------
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 = 0.05                              # 过程噪声强度
Q = q * np.array([[dt**3/3, dt**2/2, 0,        0],
                  [dt**2/2, dt,      0,        0],
                  [0,       0,       dt**3/3,  dt**2/2],
                  [0,       0,       dt**2/2,  dt      ]])

R = np.diag([15, 15])                 # 观测噪声协方差

# ---------- 生成模拟数据 ----------
N = 80
x_true_init = np.array([0, 5, 0, 3])
x_true_hist = np.zeros((N, 4))
z_hist = np.zeros((N, 2))

xk = x_true_init.copy()
for k in range(N):
    w = np.random.multivariate_normal(np.zeros(4), Q)
    xk = F @ xk + w
    x_true_hist[k] = xk
    v = np.random.multivariate_normal(np.zeros(2), R)
    z_hist[k] = H @ xk + v

# ---------- 卡尔曼滤波 ----------
x_est = np.zeros((N, 4))
P_est = np.zeros((N, 4, 4))
K_save = np.zeros((N, 4, 2))
x = np.zeros(4)
P = np.eye(4) * 50

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_hist[k] - H @ x_pred)
    P = (np.eye(4) - K @ H) @ P_pred
    x_est[k] = x
    P_est[k] = P
    K_save[k] = K

# ---------- 可视化 ----------
fig, axes = plt.subplots(2, 2, figsize=(14, 12))
(ax_traj, ax_k), (ax_err, ax_ell) = axes

# 轨迹
ax_traj.plot(x_true_hist[:,0], x_true_hist[:,2], 'k-', lw=1.0, label='真实轨迹')
ax_traj.scatter(z_hist[:,0], z_hist[:,1], s=8, c='gray', alpha=0.4, label='观测值')
ax_traj.plot(x_est[:,0], x_est[:,2], 'r-', lw=1.8, label='滤波估计')
ax_traj.set_xlabel('x'); ax_traj.set_ylabel('y')
ax_traj.set_title('轨迹追踪'); ax_traj.legend(); ax_traj.grid(alpha=0.3); ax_traj.set_aspect('equal')

# 卡尔曼增益
for d, label, ls in [(0, 'K_px', '-'), (2, 'K_py', '--')]:
    ax_k.plot(K_save[:, d, 0], ls=ls, lw=1.2, label=label)
ax_k.set_xlabel('k'); ax_k.set_ylabel('增益值')
ax_k.set_title('卡尔曼增益收敛'); ax_k.legend(); ax_k.grid(alpha=0.3)

# 估计误差
for d, label, c in [(0, 'x误差', 'tab:orange'), (2, 'y误差', 'tab:green')]:
    err = np.abs(x_true_hist[:, d] - x_est[:, d])
    ax_err.plot(err, color=c, lw=1.0, label=label)
ax_err.set_xlabel('k'); ax_err.set_ylabel('|误差|')
ax_err.set_title('估计误差'); ax_err.legend(); ax_err.grid(alpha=0.3)

# 最终帧的3σ椭圆
p_cov = P_est[-1][[0,2]][:,[0,2]]
vals, vecs = np.linalg.eigh(p_cov)
angle = np.degrees(np.arctan2(vecs[1,0], vecs[0,0]))
ell = Ellipse((x_est[-1,0], x_est[-1,2]), 2*np.sqrt(vals[0]), 2*np.sqrt(vals[1]),
              angle=angle, fc='red', alpha=0.15, ec='red', lw=1)
ax_traj.add_patch(ell)

plt.tight_layout()
plt.savefig('images/kalman_filter.svg', format='svg', bbox_inches='tight')
plt.savefig('images/kalman_filter.png', dpi=150, bbox_inches='tight')
plt.close()

# CSV
with open('images/kalman_filter_data.csv', 'w', newline='') as f:
    w = csv.writer(f)
    w.writerow(['k','true_x','true_y','obs_x','obs_y','est_x','est_y','err_x','err_y'])
    for k in range(N):
        w.writerow([k] + [f'{x_true_hist[k,0]:.4f}', f'{x_true_hist[k,2]:.4f}',
                          f'{z_hist[k,0]:.4f}', f'{z_hist[k,1]:.4f}',
                          f'{x_est[k,0]:.4f}', f'{x_est[k,2]:.4f}',
                          f'{abs(x_true_hist[k,0]-x_est[k,0]):.4f}',
                          f'{abs(x_true_hist[k,2]-x_est[k,2]):.4f}'])

# GIF：逐帧追踪
fig_gif, ax_gif = plt.subplots(figsize=(8, 6))
ax_gif.set_xlim(x_true_hist[:,0].min()-10, x_true_hist[:,0].max()+20)
ax_gif.set_ylim(x_true_hist[:,2].min()-10, x_true_hist[:,2].max()+20)
ax_gif.set_xlabel('x'); ax_gif.set_ylabel('y'); ax_gif.set_aspect('equal')

true_line, = ax_gif.plot([], [], 'k-', lw=1.0, label='真实')
meas_scat = ax_gif.scatter([], [], s=6, c='gray', alpha=0.4, label='观测')
est_line, = ax_gif.plot([], [], 'r-', lw=1.8, label='滤波估计')
ell_ani = Ellipse((0, 0), 1, 1, angle=0, fc='red', alpha=0.1, ec='red', lw=0.5)
ax_gif.add_patch(ell_ani)
ax_gif.legend(loc='upper left')

def animate(k):
    true_line.set_data(x_true_hist[:k+1, 0], x_true_hist[:k+1, 2])
    meas_scat.set_offsets(z_hist[:k+1])
    est_line.set_data(x_est[:k+1, 0], x_est[:k+1, 2])
    cov = P_est[k][[0,2]][:,[0,2]]
    ev, evec = np.linalg.eigh(cov)
    if ev[0] > 0:
        ell_ani.set_width(2*np.sqrt(ev[0])); ell_ani.set_height(2*np.sqrt(ev[1]))
    ell_ani.set_angle(np.degrees(np.arctan2(evec[1,0], evec[0,0])))
    ell_ani.set_center((x_est[k,0], x_est[k,2]))
    ax_gif.set_title(f'Kalman Filter Tracking  k={k}')
    return true_line, meas_scat, est_line, ell_ani

ani = FuncAnimation(fig_gif, animate, frames=range(0, N, 2), interval=120, blit=False)
ani.save('images/kalman_filter.gif', writer='pillow', fps=8, dpi=72)
plt.close()

print("卡尔曼滤波追踪：全部产物已保存")

非线性扩展

经典卡尔曼滤波要求系统是线性的。实际问题中，状态转移和观测函数往往是非线性的。这引出了两类主要扩展。

扩展卡尔曼滤波（EKF）

扩展卡尔曼滤波将非线性函数的泰勒一阶展开代入标准卡尔曼方程。设状态转移和观测为

\[x_k = f(x_{k-1}) + w_k, \quad z_k = h(x_k) + v_k\]

EKF 在每一时刻计算雅可比矩阵：

\[F_k = \left. \frac{\partial f}{\partial x} \right|_{\hat{x}_{k-1|k-1}}, \quad H_k = \left. \frac{\partial h}{\partial x} \right|_{\hat{x}_{k|k-1}}\]

然后将标准卡尔曼方程中的 $F_k$ 和 $H_k$ 替换为这些雅可比矩阵。其余步骤不变。

EKF 在局部线性化点附近是近似最优的，但当非线性程度较强时，线性化会引入显著误差。此外雅可比矩阵的计算在高维情况下开销很大，且需要 $f$ 和 $h$ 可微。

下面的代码对比标准卡尔曼滤波（KF）、扩展卡尔曼滤波（EKF）和一个简单的角度追踪问题。追踪一个在圆周上运动的点（非线性观测：雷达给出距离和方位角）。

# EKF：雷达追踪圆周运动
import numpy as np
import matplotlib.pyplot as plt

np.random.seed(42)
N = 100
dt = 0.1
omega = 0.5
R_true = 5.0

# 真实状态：位置+速度 [px, py, vx, vy]
x_true = np.zeros((N, 4))
x_true[0] = [R_true, 0, 0, R_true * omega]
for k in range(1, N):
    # 匀速转弯模型
    theta = omega * dt
    F_c = np.array([[np.cos(theta), -np.sin(theta), dt*np.cos(theta), -dt*np.sin(theta)],
                    [np.sin(theta),  np.cos(theta), dt*np.sin(theta),  dt*np.cos(theta)],
                    [0, 0, np.cos(theta), -np.sin(theta)],
                    [0, 0, np.sin(theta),  np.cos(theta)]])
    x_true[k] = F_c @ x_true[k-1] + np.random.multivariate_normal(np.zeros(4), np.eye(4)*0.01)

# 非线性观测：距离 + 方位角
def h(x):
    px, py = x[0], x[1]
    return np.array([np.sqrt(px**2 + py**2), np.arctan2(py, px)])
def H_jac(x):
    px, py = x[0], x[1]
    d = np.sqrt(px**2 + py**2)
    if d < 1e-9: return np.zeros((2,4))
    return np.array([[px/d, py/d, 0, 0],
                     [-py/(d**2), px/(d**2), 0, 0]])

z = np.zeros((N, 2))
for k in range(N):
    z[k] = h(x_true[k]) + np.array([np.random.randn()*0.3, np.random.randn()*0.05])

# 线性KF（用匀速模型作为线性近似）
x_lin = np.zeros((N, 4))
P_lin = np.zeros((N, 4, 4))
x = np.array([4.0, 0, 0, 3.0])
P = np.eye(4) * 5
F_lin = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]])
Q_kf = np.eye(4) * 0.05

for k in range(N):
    x_pred = F_lin @ x
    P_pred = F_lin @ P @ F_lin.T + Q_kf
    # 对观测也做线性近似
    H_k = H_jac(x_pred)
    y_pred = h(x_pred)
    S = H_k @ P_pred @ H_k.T + np.diag([0.3**2, 0.05**2])
    K = P_pred @ H_k.T @ np.linalg.inv(S)
    x = x_pred + K @ (z[k] - y_pred)
    P = (np.eye(4) - K @ H_k) @ P_pred
    x_lin[k] = x
    P_lin[k] = P

# EKF（使用真实转弯模型）
x_ekf = np.zeros((N, 4))
P_ekf = np.zeros((N, 4, 4))
x = np.array([4.0, 0, 0, 3.0])
P = np.eye(4) * 5
Q_ekf = np.eye(4) * 0.05

for k in range(N):
    # 预测使用真实非线性模型
    theta = omega * dt
    F_c = np.array([[np.cos(theta), -np.sin(theta), dt*np.cos(theta), -dt*np.sin(theta)],
                    [np.sin(theta),  np.cos(theta), dt*np.sin(theta),  dt*np.cos(theta)],
                    [0, 0, np.cos(theta), -np.sin(theta)],
                    [0, 0, np.sin(theta),  np.cos(theta)]])
    x_pred = F_c @ x
    P_pred = F_c @ P @ F_c.T + Q_ekf
    H_k = H_jac(x_pred)
    y_pred = h(x_pred)
    S = H_k @ P_pred @ H_k.T + np.diag([0.3**2, 0.05**2])
    K = P_pred @ H_k.T @ np.linalg.inv(S)
    x = x_pred + K @ (z[k] - y_pred)
    P = (np.eye(4) - K @ H_k) @ P_pred
    x_ekf[k] = x
    P_ekf[k] = P

# 绘图
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(13, 6))
ax1.plot(x_true[:,0], x_true[:,1], 'k-', lw=1.0, label='真实轨迹')
ax1.plot(x_lin[:,0], x_lin[:,1], 'b--', lw=1.2, label='线性KF')
ax1.plot(x_ekf[:,0], x_ekf[:,1], 'r-', lw=1.5, label='EKF')
ax1.set_xlabel('x'); ax1.set_ylabel('y'); ax1.set_title('圆周运动追踪：线性KF vs EKF')
ax1.legend(); ax1.grid(alpha=0.3); ax1.set_aspect('equal')

ax2.plot(np.sqrt(x_true[:,0]**2 + x_true[:,1]**2), 'k-', lw=1.0, label='真实半径')
ax2.plot(np.sqrt(x_lin[:,0]**2 + x_lin[:,1]**2), 'b--', lw=1.2, label='线性KF')
ax2.plot(np.sqrt(x_ekf[:,0]**2 + x_ekf[:,1]**2), 'r-', lw=1.5, label='EKF')
ax2.set_xlabel('k'); ax2.set_ylabel('半径'); ax2.set_title('半径估计对比')
ax2.legend(); ax2.grid(alpha=0.3)

plt.tight_layout()
plt.savefig('images/ekf_comparison.svg', format='svg', bbox_inches='tight')
plt.savefig('images/ekf_comparison.png', dpi=150, bbox_inches='tight')
plt.close()

with open('images/ekf_comparison_data.csv', 'w', newline='') as f:
    w = csv.writer(f)
    w.writerow(['k','true_x','true_y','kf_x','kf_y','ekf_x','ekf_y','kf_err','ekf_err'])
    for k in range(N):
        kf_err = np.sqrt((x_true[k,0]-x_lin[k,0])**2 + (x_true[k,1]-x_lin[k,1])**2)
        ekf_err = np.sqrt((x_true[k,0]-x_ekf[k,0])**2 + (x_true[k,1]-x_ekf[k,1])**2)
        w.writerow([k] + [f'{v:.4f}' for v in [x_true[k,0],x_true[k,1],x_lin[k,0],x_lin[k,1],x_ekf[k,0],x_ekf[k,1],kf_err,ekf_err]])
print("EKF对比：产物已保存")

无迹卡尔曼滤波（UKF）

EKF 的根本问题在于用局部的雅可比矩阵近似非线性函数。无迹卡尔曼滤波（Julier & Uhlmann, 1997）采用了不同的思路：在状态空间的均值和协方差周围选取一组称为西格玛点（sigma points）的采样点，将这些采样点通过非线性函数后再用它们重建变换后的均值和协方差。UKF 不使用雅可比，因此既能处理不可微的函数，又能在强非线性下保持比 EKF 更高的精度。

UKF 的采样规则：对于 $n$ 维状态，生成 $2n+1$ 个西格玛点（其中 $\lambda = \alpha^2(n + \kappa) - n$ 是尺度参数）：

\[\mathcal{X}_0 = \hat{x}, \quad \mathcal{X}_i = \hat{x} + \left(\sqrt{(n+\lambda)P}\right)_i, \quad \mathcal{X}_{i+n} = \hat{x} - \left(\sqrt{(n+\lambda)P}\right)_i\]

这些点经过非线性函数后，通过加权平均重建状态估计：

\[\hat{x}' = \sum_{i=0}^{2n} W_i^{(m)} f(\mathcal{X}_i), \quad P' = \sum_{i=0}^{2n} W_i^{(c)} [f(\mathcal{X}_i) - \hat{x}'][f(\mathcal{X}_i) - \hat{x}']^T + Q\]

UKF 对非线性函数前两次矩的传播方式与 EKF 不同——EKF 只保留一阶导数，UKF 通过采样近似函数整体的变换效果，保留到三阶（对高斯分布而言）。

粒子滤波（Particle Filter）

粒子滤波代表了非参数贝叶斯滤波的方向。与卡尔曼类方法用均值和协方差两个参数表示高斯分布不同，粒子滤波用大量带权重的随机样本（粒子）来表示任意分布，不做高斯假设，也不做线性假设。

粒子滤波每步包含三个操作。重要性采样：从提议分布中抽取新粒子。权重更新：根据观测似然给每个粒子赋予权重。重采样：依权重复制高权重粒子、淘汰低权重粒子。

这个方法极其通用——它可以处理任何非线性、非高斯的系统。代价是计算量大，且在高维状态空间中面临维数灾难。

各方法对比与适用场景

将前述方法放在一起比较，可以勾勒出清晰的演化脉络。下表总结各方法的核心特征：

方法	线性要求	高斯要求	递推	最优性	计算代价
移动平均	无	无	是	仅常数信号	极低
指数平滑	无	无	是	仅常数信号	极低
维纳滤波	线性时不变	平稳统计量	否	平稳信号最优	中
卡尔曼滤波	线性	高斯	是	线性高斯下最优	低
扩展卡尔曼	局部可微	近似高斯	是	局部近似	中
无迹卡尔曼	无	近似高斯	是	比EKF精度高	中
粒子滤波	无	无	是	渐近最优	高

可参考的文献

[1] Kalman, R. E. (1960). A New Approach to Linear Filtering and Prediction Problems. Journal of Basic Engineering, 82(1), 35-45.

[2] Welch, G., & Bishop, G. (2006). An Introduction to the Kalman Filter. University of North Carolina at Chapel Hill, TR 95-041.

[3] Julier, S. J., & Uhlmann, J. K. (1997). A New Extension of the Kalman Filter to Nonlinear Systems. Proceedings of SPIE, 3068, 182-193.

[4] Arulampalam, M. S., Maskell, S., Gordon, N., & Clapp, T. (2002). A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking. IEEE Transactions on Signal Processing, 50(2), 174-188.

[5] Wiener, N. (1949). Extrapolation, Interpolation, and Smoothing of Stationary Time Series. MIT Press.