CycleUser上一篇介绍了维纳滤波。它是第一个统计最优的线性滤波器，但受限于三个致命假设：信号平稳、离线处理、单变量。1960 年，鲁道夫·卡尔曼发表了《A New Approach to Linear Filtering and Prediction Problems》，用状态空间方法替代频域方法，一举突破了这三个限制。阿波罗登月的导航计算机、GPS 接收机的位置解算、自动驾驶的传感器融合、金融时间序列的在线平滑，都依赖卡尔曼滤波或其变体。
与维纳滤波的关系
维纳滤波在频域中设计最优滤波器，需要知道信号和噪声的完整功率谱密度，且只能离线处理。卡尔曼滤波把问题转换到时域：用状态方程描述系统动力学，用观测方程描述测量过程，以递推方式在每个时刻仅用当前观测更新估计。两者在平稳线性高斯的特殊情况下给出等价的结果，但卡尔曼滤波的适用范围远大于维纳滤波——它允许非平稳信号、多维耦合状态、实时在线计算。
状态空间模型
卡尔曼滤波将问题描述为一个离散线性动态系统。系统在时刻 $k$ 的状态 $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, \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$（$n \times n$）是状态转移矩阵，描述系统在没有噪声和输入时如何从上一状态演化到当前状态。$H_k$（$m \times n$）是观测矩阵，描述状态如何映射到测量空间。过程噪声 $w_k$ 和观测噪声 $v_k$ 均为零均值高斯白噪声，互相独立，协方差矩阵分别为 $Q_k$ 和 $R_k$。
预测-更新递推
卡尔曼滤波器以两步递推方式工作。
预测步——基于 $k-1$ 时刻的后验估计 $\hat{x}{k-1|k-1}$ 和后验协方差 $P$，计算 $k$ 时刻的先验估计：
$$\hat{x}{k|k-1} = F_k \hat{x} + B_k u_k$$
$$P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k$$
更新步——用时刻 $k$ 的观测 $z_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} \right)$$} + K_k \left( z_k - H_k \hat{x}_{k|k-1
$$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})$ 称为新息或残差，代表观测与预期的偏离。
卡尔曼增益的直觉
以标量情况为例（$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$ 也随之增大——滤波器意识到自己的预测不可靠，转而更多依赖观测。
贝叶斯视角
在高斯噪声假设下，预测步给出的 $\hat{x}{k|k-1}$ 和 $P}$ 恰好是先验分布 $p(x_k \mid z_{1:k-1}) = \mathcal{N}(\hat{x{k|k-1}, P})$ 的均值和协方差。更新步用贝叶斯公式将观测似然 $p(z_k \mid x_k) = \mathcal{N}(H_k x_k, R_k)$ 与先验结合，所得的后验分布 $p(x_k \mid z_{1:k})$ 的均值恰好是 $\hat{x{k|k}$，协方差恰好是 $P$。因此卡尔曼滤波本质上是线性高斯动态系统的精确贝叶斯递推。
可视化
下图的 GIF 展示了滤波追踪的动态过程：随着新观测不断到达，滤波器的估计（红线）逐步收敛到真实轨迹（黑线），红色椭圆代表估计的不确定性，随时间逐渐缩小。
Python 实现
下面的代码实现二维平面中匀速运动目标的卡尔曼滤波跟踪。状态向量 $x_k = [p_x, v_x, p_y, v_y]^T$ 包含两个方向的位置和速度，观测只能获取位置分量。
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 = np.zeros((N, 4))
z_obs = np.zeros((N, 2))
xk = np.array([0, 5, 0, 3])
for k in range(N):
xk = F @ xk + np.random.multivariate_normal(np.zeros(4), Q)
x_true[k] = xk
z_obs[k] = H @ xk + np.random.multivariate_normal(np.zeros(2), R)
x_est = np.zeros((N, 4))
P_est = np.zeros((N, 4, 4))
K_hist = 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_obs[k] - H @ x_pred)
P = (np.eye(4) - K @ H) @ P_pred
x_est[k] = x
P_est[k] = P
K_hist[k] = K
fig, axes = plt.subplots(2, 2, figsize=(14, 12))
ax = axes[0, 0]
ax.plot(x_true[:, 0], x_true[:, 2], 'k-', lw=1.0, label='True Trajectory')
ax.scatter(z_obs[:, 0], z_obs[:, 1], s=8, c='gray', alpha=0.4, label='Obs')
ax.plot(x_est[:, 0], x_est[:, 2], 'r-', lw=1.8, label='KF Estimate')
for k in range(0, N, 5):
cov = P_est[k][[0,2]][:,[0,2]]
vals, vecs = np.linalg.eigh(cov)
ang = np.degrees(np.arctan2(vecs[1, 0], vecs[0, 0]))
ell = Ellipse((x_est[k,0], x_est[k,2]), 2*np.sqrt(vals[0]), 2*np.sqrt(vals[1]),
angle=ang, fc='red', alpha=0.06, ec='red', lw=0.3)
ax.add_patch(ell)
ax.set_xlabel('x'); ax.set_ylabel('y')
ax.set_title('Position Trajectory with Uncertainty Ellipses')
ax.legend(fontsize=8); ax.grid(alpha=0.3); ax.set_aspect('equal')
ax = axes[0, 1]
for d, label, ls in [(0, 'K pos x', '-'), (2, 'K pos y', '--')]:
ax.plot(K_hist[:, d, 0], ls=ls, lw=1.2, label=label)
ax.set_xlabel('k'); ax.set_ylabel('Gain')
ax.set_title('Kalman Gain Convergence'); ax.legend(); ax.grid(alpha=0.3)
ax = axes[1, 0]
for d, label, c in [(0, 'x Error', 'tab:orange'), (2, 'y Error', 'tab:green')]:
ax.plot(np.abs(x_true[:, d] - x_est[:, d]), color=c, lw=1.0, label=label)
ax.set_xlabel('k'); ax.set_ylabel('|Error|')
ax.set_title('Estimation Error'); ax.legend(); ax.grid(alpha=0.3)
ax = axes[1, 1]
rmse = np.sqrt(np.cumsum((x_true[:, 1] - x_est[:, 1])**2) / np.arange(1, N+1))
ax.plot(rmse, 'tab:purple', lw=1.5)
ax.set_xlabel('k'); ax.set_ylabel('RMSE')
ax.set_title('Velocity RMSE'); ax.grid(alpha=0.3)
plt.tight_layout()
plt.savefig('images/kalman_tracking.svg', format='svg', bbox_inches='tight')
plt.savefig('images/kalman_tracking.png', dpi=150, bbox_inches='tight')
plt.close()
with open('images/kalman_tracking_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'{v:.4f}' for v in [
x_true[k,0], x_true[k,2], z_obs[k,0], z_obs[k,1],
x_est[k,0], x_est[k,2],
abs(x_true[k,0]-x_est[k,0]), abs(x_true[k,2]-x_est[k,2])]])
fig_gif, ax_gif = plt.subplots(figsize=(8, 6))
ax_gif.set_xlim(x_true[:,0].min()-10, x_true[:,0].max()+20)
ax_gif.set_ylim(x_true[:,2].min()-10, x_true[:,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='True Trajectory')
meas_scat = ax_gif.scatter([], [], s=6, c='gray', alpha=0.4, label='Obs')
est_line, = ax_gif.plot([], [], 'r-', lw=1.8, label='KF Estimate')
ell_patch = Ellipse((0,0), 1, 1, angle=0, fc='red', alpha=0.1, ec='red', lw=0.5)
ax_gif.add_patch(ell_patch)
ax_gif.legend(loc='upper left')
def animate(k):
true_line.set_data(x_true[:k+1, 0], x_true[:k+1, 2])
meas_scat.set_offsets(z_obs[: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_patch.set_width(2*np.sqrt(ev[0]))
ell_patch.set_height(2*np.sqrt(ev[1]))
ell_patch.set_angle(np.degrees(np.arctan2(evec[1,0], evec[0,0])))
ell_patch.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_patch
ani = FuncAnimation(fig_gif, animate, frames=range(0, N, 2),
interval=120, blit=False)
ani.save('images/滤波04-图1.gif', writer='pillow', fps=8, dpi=72)
plt.close()
print('Kalman filter outputs saved')
演化与局限
卡尔曼滤波相比维纳滤波是革命性的进步：它支持多维状态、实时递推、非平稳信号，且在线性高斯假设下是最优的。但"线性"这个假设本身就是局限。雷达测量的是距离和方位角——观测方程是非线性的。飞行器的动力学涉及姿态旋转——状态方程是非线性的。一旦 $f$ 或 $h$ 不是线性函数，标准卡尔曼方程中的矩阵乘法就不再适用。
最直接的推广思路是：把非线性函数在当前估计点做一阶泰勒展开，用雅可比矩阵替代线性模型中的 $F_k$ 和 $H_k$。这就是扩展卡尔曼滤波（EKF），下一篇的主题。
参考文献
[1] arXiv:2203.06105 — A Summary on the UD Kalman Filter.
[2] arXiv:2305.00252 — Model-Based Monitoring and State Estimation for Digital Twins: The Kalman Filter.