目录
[19.1.1 推理](#19.1.1 推理)
[19.1.2 学习](#19.1.2 学习)
[19.2.1 推理（卡尔曼滤波核心步骤）](#19.2.1 推理（卡尔曼滤波核心步骤）)
[19.2.2 改写测量合并阶段](#19.2.2 改写测量合并阶段)
[19.2.3 推理总结](#19.2.3 推理总结)
[19.2.4/5 示例（完整代码 + 可视化对比）](#19.2.4/5 示例（完整代码 + 可视化对比）)
[完整 Python 代码（可直接运行，Mac 适配）](#完整 Python 代码（可直接运行，Mac 适配）)
[19.2.6 滤波](#19.2.6 滤波)
[19.2.7 时序和测量模型](#19.2.7 时序和测量模型)
[19.2.8 卡尔曼滤波器的问题](#19.2.8 卡尔曼滤波器的问题)
[完整 Python 代码（非线性运动跟踪示例）](#完整 Python 代码（非线性运动跟踪示例）)
[19.4.1 状态演化](#19.4.1 状态演化)
[19.4.2 测量合并过程](#19.4.2 测量合并过程)
[完整 Python 代码（UKF 实现）](#完整 Python 代码（UKF 实现）)
[完整 Python 代码（粒子滤波实现）](#完整 Python 代码（粒子滤波实现）)
前言
在计算机视觉领域，时序模型是处理动态视觉问题的核心（比如视频目标跟踪、运动物体姿态估计、无人车视觉导航等）。《计算机视觉：模型、学习和推理》第 19 章的时序模型，核心是解决 "如何从随时间变化的观测数据中，估计出物体的真实状态"。
本文会用通俗易懂的语言拆解本章核心知识点，搭配可直接运行的 Python 完整代码 和可视化对比图，让你彻底搞懂卡尔曼滤波器、扩展卡尔曼滤波器等核心内容。
一、时序估计框架（19.1）
核心概念
时序估计的本质是 "追根溯源"：我们能拿到的是带噪声的观测数据 （比如摄像头每一帧看到的物体位置），但想知道的是物体的真实状态（比如物体实际的位置、速度）。
这个框架分为两大核心环节：
19.1.1 推理
推理是 "根据已有信息猜真实状态"：就像你看到一个人在画面里的位置（观测），结合他上一帧的运动状态（历史），猜他此刻的真实位置（状态）。
19.1.2 学习
学习是 "优化猜的规则"：如果每次猜的结果和真实值有偏差，就调整规则（比如状态转移的参数），让下次猜得更准。
二、卡尔曼滤波器（19.2）
核心概念
卡尔曼滤波器是时序估计的 "基础神器"，专门解决线性系统 + 高斯噪声的状态估计问题。可以把它比喻成：
一个 "智能猜数器"：每次先根据上一次的猜测结果 "预测" 当前状态，再用新的观测数据 "修正" 这个预测，最终得到更准的结果。
19.2.1 推理（卡尔曼滤波核心步骤）
卡尔曼滤波的推理过程分两大步：
1.预测阶段：用系统的状态转移模型，从 k-1 时刻的状态，预测 k 时刻的先验状态；
2.更新阶段：用 k 时刻的观测数据，修正先验状态，得到 k 时刻的后验状态（最终的最优估计）。
19.2.2 改写测量合并阶段
测量合并阶段的核心是 "权衡"：观测数据有噪声、预测结果也有误差，卡尔曼滤波器会计算一个 "卡尔曼增益"，来决定 "更相信预测" 还是 "更相信观测"。
19.2.3 推理总结
卡尔曼滤波的核心公式（用通俗语言翻译）：
预测：当前先验状态 = 状态转移矩阵 × 上一时刻状态 + 控制输入
卡尔曼增益：增益 = 先验协方差 × 观测矩阵^T / (观测矩阵×先验协方差×观测矩阵^T + 观测噪声)
更新：最终状态 = 先验状态 + 增益 × (观测值 - 观测矩阵×先验状态)
19.2.4/5 示例（完整代码 + 可视化对比）
下面用一个 "跟踪匀速运动的小球" 的例子，实现卡尔曼滤波器，并对比 "原始观测（带噪声）" 和 "卡尔曼滤波后结果" 的差异。
完整 Python 代码（可直接运行，Mac 适配）
import numpy as np
import matplotlib.pyplot as plt
# ==================== Mac系统Matplotlib中文显示配置 ====================
plt.rcParams['font.sans-serif'] = ['Arial Unicode MS', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False
plt.rcParams['font.family'] = 'sans-serif'
plt.rcParams['font.family'] = 'Arial Unicode MS'
plt.rcParams['axes.facecolor'] = 'white'
class KalmanFilter:
"""卡尔曼滤波器实现类"""
def __init__(self):
# 1. 初始化状态：[位置x, 速度vx]
self.x = np.array([[0.0], [0.0]]) # 初始状态（假设小球从原点出发，初始速度0）
# 2. 状态转移矩阵F：x_k = F * x_{k-1}（匀速运动模型）
# [x_k; vx_k] = [1, dt; 0, 1] * [x_{k-1}; vx_{k-1}]
self.dt = 0.1 # 时间步长
self.F = np.array([[1, self.dt],
[0, 1]])
# 3. 观测矩阵H：观测值z = H * x（我们只能观测到位置，观测不到速度）
self.H = np.array([[1, 0]])
# 4. 过程噪声协方差Q：描述系统模型的不确定性
self.Q = np.array([[0.001, 0],
[0, 0.001]])
# 5. 观测噪声协方差R：描述观测数据的噪声大小
self.R = np.array([[0.1]]) # 观测位置的噪声
# 6. 状态协方差矩阵P：描述状态估计的不确定性
self.P = np.eye(2) # 初始协方差
def predict(self):
"""预测阶段：计算先验状态和先验协方差"""
# 预测状态
self.x = np.dot(self.F, self.x)
# 预测协方差
self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q
return self.x
def update(self, z):
"""更新阶段：用观测值修正预测结果"""
# 计算卡尔曼增益K
S = np.dot(np.dot(self.H, self.P), self.H.T) + self.R # 观测残差协方差
K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S)) # 卡尔曼增益
# 修正状态
self.x = self.x + np.dot(K, (z - np.dot(self.H, self.x)))
# 修正协方差
I = np.eye(self.x.shape[0]) # 单位矩阵
self.P = np.dot((I - np.dot(K, self.H)), self.P)
return self.x
# ==================== 生成模拟数据 ====================
# 生成真实状态：小球以匀速2 m/s运动，共100个时间步
t = np.arange(0, 10, 0.1) # 0到10秒，步长0.1
true_x = 2 * t # 真实位置
true_vx = np.ones_like(t) * 2 # 真实速度（匀速）
# 生成带噪声的观测数据（模拟摄像头观测的位置）
np.random.seed(42) # 固定随机种子，保证结果可复现
obs_x = true_x + np.random.normal(0, np.sqrt(0.1), size=len(true_x)) # 加高斯噪声
# ==================== 运行卡尔曼滤波 ====================
kf = KalmanFilter()
filtered_x = [] # 存储滤波后的位置
filtered_vx = [] # 存储滤波后的速度
for z in obs_x:
kf.predict()
x_est = kf.update(z)
filtered_x.append(x_est[0, 0])
filtered_vx.append(x_est[1, 0])
# ==================== 可视化对比 ====================
plt.figure(figsize=(12, 6))
# 绘制位置对比
plt.subplot(1, 2, 1)
plt.plot(t, true_x, 'g-', label='真实位置', linewidth=2)
plt.plot(t, obs_x, 'r.', label='带噪声观测位置', alpha=0.5)
plt.plot(t, filtered_x, 'b-', label='卡尔曼滤波后位置', linewidth=2)
plt.xlabel('时间 (s)')
plt.ylabel('位置 (m)')
plt.title('卡尔曼滤波：位置估计对比')
plt.legend()
plt.grid(True, alpha=0.3)
# 绘制速度对比
plt.subplot(1, 2, 2)
plt.plot(t, true_vx, 'g-', label='真实速度', linewidth=2)
plt.plot(t, filtered_vx, 'b-', label='卡尔曼滤波后速度', linewidth=2)
plt.xlabel('时间 (s)')
plt.ylabel('速度 (m/s)')
plt.title('卡尔曼滤波：速度估计对比')
plt.legend()
plt.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()代码解释
1.KalmanFilter 类 ：封装了卡尔曼滤波的核心逻辑，包含predict（预测）和update（更新）两个核心方法；
2.模拟数据生成：生成小球匀速运动的真实位置，再给观测值加高斯噪声（模拟真实场景的观测误差）；
3.可视化：左侧对比 "真实位置、带噪声观测位置、滤波后位置"，右侧对比 "真实速度、滤波后速度"。
运行效果
- 红色散点（带噪声观测）会围绕绿色实线（真实位置）上下波动；
- 蓝色实线（滤波后结果）几乎和绿色实线重合，完美过滤了噪声；
- 速度估计也能稳定收敛到真实值 2 m/s。
19.2.6 滤波
卡尔曼滤波的 "滤波" 本质是 "从噪声中提取信号"：就像给观测数据 "开了美颜"，去掉噪声的瑕疵，还原真实状态。
19.2.7 时序和测量模型
时序模型（状态转移模型） ：描述物体状态随时间的变化规律（比如匀速、匀加速运动），对应代码中的F矩阵；
测量模型 ：描述 "真实状态" 和 "观测数据" 之间的关系，对应代码中的H矩阵。
19.2.8 卡尔曼滤波器的问题
卡尔曼滤波器有个 "致命缺点"：只适用于线性系统。如果物体的运动是非线性的（比如圆周运动、加速运动），卡尔曼滤波的效果会急剧下降。
三、扩展卡尔曼滤波器（19.3）
核心概念
扩展卡尔曼滤波器（EKF）是卡尔曼滤波的 "非线性版"，核心思路是：
把非线性系统 "局部线性化"（用泰勒展开的一阶近似），然后再用卡尔曼滤波的逻辑处理。
可以比喻成：
非线性的山路，我们把它切成无数小段，每一小段都近似看成直线，然后用走直线的方法（卡尔曼滤波）走完全程。
完整 Python 代码（非线性运动跟踪示例）
import numpy as np
import matplotlib.pyplot as plt
# ==================== Mac系统Matplotlib中文显示配置 ====================
plt.rcParams['font.sans-serif'] = ['Arial Unicode MS', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False
plt.rcParams['font.family'] = 'sans-serif'
plt.rcParams['font.family'] = 'Arial Unicode MS'
plt.rcParams['axes.facecolor'] = 'white'
class ExtendedKalmanFilter:
"""扩展卡尔曼滤波器（EKF）实现类：处理非线性运动（圆周运动）"""
def __init__(self):
# 初始化状态：[x, y, 角度theta, 角速度omega]
self.x = np.array([[1.0], [0.0], [0.0], [0.1]]) # 初始在(1,0)，初始角速度0.1 rad/s
# 状态协方差矩阵P
self.P = np.eye(4) * 0.1
# 过程噪声协方差Q
self.Q = np.eye(4) * 0.001
# 观测噪声协方差R（观测x和y的噪声）
self.R = np.eye(2) * 0.01
self.dt = 0.1 # 时间步长
def state_transition(self, x):
"""非线性状态转移函数（圆周运动）：x_k = f(x_{k-1})"""
x_pos, y_pos, theta, omega = x[:, 0]
# 圆周运动：角度随时间增加，位置随角度变化
new_theta = theta + omega * self.dt
new_x = np.cos(new_theta)
new_y = np.sin(new_theta)
new_omega = omega # 角速度不变
return np.array([[new_x], [new_y], [new_theta], [new_omega]])
def jacobian_F(self, x):
"""状态转移函数的雅可比矩阵（线性化核心）"""
theta = x[2, 0]
omega = x[3, 0]
# 雅可比矩阵J_F = df/dx
J = np.array([
[0, 0, -np.sin(theta + omega*self.dt)*omega, -np.sin(theta + omega*self.dt)*self.dt*omega + np.cos(theta + omega*self.dt)*self.dt],
[0, 0, np.cos(theta + omega*self.dt)*omega, np.cos(theta + omega*self.dt)*self.dt*omega + np.sin(theta + omega*self.dt)*self.dt],
[0, 0, 1, self.dt],
[0, 0, 0, 1]
])
return J
def observation_model(self, x):
"""观测模型：z = h(x) = [x_pos; y_pos]"""
return np.array([[x[0, 0]], [x[1, 0]]])
def jacobian_H(self):
"""观测模型的雅可比矩阵"""
return np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
def predict(self):
"""预测阶段：非线性预测 + 线性化协方差更新"""
# 1. 非线性预测状态
self.x = self.state_transition(self.x)
# 2. 计算雅可比矩阵
J_F = self.jacobian_F(self.x)
# 3. 更新协方差
self.P = np.dot(np.dot(J_F, self.P), J_F.T) + self.Q
return self.x
def update(self, z):
"""更新阶段：用观测值修正预测结果"""
# 1. 计算雅可比矩阵H
J_H = self.jacobian_H()
# 2. 计算卡尔曼增益
S = np.dot(np.dot(J_H, self.P), J_H.T) + self.R
K = np.dot(np.dot(self.P, J_H.T), np.linalg.inv(S))
# 3. 修正状态
z_pred = self.observation_model(self.x)
self.x = self.x + np.dot(K, (z - z_pred))
# 4. 修正协方差
I = np.eye(self.x.shape[0])
self.P = np.dot((I - np.dot(K, J_H)), self.P)
return self.x
# ==================== 生成模拟数据 ====================
np.random.seed(42)
t = np.arange(0, 20, 0.1) # 0到20秒，步长0.1
n_steps = len(t)
# 真实状态：圆周运动（半径1，角速度0.1 rad/s）
true_theta = 0.1 * t
true_x = np.cos(true_theta)
true_y = np.sin(true_theta)
# 带噪声的观测数据
obs_x = true_x + np.random.normal(0, 0.1, n_steps)
obs_y = true_y + np.random.normal(0, 0.1, n_steps)
# ==================== 运行EKF ====================
ekf = ExtendedKalmanFilter()
ekf_x = []
ekf_y = []
for z_x, z_y in zip(obs_x, obs_y):
ekf.predict()
x_est = ekf.update(np.array([[z_x], [z_y]]))
ekf_x.append(x_est[0, 0])
ekf_y.append(x_est[1, 0])
# ==================== 可视化对比 ====================
plt.figure(figsize=(10, 10))
plt.plot(true_x, true_y, 'g-', label='真实轨迹（圆周）', linewidth=3)
plt.scatter(obs_x, obs_y, c='r', s=10, label='带噪声观测点', alpha=0.5)
plt.plot(ekf_x, ekf_y, 'b-', label='EKF估计轨迹', linewidth=2)
plt.axis('equal')
plt.xlabel('X位置 (m)')
plt.ylabel('Y位置 (m)')
plt.title('扩展卡尔曼滤波器（EKF）：圆周运动跟踪对比')
plt.legend()
plt.grid(True, alpha=0.3)
plt.show()
代码解释
1.非线性状态转移：用圆周运动模型替代线性模型，体现非线性系统的特点；
2.雅可比矩阵：EKF 的核心，用于对非线性函数做一阶泰勒展开，实现线性化；
3.可视化：对比 "真实圆周轨迹、带噪声观测点、EKF 估计轨迹"，能看到 EKF 完美拟合真实轨迹。
四、无损卡尔曼滤波器（19.4）
核心概念
无损卡尔曼滤波器（UKF）是比 EKF 更优的非线性滤波方法，核心思路是：
不做线性化，而是通过 "采样 sigma 点" 来近似状态的概率分布，再用这些点来计算预测和更新。
可以比喻成：
EKF 是 "用一根直线近似曲线"，而 UKF 是 "用多个点拟合曲线"，拟合效果更准。
19.4.1 状态演化
UKF 的状态演化是通过 sigma 点的传播实现的：先根据当前状态的均值和协方差生成 sigma 点，再让这些点通过非线性的状态转移函数，得到新的 sigma 点，最后用这些新点计算先验状态的均值和协方差。
19.4.2 测量合并过程
测量合并阶段和 EKF 类似，但也是通过 sigma 点的传播来计算观测的均值和协方差，再计算卡尔曼增益，最终修正状态。
完整 Python 代码（UKF 实现）
python
import numpy as np
import matplotlib.pyplot as plt
# ==================== Mac系统Matplotlib中文显示配置 ====================
plt.rcParams['font.sans-serif'] = ['Arial Unicode MS', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False
plt.rcParams['font.family'] = 'sans-serif'
plt.rcParams['font.family'] = 'Arial Unicode MS'
plt.rcParams['axes.facecolor'] = 'white'
# ==================== 补充EKF类定义（关键修复） ====================
class ExtendedKalmanFilter:
"""扩展卡尔曼滤波器（EKF）实现类：处理非线性运动（圆周运动）"""
def __init__(self):
# 初始化状态：[x, y, 角度theta, 角速度omega]
self.x = np.array([[1.0], [0.0], [0.0], [0.1]]) # 初始在(1,0)，初始角速度0.1 rad/s
# 状态协方差矩阵P
self.P = np.eye(4) * 0.1
# 过程噪声协方差Q
self.Q = np.eye(4) * 0.001
# 观测噪声协方差R（观测x和y的噪声）
self.R = np.eye(2) * 0.01
self.dt = 0.1 # 时间步长
def state_transition(self, x):
"""非线性状态转移函数（圆周运动）：x_k = f(x_{k-1})"""
x_pos, y_pos, theta, omega = x[:, 0]
# 圆周运动：角度随时间增加，位置随角度变化
new_theta = theta + omega * self.dt
new_x = np.cos(new_theta)
new_y = np.sin(new_theta)
new_omega = omega # 角速度不变
return np.array([[new_x], [new_y], [new_theta], [new_omega]])
def jacobian_F(self, x):
"""状态转移函数的雅可比矩阵（线性化核心）"""
theta = x[2, 0]
omega = x[3, 0]
# 雅可比矩阵J_F = df/dx
J = np.array([
[0, 0, -np.sin(theta + omega*self.dt)*omega, -np.sin(theta + omega*self.dt)*self.dt*omega + np.cos(theta + omega*self.dt)*self.dt],
[0, 0, np.cos(theta + omega*self.dt)*omega, np.cos(theta + omega*self.dt)*self.dt*omega + np.sin(theta + omega*self.dt)*self.dt],
[0, 0, 1, self.dt],
[0, 0, 0, 1]
])
return J
def observation_model(self, x):
"""观测模型：z = h(x) = [x_pos; y_pos]"""
return np.array([[x[0, 0]], [x[1, 0]]])
def jacobian_H(self):
"""观测模型的雅可比矩阵"""
return np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
def predict(self):
"""预测阶段：非线性预测 + 线性化协方差更新"""
# 1. 非线性预测状态
self.x = self.state_transition(self.x)
# 2. 计算雅可比矩阵
J_F = self.jacobian_F(self.x)
# 3. 更新协方差
self.P = np.dot(np.dot(J_F, self.P), J_F.T) + self.Q
return self.x
def update(self, z):
"""更新阶段：用观测值修正预测结果"""
# 1. 计算雅可比矩阵H
J_H = self.jacobian_H()
# 2. 计算卡尔曼增益
S = np.dot(np.dot(J_H, self.P), J_H.T) + self.R
K = np.dot(np.dot(self.P, J_H.T), np.linalg.inv(S))
# 3. 修正状态
z_pred = self.observation_model(self.x)
self.x = self.x + np.dot(K, (z - z_pred))
# 4. 修正协方差
I = np.eye(self.x.shape[0])
self.P = np.dot((I - np.dot(K, J_H)), self.P)
return self.x
# ==================== UKF类定义（原有代码） ====================
class UnscentedKalmanFilter:
"""无损卡尔曼滤波器（UKF）实现类：圆周运动跟踪"""
def __init__(self):
# 状态维度
self.n_x = 4 # [x, y, theta, omega]
# 观测维度
self.n_z = 2 # [x, y]
# 初始化状态
self.x = np.array([[1.0], [0.0], [0.0], [0.1]])
# 状态协方差
self.P = np.eye(self.n_x) * 0.1
# UKF参数
self.alpha = 0.01 # 控制sigma点的分布
self.beta = 2.0 # 对高斯分布的最优选择
self.kappa = 3 - self.n_x # 缩放参数
# 计算权重
self.lambda_ = self.alpha ** 2 * (self.n_x + self.kappa) - self.n_x
self.Wm = np.zeros(2 * self.n_x + 1) # 均值权重
self.Wc = np.zeros(2 * self.n_x + 1) # 协方差权重
self.Wm[0] = self.lambda_ / (self.n_x + self.lambda_)
self.Wc[0] = self.Wm[0] + (1 - self.alpha ** 2 + self.beta)
for i in range(1, 2 * self.n_x + 1):
self.Wm[i] = 1 / (2 * (self.n_x + self.lambda_))
self.Wc[i] = self.Wm[i]
# 噪声协方差
self.Q = np.eye(self.n_x) * 0.001 # 过程噪声
self.R = np.eye(self.n_z) * 0.01 # 观测噪声
self.dt = 0.1 # 时间步长
def generate_sigma_points(self):
"""生成sigma点"""
sigma = np.zeros((self.n_x, 2 * self.n_x + 1))
sigma[:, 0] = self.x[:, 0] # 中心sigma点
sqrt_P = np.linalg.cholesky((self.n_x + self.lambda_) * self.P)
for i in range(self.n_x):
sigma[:, i + 1] = self.x[:, 0] + sqrt_P[:, i]
sigma[:, i + 1 + self.n_x] = self.x[:, 0] - sqrt_P[:, i]
return sigma
def state_transition(self, sigma):
"""非线性状态转移：sigma点传播"""
sigma_pred = np.zeros_like(sigma)
for i in range(sigma.shape[1]):
x_pos, y_pos, theta, omega = sigma[:, i]
new_theta = theta + omega * self.dt
new_x = np.cos(new_theta)
new_y = np.sin(new_theta)
new_omega = omega
sigma_pred[:, i] = [new_x, new_y, new_theta, new_omega]
return sigma_pred
def predict(self):
"""预测阶段"""
# 1. 生成sigma点
sigma = self.generate_sigma_points()
# 2. 传播sigma点
sigma_pred = self.state_transition(sigma)
# 3. 计算先验状态均值
self.x = np.dot(sigma_pred, self.Wm.reshape(-1, 1))
# 4. 计算先验协方差
self.P = self.Q.copy()
for i in range(sigma_pred.shape[1]):
diff = sigma_pred[:, i].reshape(-1, 1) - self.x
self.P += self.Wc[i] * np.dot(diff, diff.T)
return self.x, sigma_pred
def observation_model(self, sigma):
"""观测模型：sigma点的观测值"""
sigma_z = np.zeros((self.n_z, sigma.shape[1]))
for i in range(sigma.shape[1]):
sigma_z[:, i] = [sigma[0, i], sigma[1, i]]
return sigma_z
def update(self, z, sigma_pred):
"""更新阶段"""
# 1. 计算观测的sigma点
sigma_z = self.observation_model(sigma_pred)
# 2. 计算观测均值
z_pred = np.dot(sigma_z, self.Wm.reshape(-1, 1))
# 3. 计算观测协方差和互协方差
Pzz = self.R.copy()
Pxz = np.zeros((self.n_x, self.n_z))
for i in range(sigma_z.shape[1]):
diff_z = sigma_z[:, i].reshape(-1, 1) - z_pred
diff_x = sigma_pred[:, i].reshape(-1, 1) - self.x
Pzz += self.Wc[i] * np.dot(diff_z, diff_z.T)
Pxz += self.Wc[i] * np.dot(diff_x, diff_z.T)
# 4. 计算卡尔曼增益
K = np.dot(Pxz, np.linalg.inv(Pzz))
# 5. 修正状态和协方差
self.x = self.x + np.dot(K, (z - z_pred))
self.P = self.P - np.dot(np.dot(K, Pzz), K.T)
return self.x
# ==================== 生成模拟数据（和EKF示例相同） ====================
np.random.seed(42)
t = np.arange(0, 20, 0.1)
n_steps = len(t)
true_theta = 0.1 * t
true_x = np.cos(true_theta)
true_y = np.sin(true_theta)
obs_x = true_x + np.random.normal(0, 0.1, n_steps)
obs_y = true_y + np.random.normal(0, 0.1, n_steps)
# ==================== 运行UKF ====================
ukf = UnscentedKalmanFilter()
ukf_x = []
ukf_y = []
for z_x, z_y in zip(obs_x, obs_y):
x_pred, sigma_pred = ukf.predict()
x_est = ukf.update(np.array([[z_x], [z_y]]), sigma_pred)
ukf_x.append(x_est[0, 0])
ukf_y.append(x_est[1, 0])
# ==================== 可视化对比（EKF vs UKF） ====================
# 先运行EKF获取结果
ekf = ExtendedKalmanFilter()
ekf_x = []
ekf_y = []
for z_x, z_y in zip(obs_x, obs_y):
ekf.predict()
x_est = ekf.update(np.array([[z_x], [z_y]]))
ekf_x.append(x_est[0, 0])
ekf_y.append(x_est[1, 0])
# 绘图
plt.figure(figsize=(12, 6))
# 左图：EKF结果
plt.subplot(1, 2, 1)
plt.plot(true_x, true_y, 'g-', label='真实轨迹', linewidth=3)
plt.scatter(obs_x, obs_y, c='r', s=10, label='观测点', alpha=0.5)
plt.plot(ekf_x, ekf_y, 'b-', label='EKF估计', linewidth=2)
plt.axis('equal')
plt.title('扩展卡尔曼滤波器（EKF）')
plt.legend()
plt.grid(True, alpha=0.3)
# 右图：UKF结果
plt.subplot(1, 2, 2)
plt.plot(true_x, true_y, 'g-', label='真实轨迹', linewidth=3)
plt.scatter(obs_x, obs_y, c='r', s=10, label='观测点', alpha=0.5)
plt.plot(ukf_x, ukf_y, 'orange', label='UKF估计', linewidth=2)
plt.axis('equal')
plt.title('无损卡尔曼滤波器（UKF）')
plt.legend()
plt.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()运行效果
- UKF 的估计轨迹比 EKF 更贴近真实圆周轨迹，尤其是在噪声较大时；
- 对比图能清晰看到：EKF 有轻微的偏差，而 UKF 几乎和真实轨迹重合。
五、粒子滤波（19.5，补充完整）
核心概念
粒子滤波是 "万能的非线性滤波方法"，核心思路是：
用一堆 "粒子"（每个粒子代表一个可能的状态）来近似状态的概率分布，通过 "重采样" 让粒子向真实状态聚集。
可以比喻成：
找一个藏在草丛里的兔子，先撒一堆兔子玩偶（粒子）在草丛里，然后根据兔子的脚印（观测数据），把离脚印远的玩偶扔掉，离脚印近的玩偶多复制几个，最终所有玩偶都会聚集在兔子真实位置周围。
完整 Python 代码（粒子滤波实现）
import numpy as np
import matplotlib.pyplot as plt
# ==================== Mac系统Matplotlib中文显示配置 ====================
plt.rcParams['font.sans-serif'] = ['Arial Unicode MS', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False
plt.rcParams['font.family'] = 'sans-serif'
plt.rcParams['font.family'] = 'Arial Unicode MS'
plt.rcParams['axes.facecolor'] = 'white'
class ParticleFilter:
"""粒子滤波器实现类：圆周运动跟踪"""
def __init__(self, n_particles=1000):
self.n_particles = n_particles # 粒子数量
self.particles = np.zeros((4, n_particles)) # 粒子状态：[x, y, theta, omega]
# 初始化粒子（围绕初始位置随机分布）
self.particles[0, :] = 1.0 + np.random.normal(0, 0.1, n_particles)
self.particles[1, :] = 0.0 + np.random.normal(0, 0.1, n_particles)
self.particles[2, :] = 0.0 + np.random.normal(0, 0.1, n_particles)
self.particles[3, :] = 0.1 + np.random.normal(0, 0.01, n_particles)
self.weights = np.ones(n_particles) / n_particles # 粒子权重
self.dt = 0.1 # 时间步长
self.Q = np.diag([0.001, 0.001, 0.001, 0.001]) # 过程噪声
self.R = np.diag([0.01, 0.01]) # 观测噪声
def predict(self):
"""预测阶段：粒子传播（加过程噪声）"""
for i in range(self.n_particles):
# 非线性状态转移（圆周运动）
x_pos, y_pos, theta, omega = self.particles[:, i]
new_theta = theta + omega * self.dt
new_x = np.cos(new_theta)
new_y = np.sin(new_theta)
new_omega = omega
# 加过程噪声
new_x += np.random.normal(0, np.sqrt(self.Q[0, 0]))
new_y += np.random.normal(0, np.sqrt(self.Q[1, 1]))
new_theta += np.random.normal(0, np.sqrt(self.Q[2, 2]))
new_omega += np.random.normal(0, np.sqrt(self.Q[3, 3]))
self.particles[:, i] = [new_x, new_y, new_theta, new_omega]
def update(self, z):
"""更新阶段：计算权重 + 重采样"""
# 1. 计算每个粒子的权重（观测似然）
for i in range(self.n_particles):
z_pred = np.array([self.particles[0, i], self.particles[1, i]])
# 高斯似然
diff = z - z_pred
self.weights[i] = np.exp(-0.5 * np.dot(diff.T, np.dot(np.linalg.inv(self.R), diff)))
# 2. 归一化权重
self.weights += 1e-300 # 避免权重为0
self.weights /= np.sum(self.weights)
# 3. 重采样（轮盘赌法）
indices = np.random.choice(self.n_particles, size=self.n_particles, p=self.weights)
self.particles = self.particles[:, indices]
self.weights = np.ones(self.n_particles) / self.n_particles
def get_state(self):
"""计算粒子的均值作为估计状态"""
return np.average(self.particles, axis=1, weights=self.weights)
# ==================== 生成模拟数据 ====================
np.random.seed(42)
t = np.arange(0, 20, 0.1)
n_steps = len(t)
true_theta = 0.1 * t
true_x = np.cos(true_theta)
true_y = np.sin(true_theta)
obs_x = true_x + np.random.normal(0, 0.1, n_steps)
obs_y = true_y + np.random.normal(0, 0.1, n_steps)
# ==================== 运行粒子滤波 ====================
pf = ParticleFilter(n_particles=1000)
pf_x = []
pf_y = []
for z_x, z_y in zip(obs_x, obs_y):
pf.predict()
pf.update(np.array([z_x, z_y]))
state = pf.get_state()
pf_x.append(state[0])
pf_y.append(state[1])
# ==================== 可视化对比 ====================
plt.figure(figsize=(10, 10))
plt.plot(true_x, true_y, 'g-', label='真实轨迹', linewidth=3)
plt.scatter(obs_x, obs_y, c='r', s=10, label='观测点', alpha=0.5)
plt.plot(pf_x, pf_y, 'purple', label='粒子滤波估计', linewidth=2)
plt.axis('equal')
plt.xlabel('X位置 (m)')
plt.ylabel('Y位置 (m)')
plt.title('粒子滤波器：圆周运动跟踪对比')
plt.legend()
plt.grid(True, alpha=0.3)
plt.show()
总结
核心知识点回顾
1.卡尔曼滤波器 ：仅适用于线性系统 + 高斯噪声，核心是 "预测 - 更新" 两步，通过卡尔曼增益权衡预测和观测的权重；
2.扩展卡尔曼滤波器（EKF） ：通过雅可比矩阵线性化处理非线性系统，是卡尔曼滤波的非线性扩展，但一阶近似会引入误差；
3.无损卡尔曼滤波器（UKF） ：通过sigma 点采样替代线性化，非线性估计精度比 EKF 更高；
4.粒子滤波 ：基于蒙特卡洛采样，适用于任意非线性、非高斯系统，是最通用的时序估计方法（但计算量更大）。
代码使用说明
1.所有代码均已适配 Mac 系统的 Matplotlib 中文显示，可直接复制运行；
2.核心类（KalmanFilter/EKF/UKF/ParticleFilter）可复用，只需修改状态转移模型 / 观测模型即可适配不同场景；
3.可视化部分通过对比图直观展示了各算法的效果，便于理解不同滤波器的优缺点。
希望这篇解析能帮你彻底搞懂《计算机视觉：模型、学习和推理》第 19 章的时序模型！如果有问题，欢迎在评论区交流～