当前位置：首页 > news >正文

告别EKF的雅可比矩阵：用Python从零实现一个UKF（附完整代码与车辆轨迹预测Demo）

news 2026/8/12 15:21:13

从零构建无迹卡尔曼滤波器：用Python实现车辆轨迹预测

在工程实践中，我们常常需要对动态系统进行状态估计。当系统满足线性高斯假设时，经典的卡尔曼滤波器（KF）无疑是最佳选择。然而现实世界充满了非线性——无论是车辆的运动模型还是传感器的测量过程。传统解决方案是使用扩展卡尔曼滤波器（EKF），但它需要计算复杂的雅可比矩阵，这让许多工程师望而却步。今天，我们将探索一种更优雅的解决方案：无迹卡尔曼滤波器（UKF）。

UKF通过一种称为"无损变换"的技术，巧妙地避开了雅可比矩阵的计算。它不像EKF那样对非线性函数进行线性近似，而是对概率分布进行近似。这种方法不仅实现更简单，而且在许多情况下精度更高。本文将带您从零开始实现一个完整的UKF，并用它来预测车辆的运动轨迹。

1. 为什么选择UKF而非EKF？

在深入代码之前，我们需要理解UKF的核心优势。EKF通过泰勒展开对非线性函数进行一阶近似，这要求函数是可导的，并且需要手动计算雅可比矩阵。这个过程既繁琐又容易出错，特别是当系统维度增加时。

相比之下，UKF采用了完全不同的思路：

概率分布近似：UKF通过在原始分布上精心选取一组"Sigma点"，将这些点通过非线性变换后，再重构新的高斯分布
无需导数计算：完全避免了雅可比矩阵的计算，可以处理不可导的非线性函数
更高精度：UKF的近似精度可以达到三阶矩，而EKF仅为一阶

# EKF与UKF核心区别对比 import numpy as np def ekf_predict(x, P, f, F_jacobian): """EKF预测步骤需要计算雅可比矩阵""" x_pred = f(x) F = F_jacobian(x) # 需要提供雅可比矩阵函数 P_pred = F @ P @ F.T + Q return x_pred, P_pred def ukf_predict(x, P, f): """UKF预测步骤只需要非线性函数本身""" sigma_points = compute_sigma_points(x, P) sigma_points_pred = np.array([f(point) for point in sigma_points]) x_pred, P_pred = unscented_transform(sigma_points_pred) return x_pred, P_pred + Q

从代码对比可以看出，UKF的实现明显更简洁。我们不需要为每个非线性函数单独推导和实现其雅可比矩阵，这大大降低了实现难度和维护成本。

2. Sigma点生成与权重计算

UKF的核心在于Sigma点的生成和变换。Sigma点是一组精心选择的采样点，它们能够完全捕获输入分布的统计特性（均值和协方差）。

2.1 Sigma点生成算法

对于n维状态向量x，UKF通常生成2n+1个Sigma点。这些点的位置由以下公式决定：

χ⁽⁰⁾ = x χ⁽ⁱ⁾ = x + √((n+λ)P)ᵢ, i=1,...,n χ⁽ⁱ⁺ⁿ⁾ = x - √((n+λ)P)ᵢ, i=1,...,n

其中λ = α²(n+κ)-n是缩放参数，α和κ是调节参数，√((n+λ)P)ᵢ表示矩阵平方根的第i列。

def compute_sigma_points(x, P): n = len(x) lambda_ = alpha**2 * (n + kappa) - n # 计算矩阵平方根 sqrt_matrix = scipy.linalg.sqrtm((n + lambda_) * P) sigma_points = np.zeros((2*n+1, n)) sigma_points[0] = x for i in range(n): sigma_points[1+i] = x + sqrt_matrix[:, i] sigma_points[1+n+i] = x - sqrt_matrix[:, i] return sigma_points

2.2 权重分配

每个Sigma点都有两个权重：一个用于计算均值，一个用于计算协方差。权重计算如下：

wₘ⁽⁰⁾ = λ/(n+λ) wₖ⁽⁰⁾ = λ/(n+λ) + (1-α²+β) wₘ⁽ⁱ⁾ = wₖ⁽ⁱ⁾ = 1/(2(n+λ)), i=1,...,2n

其中β用于合并先验知识（对于高斯分布，β=2是最优的）。

def compute_weights(n): lambda_ = alpha**2 * (n + kappa) - n w_m = np.zeros(2*n+1) w_c = np.zeros(2*n+1) w_m[0] = lambda_ / (n + lambda_) w_c[0] = w_m[0] + (1 - alpha**2 + beta) for i in range(1, 2*n+1): w_m[i] = 1 / (2*(n + lambda_)) w_c[i] = w_m[i] return w_m, w_c

3. 完整UKF实现

现在我们将上述概念整合成一个完整的UKF类。我们将使用CTRV（Constant Turn Rate and Velocity）模型作为车辆运动模型。

3.1 UKF类框架

class UKF: def __init__(self, dim_x, dim_z, dt, fx, hx, alpha=1e-3, beta=2, kappa=0): self.dim_x = dim_x # 状态维度 self.dim_z = dim_z # 测量维度 self.dt = dt # 时间步长 self.fx = fx # 状态转移函数 self.hx = hx # 测量函数 # UKF参数 self.alpha = alpha self.beta = beta self.kappa = kappa # 初始化状态和协方差 self.x = np.zeros(dim_x) self.P = np.eye(dim_x) # 过程噪声和测量噪声 self.Q = np.eye(dim_x) self.R = np.eye(dim_z) # 权重 self.w_m, self.w_c = self._compute_weights() def _compute_weights(self): n = self.dim_x lambda_ = self.alpha**2 * (n + self.kappa) - n w_m = np.zeros(2*n+1) w_c = np.zeros(2*n+1) w_m[0] = lambda_ / (n + lambda_) w_c[0] = w_m[0] + (1 - self.alpha**2 + self.beta) for i in range(1, 2*n+1): w_m[i] = 1 / (2*(n + lambda_)) w_c[i] = w_m[i] return w_m, w_c def _compute_sigma_points(self): n = self.dim_x lambda_ = self.alpha**2 * (n + self.kappa) - n # 计算矩阵平方根 U = scipy.linalg.cholesky((n + lambda_) * self.P) sigma_points = np.zeros((2*n+1, n)) sigma_points[0] = self.x for i in range(n): sigma_points[1+i] = self.x + U[i] sigma_points[1+n+i] = self.x - U[i] return sigma_points def _unscented_transform(self, sigma_points, noise_cov=None): # 计算变换后的均值和协方差 mean = np.sum(self.w_m[:, None] * sigma_points, axis=0) diff = sigma_points - mean[None, :] cov = np.sum(self.w_c[:, None, None] * diff[:, :, None] * diff[:, None, :], axis=0) if noise_cov is not None: cov += noise_cov return mean, cov

3.2 预测步骤实现

预测步骤将Sigma点通过状态转移函数传播，然后计算预测的均值和协方差。

def predict(self): # 生成Sigma点 sigma_points = self._compute_sigma_points() # 通过状态转移函数传播Sigma点 sigma_points_pred = np.array([self.fx(point, self.dt) for point in sigma_points]) # 计算预测均值和协方差 self.x_pred, self.P_pred = self._unscented_transform(sigma_points_pred, self.Q) return self.x_pred, self.P_pred

3.3 更新步骤实现

更新步骤将预测的Sigma点通过测量函数传播，然后计算卡尔曼增益并更新状态估计。

def update(self, z): # 生成预测Sigma点 sigma_points_pred = self._compute_sigma_points_pred() # 通过测量函数传播Sigma点 sigma_points_z = np.array([self.hx(point) for point in sigma_points_pred]) # 计算测量预测的均值和协方差 z_pred, P_z = self._unscented_transform(sigma_points_z, self.R) # 计算状态-测量互协方差 diff_x = sigma_points_pred - self.x_pred[None, :] diff_z = sigma_points_z - z_pred[None, :] P_xz = np.sum(self.w_c[:, None, None] * diff_x[:, :, None] * diff_z[:, None, :], axis=0) # 计算卡尔曼增益 K = P_xz @ np.linalg.inv(P_z) # 更新状态估计 self.x = self.x_pred + K @ (z - z_pred) self.P = self.P_pred - K @ P_z @ K.T return self.x, self.P

4. 车辆轨迹预测Demo

现在我们将实现的UKF应用于车辆轨迹预测问题。我们使用CTRV模型来描述车辆运动。

4.1 CTRV模型

CTRV模型假设车辆以恒定转率和速度运动。状态向量为：

x = [px, py, v, ψ, ψ̇]ᵀ

其中(px,py)是位置，v是速度，ψ是航向角，ψ̇是转率。

def ctrv_model(x, dt): px, py, v, psi, psi_dot = x # 避免除零错误 if abs(psi_dot) < 1e-5: px_new = px + v * np.cos(psi) * dt py_new = py + v * np.sin(psi) * dt else: px_new = px + (v/psi_dot) * (np.sin(psi + psi_dot*dt) - np.sin(psi)) py_new = py + (v/psi_dot) * (-np.cos(psi + psi_dot*dt) + np.cos(psi)) v_new = v psi_new = psi + psi_dot * dt psi_dot_new = psi_dot return np.array([px_new, py_new, v_new, psi_new, psi_dot_new])

4.2 测量模型

假设我们使用雷达测量，可以直接测量距离和角度：

def radar_measurement(x): px, py, v, psi, _ = x rho = np.sqrt(px**2 + py**2) # 距离 phi = np.arctan2(py, px) # 角度 rho_dot = (px * v * np.cos(psi) + py * v * np.sin(psi)) / rho # 径向速度 return np.array([rho, phi, rho_dot])

4.3 轨迹预测与可视化

现在我们可以整合所有组件进行轨迹预测和可视化：

# 初始化UKF ukf = UKF(dim_x=5, dim_z=3, dt=0.1, fx=ctrv_model, hx=radar_measurement) # 设置初始状态和噪声 ukf.x = np.array([0, 0, 5, 0, 0.1]) # 初始状态 ukf.P = np.diag([0.1, 0.1, 0.5, 0.1, 0.01]) # 初始协方差 ukf.Q = np.diag([0.1, 0.1, 0.1, 0.05, 0.01]) # 过程噪声 ukf.R = np.diag([0.5, 0.05, 0.5]) # 测量噪声 # 模拟真实轨迹和测量 true_states = [] measurements = [] filtered_states = [] for t in np.arange(0, 10, 0.1): # 真实状态更新 if t == 0: true_state = ukf.x.copy() else: true_state = ctrv_model(true_state, 0.1) # 生成带噪声的测量 z = radar_measurement(true_state) + np.random.randn(3) * np.sqrt(np.diag(ukf.R)) # UKF预测和更新 ukf.predict() ukf.update(z) # 保存结果 true_states.append(true_state) measurements.append(z) filtered_states.append(ukf.x.copy()) # 可视化 plt.figure(figsize=(12, 6)) true_states = np.array(true_states) filtered_states = np.array(filtered_states) # 绘制真实轨迹 plt.plot(true_states[:, 0], true_states[:, 1], 'g-', label='真实轨迹') # 绘制滤波结果 plt.plot(filtered_states[:, 0], filtered_states[:, 1], 'b--', label='UKF估计') # 绘制测量点 meas_x = [z[0] * np.cos(z[1]) for z in measurements] meas_y = [z[0] * np.sin(z[1]) for z in measurements] plt.scatter(meas_x, meas_y, c='r', s=10, label='测量点') plt.xlabel('X位置 (m)') plt.ylabel('Y位置 (m)') plt.title('UKF车辆轨迹预测') plt.legend() plt.grid(True) plt.show()

4.4 协方差椭圆可视化

为了更直观地理解UKF的估计不确定性，我们可以绘制协方差椭圆：

from matplotlib.patches import Ellipse def plot_covariance_ellipse(ax, mean, cov, nstd=2, **kwargs): """绘制协方差椭圆""" eigvals, eigvecs = np.linalg.eigh(cov[:2, :2]) angle = np.degrees(np.arctan2(eigvecs[1, 0], eigvecs[0, 0])) width, height = 2 * nstd * np.sqrt(eigvals) ellipse = Ellipse(mean[:2], width, height, angle=angle, **kwargs) ax.add_patch(ellipse) # 每隔10个点绘制一个协方差椭圆 plt.figure(figsize=(12, 6)) plt.plot(true_states[:, 0], true_states[:, 1], 'g-', label='真实轨迹') plt.plot(filtered_states[:, 0], filtered_states[:, 1], 'b--', label='UKF估计') for i in range(0, len(filtered_states), 10): plot_covariance_ellipse(plt.gca(), filtered_states[i], ukf.P, alpha=0.3, color='blue') plt.xlabel('X位置 (m)') plt.ylabel('Y位置 (m)') plt.title('UKF估计不确定性（协方差椭圆）') plt.legend() plt.grid(True) plt.show()

5. UKF调参与性能优化

虽然UKF实现相对简单，但要获得最佳性能仍需要仔细调整参数。以下是几个关键考虑因素：

5.1 UKF参数选择

α：控制Sigma点围绕均值的分布范围（通常0.001 ≤ α ≤ 1）
β：包含分布的先验知识（高斯分布时β=2最优）
κ：次要缩放参数（通常设为0或3-dim_x）

# 参数调优建议 alpha = 0.1 # 中等大小的α平衡了精度和稳定性 beta = 2 # 高斯分布假设下的最优值 kappa = 3 - 5 # 对于5维状态向量，κ=3-5= -2

5.2 噪声协方差调整

过程噪声Q和测量噪声R的选择对滤波器性能至关重要：

Q过大：滤波器过于信任测量，导致估计结果波动大
Q过小：滤波器过于信任模型，对测量变化反应迟钝
R过大：滤波器过于信任模型，忽略测量信息
R过小：滤波器过于信任测量，对噪声敏感

# 噪声协方差经验法则 ukf.Q = np.diag([0.1, 0.1, 0.1, 0.05, 0.01]) # 位置噪声 > 角度噪声 > 速度噪声 ukf.R = np.diag([0.5, 0.05, 0.5]) # 距离噪声 > 径向速度噪声 > 角度噪声

5.3 数值稳定性技巧

在实际实现中，我们需要确保协方差矩阵保持正定：

def ensure_positive_definite(P): """确保协方差矩阵正定""" min_eig = np.min(np.real(np.linalg.eigvals(P))) if min_eig < 0: P -= (min_eig - 1e-6) * np.eye(P.shape[0]) return P

此外，使用平方根UKF（SR-UKF）可以进一步提高数值稳定性：

class SquareRootUKF(UKF): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self.S = np.linalg.cholesky(self.P) # 协方差的平方根 def _cholupdate(self, S, x, w): """秩1 Cholesky更新""" for i in range(len(x)): S = scipy.linalg.cholesky_update(S, np.sqrt(abs(w)) * x[i]) return S def predict(self): # 平方根版本的预测步骤 sigma_points = self._compute_sigma_points() sigma_points_pred = np.array([self.fx(point, self.dt) for point in sigma_points]) # 平方根无迹变换 self.x_pred = np.sum(self.w_m[:, None] * sigma_points_pred, axis=0) diff = sigma_points_pred - self.x_pred[None, :] self.S_pred = self._cholupdate(np.zeros_like(self.S), diff[0], self.w_c[0]) for i in range(1, len(sigma_points_pred)): self.S_pred = self._cholupdate(self.S_pred, diff[i], self.w_c[i]) # 添加过程噪声 self.S_pred = scipy.linalg.cholesky(self.S_pred @ self.S_pred.T + self.Q) return self.x_pred, self.S_pred @ self.S_pred.T

6. UKF在工程实践中的扩展应用

虽然我们以车辆轨迹预测为例，但UKF的应用远不止于此。以下是几个典型的应用场景：

6.1 无人机状态估计

无人机通常配备IMU、GPS和视觉传感器，UKF可以融合这些传感器数据：

def imu_prediction(x, dt, accel, gyro): """IMU运动模型""" px, py, pz, vx, vy, vz, q0, q1, q2, q3 = x # 四元数更新 omega = np.linalg.norm(gyro) if omega > 1e-5: axis = gyro / omega dq = np.array([ np.cos(omega*dt/2), *axis*np.sin(omega*dt/2) ]) q = quaternion_multiply([q0, q1, q2, q3], dq) else: q = [q0, q1, q2, q3] # 速度更新 accel_world = rotate_vector(accel, q) vx += accel_world[0] * dt vy += accel_world[1] * dt vz += (accel_world[2] - 9.81) * dt # 位置更新 px += vx * dt py += vy * dt pz += vz * dt return np.array([px, py, pz, vx, vy, vz, *q])

6.2 金融时间序列预测

UKF可以用于非线性金融模型的参数估计和状态预测：

def stochastic_volatility(x, dt): """随机波动率模型""" log_price, log_vol = x theta, kappa, sigma = 0.05, 0.1, 0.2 # 模型参数 # 波动率过程 d_log_vol = kappa * (theta - log_vol) * dt + sigma * np.sqrt(dt) * np.random.randn() # 价格过程 d_log_price = -0.5 * np.exp(log_vol)**2 * dt + np.exp(log_vol) * np.sqrt(dt) * np.random.randn() return np.array([log_price + d_log_price, log_vol + d_log_vol])

6.3 机器人定位与建图

在SLAM（同时定位与建图）问题中，UKF可以有效地处理非线性观测模型：

def landmark_observation(x, landmark_pos): """地标观测模型""" robot_x, robot_y, robot_theta = x[:3] lx, ly = landmark_pos dx = lx - robot_x dy = ly - robot_y distance = np.sqrt(dx**2 + dy**2) bearing = np.arctan2(dy, dx) - robot_theta return np.array([distance, bearing])

7. UKF与其他滤波器的对比

为了全面理解UKF的优势，我们将其与几种常见滤波器进行对比：

特性	KF	EKF	UKF	粒子滤波器
非线性处理能力	无	一阶近似	三阶近似	完全非线性
计算复杂度	低	中	中	高
实现难度	低	中	中	高
导数计算需求	不需要	需要	不需要	不需要
适用维度	低到中	低到中	中到高	低
数值稳定性	高	中	高	中

从对比中可以看出，UKF在非线性处理能力、实现难度和数值稳定性之间取得了很好的平衡。特别是对于中等维度的非线性系统，UKF通常是首选方案。

8. 常见问题与调试技巧

在实际应用中，可能会遇到各种问题。以下是一些常见问题及其解决方案：

8.1 滤波器发散

症状：估计误差不断增大，最终完全偏离真实值。

可能原因：

过程噪声Q设置过小
模型不准确
数值不稳定

解决方案：

# 增大过程噪声 ukf.Q *= 10 # 检查模型实现 assert np.allclose(ctrv_model(x, dt), expected_result) # 使用平方根UKF提高数值稳定性 ukf = SquareRootUKF(...)

8.2 估计结果过于平滑

症状：滤波器对快速变化的信号响应迟钝。

可能原因：

过程噪声Q设置过大
测量噪声R设置过小

解决方案：

# 调整噪声参数 ukf.Q /= 5 ukf.R *= 2

8.3 协方差矩阵非正定

症状：协方差矩阵出现负特征值，导致Cholesky分解失败。

可能原因：

数值误差累积
不稳定的矩阵运算

解决方案：

# 添加小的正则化项 ukf.P += 1e-6 * np.eye(ukf.dim_x) # 或者使用更稳健的矩阵分解 try: U = scipy.linalg.cholesky(P) except: U = scipy.linalg.sqrtm(P).real

9. 性能优化与高级技巧

对于需要更高性能的应用，可以考虑以下优化技巧：

9.1 并行化Sigma点变换

Sigma点的变换可以并行计算，这在状态维度高时特别有用：

from concurrent.futures import ThreadPoolExecutor def parallel_transform(sigma_points, func): """并行变换Sigma点""" with ThreadPoolExecutor() as executor: results = list(executor.map(func, sigma_points)) return np.array(results) # 在predict和update中使用 sigma_points_pred = parallel_transform(sigma_points, lambda x: self.fx(x, self.dt))

9.2 自适应噪声估计

动态调整过程噪声和测量噪声可以提高滤波器在变化环境中的鲁棒性：

def adaptive_noise_estimation(innovations): """根据新息序列自适应估计噪声""" window = 10 if len(innovations) > window: recent_innov = innovations[-window:] cov = np.cov(recent_innov, rowvar=False) ukf.R = 0.9 * ukf.R + 0.1 * cov

9.3 混合UKF架构

对于特别复杂的系统，可以考虑混合UKF架构，将系统分解为线性部分和非线性部分：

class HybridUKF: def __init__(self, linear_states, nonlinear_states, ...): self.linear_states = linear_states self.nonlinear_states = nonlinear_states def predict(self): # 对线性部分使用标准KF更新 self.linear_x, self.linear_P = kf_predict(self.linear_x, self.linear_P, F, Q_linear) # 对非线性部分使用UKF self.nonlinear_x, self.nonlinear_P = ukf_predict(self.nonlinear_x, self.nonlinear_P, f_nonlinear, Q_nonlinear) def update(self, z): # 类似地分开处理线性和非线性部分 ...

10. 从理论到实践：UKF实现中的工程考量

在将UKF从理论转化为实际代码时，有几个关键工程问题需要考虑：

10.1 状态参数化选择

状态向量的表示方式会显著影响UKF的性能。例如，在姿态估计中：

欧拉角：直观但存在万向节锁问题
四元数：无奇点但需要特殊处理以保证单位约束
旋转矩阵：无奇点但参数多

def quaternion_normalize(q): """保证四元数单位长度""" norm = np.linalg.norm(q) if norm < 1e-6: return np.array([1, 0, 0, 0]) return q / norm def quaternion_sigma_points(q, P_q): """四元数的Sigma点生成""" # 先生成增量的Sigma点 delta_sigma = compute_sigma_points(np.zeros(3), P_q[3:,3:]) # 将增量转换为四元数并乘以均值 sigma_points = [] for delta in delta_sigma: axis = delta / (np.linalg.norm(delta) + 1e-6) angle = np.linalg.norm(delta) dq = np.array([ np.cos(angle/2), *(axis * np.sin(angle/2)) ]) sigma_points.append(quaternion_multiply(q, dq)) return np.array(sigma_points)

10.2 数值精度与鲁棒性

UKF对数值精度敏感，特别是在矩阵平方根计算和协方差更新时：

def robust_cholesky(P): """鲁棒的Cholesky分解""" try: return scipy.linalg.cholesky(P, lower=True) except: # 添加小的正则化项后重试 return scipy.linalg.cholesky(P + 1e-6*np.eye(P.shape[0]), lower=True)

10.3 实时性优化

对于实时应用，可以预先分配内存并优化热点代码：

class RealtimeUKF(UKF): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) # 预分配内存 self.sigma_points = np.zeros((2*self.dim_x+1, self.dim_x)) self.sigma_points_pred = np.zeros_like(self.sigma_points) self.sigma_points_z = np.zeros((2*self.dim_x+1, self.dim_z)) def predict(self): # 重用预分配数组 self._compute_sigma_points(out=self.sigma_points) for i, point in enumerate(self.sigma_points): self.sigma_points_pred[i] = self.fx(point, self.dt) self._unscented_transform(self.sigma_points_pred, out=(self.x_pred, self.P_pred)) self.P_pred += self.Q return self.x_pred, self.P_pred

11. UKF的局限性与替代方案

尽管UKF在许多场景表现优异，但它并非万能钥匙。了解其局限性有助于选择正确的工具：

11.1 UKF的主要局限

高斯假设：UKF假设状态服从高斯分布，对于多模分布效果不佳
维度灾难：Sigma点数量随维度线性增长(2n+1)，高维时计算成本高
非光滑非线性：对于高度不连续的非线性函数，UT变换可能失效

11.2 替代方案比较

场景	推荐方案	原因
高度非线性、非高斯	粒子滤波器	可以表示任意分布，适合多模情况
非常高维度系统	Ensemble KF	计算复杂度与维度无关
混合线性/非线性系统	混合UKF	对线性部分使用KF，非线性部分使用UKF
计算资源受限的嵌入式系统	EKF	实现简单，计算量可预测