别再死记公式了！用Python代码实例图解KF、EKF、ESKF的核心差异

用Python代码实例图解KF、EKF、ESKF的核心差异

在传感器数据处理和状态估计领域，卡尔曼滤波家族（Kalman Filter Family）一直是工程师和研究人员的重要工具。但对于许多初学者来说，面对复杂的数学公式和抽象的理论推导，往往感到无从下手。本文将通过一个统一的Python示例，直观展示标准卡尔曼滤波（KF）、扩展卡尔曼滤波（EKF）和误差状态卡尔曼滤波（ESKF）的实现差异，帮助读者从代码层面理解它们的适用场景和核心思想。

我们将模拟一个典型的非线性系统状态估计问题：在二维平面中追踪一个进行圆周运动的物体。这个物体受到系统噪声和观测噪声的影响，我们需要通过滤波算法从带噪声的观测数据中估计其真实状态。下面先定义我们的仿真环境：

import numpy as np import matplotlib.pyplot as plt from scipy.linalg import expm # 系统参数设置 dt = 0.1 # 时间步长 T = 10.0 # 总时长 steps = int(T/dt) # 总步数 radius = 5.0 # 圆周运动半径 omega = 1.0 # 角速度(rad/s) # 真实状态函数 - 圆周运动 def true_motion(t): x = radius * np.cos(omega * t) y = radius * np.sin(omega * t) vx = -radius * omega * np.sin(omega * t) vy = radius * omega * np.cos(omega * t) return np.array([x, y, vx, vy]) # 生成真实轨迹 true_states = np.array([true_motion(t*dt) for t in range(steps)])

1. 标准卡尔曼滤波(KF)实现

标准卡尔曼滤波是处理线性高斯系统的最优估计算法。我们先看KF在这种非线性系统中的表现，虽然理论上它并不适合这种场景。

1.1 KF系统建模

KF假设系统是线性的，因此我们需要对非线性运动进行线性近似：

# KF系统矩阵 - 线性近似(假设小角度变化) F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) # 状态转移矩阵 H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) # 观测矩阵 # 噪声协方差矩阵 Q = np.diag([0.1, 0.1, 0.3, 0.3])**2 # 过程噪声 R = np.diag([0.5, 0.5])**2 # 观测噪声 # 初始状态和协方差 x0 = np.array([radius, 0, 0, radius*omega]) # 初始状态(起点在x=5,y=0) P0 = np.diag([0.5, 0.5, 0.5, 0.5]) # 初始协方差

1.2 KF预测与更新实现

KF的核心在于预测和更新两个步骤的交替进行：

def kalman_filter(observations): x = x0.copy() P = P0.copy() estimates = [] for z in observations: # 预测步骤 x = F @ x P = F @ P @ F.T + Q # 更新步骤 y = z - H @ x # 新息 S = H @ P @ H.T + R K = P @ H.T @ np.linalg.inv(S) # 卡尔曼增益 x = x + K @ y P = (np.eye(4) - K @ H) @ P estimates.append(x.copy()) return np.array(estimates)

1.3 KF性能分析

我们生成带噪声的观测数据并运行KF：

# 生成带噪声的观测 noisy_obs = true_states[:,:2] + np.random.multivariate_normal([0,0], R, steps) # 运行KF kf_estimates = kalman_filter(noisy_obs) # 计算误差 kf_error = np.linalg.norm(kf_estimates[:,:2] - true_states[:,:2], axis=1)

KF在这个非线性系统中的表现存在明显局限。由于它强制使用线性模型近似非线性运动，当物体运动方向变化较快时（如圆周运动的拐弯处），估计误差会显著增大。下表比较了KF在不同位置的表现：

注意：KF在非线性系统中的性能下降是预期内的，这正好引出了我们需要更高级滤波方法的原因。

2. 扩展卡尔曼滤波(EKF)实现

EKF通过局部线性化处理非线性系统，是KF在非线性领域的直接扩展。

2.1 EKF系统建模

对于我们的圆周运动系统，需要定义非线性状态转移和观测函数：

def f(x, dt): """非线性状态转移函数""" theta = np.arctan2(x[1], x[0]) new_theta = theta + omega * dt return np.array([ radius * np.cos(new_theta), radius * np.sin(new_theta), -radius * omega * np.sin(new_theta), radius * omega * np.cos(new_theta) ]) def h(x): """非线性观测函数""" return x[:2] # 只能观测位置

2.2 EKF雅可比矩阵计算

EKF的核心是计算雅可比矩阵，实现局部线性化：

def compute_jacobian_F(x, dt): """计算状态转移雅可比矩阵""" theta = np.arctan2(x[1], x[0]) return np.array([ [0, 0, 1, 0], [0, 0, 0, 1], [-omega**2 * np.cos(theta), -omega**2 * np.sin(theta), 0, 0], [omega**2 * np.sin(theta), -omega**2 * np.cos(theta), 0, 0] ]) * dt + np.eye(4) def compute_jacobian_H(x): """计算观测雅可比矩阵""" return np.array([[1, 0, 0, 0], [0, 1, 0, 0]])

2.3 EKF算法实现

EKF的预测和更新步骤与KF类似，但使用雅可比矩阵：

def extended_kalman_filter(observations): x = x0.copy() P = P0.copy() estimates = [] for z in observations: # 预测步骤 x = f(x, dt) F_jac = compute_jacobian_F(x, dt) P = F_jac @ P @ F_jac.T + Q # 更新步骤 H_jac = compute_jacobian_H(x) y = z - h(x) S = H_jac @ P @ H_jac.T + R K = P @ H_jac.T @ np.linalg.inv(S) x = x + K @ y P = (np.eye(4) - K @ H_jac) @ P estimates.append(x.copy()) return np.array(estimates)

2.4 EKF性能分析

运行EKF并分析其性能：

ekf_estimates = extended_kalman_filter(noisy_obs) ekf_error = np.linalg.norm(ekf_estimates[:,:2] - true_states[:,:2], axis=1)

EKF的表现明显优于KF，特别是在转弯区域。下表展示了EKF的误差分布：

EKF的关键优势在于它通过雅可比矩阵捕捉了系统的非线性特性。然而，计算雅可比矩阵增加了计算复杂度，且在某些高度非线性区域，一阶近似可能不够精确。

3. 误差状态卡尔曼滤波(ESKF)实现

ESKF采用不同的思路，它估计状态误差而非状态本身，特别适合惯性导航等应用。

3.1 ESKF系统建模

ESKF将状态分解为名义状态和误差状态：

# ESKF参数设置 eskf_Q = np.diag([0.1, 0.1, 0.01])**2 # 误差状态过程噪声 eskf_R = np.diag([0.5, 0.5])**2 # 观测噪声 # 初始误差状态和协方差 delta_x0 = np.zeros(3) # [dx, dy, dtheta] eskf_P0 = np.diag([0.1, 0.1, 0.01]) # 误差状态协方差

3.2 ESKF预测与更新

ESKF的实现相对复杂，需要管理名义状态和误差状态：

def error_state_kalman_filter(observations): # 名义状态初始化 nominal_state = x0.copy() # 误差状态初始化 delta_x = delta_x0.copy() P = eskf_P0.copy() estimates = [] for z in observations: # 名义状态预测(理想模型) nominal_state = f(nominal_state, dt) # 误差状态预测 theta = np.arctan2(nominal_state[1], nominal_state[0]) F_delta = np.array([ [np.cos(theta), -np.sin(theta), 0], [np.sin(theta), np.cos(theta), 0], [0, 0, 1] ]) delta_x = F_delta @ delta_x P = F_delta @ P @ F_delta.T + eskf_Q # 更新步骤 H = np.array([[1, 0, -nominal_state[1]], # 观测关于误差状态的雅可比 [0, 1, nominal_state[0]]]) # 计算观测残差 predicted_obs = nominal_state[:2] + np.array([ delta_x[0]*np.cos(theta) - delta_x[1]*np.sin(theta), delta_x[0]*np.sin(theta) + delta_x[1]*np.cos(theta) ]) y = z - predicted_obs S = H @ P @ H.T + eskf_R K = P @ H.T @ np.linalg.inv(S) delta_x = delta_x + K @ y P = (np.eye(3) - K @ H) @ P # 误差状态注入和重置 nominal_state[:2] += np.array([ delta_x[0]*np.cos(theta) - delta_x[1]*np.sin(theta), delta_x[0]*np.sin(theta) + delta_x[1]*np.cos(theta) ]) nominal_state[2:] += np.array([ -omega * (delta_x[0]*np.sin(theta) + delta_x[1]*np.cos(theta)), omega * (delta_x[0]*np.cos(theta) - delta_x[1]*np.sin(theta)) ]) delta_x = np.zeros_like(delta_x) estimates.append(nominal_state.copy()) return np.array(estimates)

3.3 ESKF性能分析

运行ESKF并评估其表现：

eskf_estimates = error_state_kalman_filter(noisy_obs) eskf_error = np.linalg.norm(eskf_estimates[:,:2] - true_states[:,:2], axis=1)

ESKF在保持与EKF相当的估计精度的同时，具有一些独特优势：

1. 数值稳定性：误差状态通常很小，避免了数值计算问题
2. 计算效率：误差状态的雅可比矩阵通常更简单
3. 模块化设计：名义状态和误差状态分离，便于系统集成

下表比较了三种滤波器的性能指标：

4. 三种滤波器的核心差异与选型指南

通过上述实现和分析，我们可以总结出KF、EKF和ESKF的关键区别：

4.1 算法原理对比

4.2 适用场景分析

• KF适用场景：

  • 严格线性系统
  • 计算资源受限的嵌入式系统
  • 需要快速实现的简单应用

• EKF适用场景：

  • 中度非线性系统
  • 系统模型已知且可微
  • 计算资源相对充足的场景

• ESKF适用场景：

  • 高动态非线性系统(如无人机、自动驾驶)
  • 需要高数值稳定性的应用
  • 惯性导航等误差敏感系统

4.3 实际应用建议

1. 从KF开始：对于新问题，总是先尝试KF，它作为基准能帮助理解问题的基本性质。

2. EKF的调优技巧：

   • 仔细验证雅可比矩阵的实现
   • 调整过程噪声Q和观测噪声R矩阵
   • 考虑使用数值微分验证解析雅可比

3. ESKF的实现要点：

   • 确保名义状态和误差状态的正确转换
   • 合理设计误差状态重置策略
   • 特别注意角度/旋转相关的误差表示

# 可视化三种滤波器的性能对比 plt.figure(figsize=(12,6)) plt.plot(kf_error, label='KF') plt.plot(ekf_error, label='EKF') plt.plot(eskf_error, label='ESKF') plt.xlabel('Time step') plt.ylabel('Position error (m)') plt.title('Filter Performance Comparison') plt.legend() plt.grid(True)

在实际项目中，滤波算法的选择还需要考虑传感器特性、系统动态性、实时性要求等因素。EKF因其平衡的性能和相对简单的实现，仍然是大多数非线性系统的首选。而ESKF在特定领域如惯性导航中展现出独特优势。