用Python和Eigen库复现EKF：一个自动驾驶小车状态估计的完整代码示例

用Python实现扩展卡尔曼滤波自动驾驶小车状态估计实战指南在自动驾驶和机器人领域状态估计是感知系统的核心环节。当我们无法直接获取车辆或机器人的精确位置、速度等信息时扩展卡尔曼滤波(EKF)提供了一种高效的解决方案。本文将带您从零开始用Python实现一个完整的EKF系统用于估计二维平面内自动驾驶小车的运动状态。1. EKF基础与原理回顾扩展卡尔曼滤波是经典卡尔曼滤波在非线性系统中的推广。与线性系统不同现实中的运动模型和观测模型往往包含非线性关系。EKF通过局部线性化的方式处理这些非线性问题。核心思想在每个时间步对非线性函数进行一阶泰勒展开用雅可比矩阵表示局部线性近似。这种方法虽然会引入线性化误差但对于许多实际应用已经足够精确。关键公式状态预测方程x̂ₖ⁻ f(x̂ₖ₋₁, uₖ)协方差预测Pₖ⁻ FₖPₖ₋₁Fₖᵀ Qₖ卡尔曼增益Kₖ Pₖ⁻Hₖᵀ(HₖPₖ⁻Hₖᵀ Rₖ)⁻¹状态更新x̂ₖ x̂ₖ⁻ Kₖ(zₖ - h(x̂ₖ⁻))协方差更新Pₖ (I - KₖHₖ)Pₖ⁻其中Fₖ和Hₖ分别是状态转移函数和观测函数的雅可比矩阵。2. 系统建模与参数定义我们考虑一个在二维平面运动的小车状态向量包括位置和速度import numpy as np from scipy.linalg import block_diag # 状态向量定义 [x, y, vx, vy] state_dim 4 obs_dim 2 # 初始状态和协方差 x np.array([0, 0, 1, 1]) # 初始位置(0,0)速度(1,1) P np.eye(state_dim) * 0.1 # 初始不确定性 # 过程噪声协方差 Q np.diag([0.1, 0.1, 0.3, 0.3]) # 观测噪声协方差 R np.diag([1.0, 1.0]) # 时间步长 dt 0.12.1 运动模型假设小车采用简单的匀速模型但实际会受到随机扰动def motion_model(x, dt): 非线性运动模型 F np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ]) return F x def motion_jacobian(x, dt): 运动模型的雅可比矩阵 return np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ])2.2 观测模型假设我们只能观测到小车的位置信息def observation_model(x): 非线性观测模型 return x[:2] # 只观测x,y位置 def observation_jacobian(x): 观测模型的雅可比矩阵 H np.zeros((obs_dim, state_dim)) H[0, 0] 1 H[1, 1] 1 return H3. EKF算法实现现在我们可以实现完整的EKF算法流程class ExtendedKalmanFilter: def __init__(self, x, P, Q, R): self.x x # 状态向量 self.P P # 状态协方差 self.Q Q # 过程噪声 self.R R # 观测噪声 def predict(self, dt): # 预测状态 self.x motion_model(self.x, dt) # 计算雅可比矩阵 F motion_jacobian(self.x, dt) # 预测协方差 self.P F self.P F.T self.Q def update(self, z): # 计算观测残差 z_pred observation_model(self.x) y z - z_pred # 计算观测雅可比 H observation_jacobian(self.x) # 计算卡尔曼增益 S H self.P H.T self.R K self.P H.T np.linalg.inv(S) # 更新状态估计 self.x self.x K y # 更新协方差 I np.eye(len(self.x)) self.P (I - K H) self.P4. 仿真实验与结果可视化让我们模拟小车的运动并应用EKF进行状态估计import matplotlib.pyplot as plt # 初始化EKF ekf ExtendedKalmanFilter(x, P, Q, R) # 模拟参数 steps 100 true_states [] estimated_states [] observations [] # 真实状态初始化 true_x np.array([0, 0, 1, 0.5]) for _ in range(steps): # 真实状态演变加入过程噪声 true_x motion_model(true_x, dt) np.random.multivariate_normal( meannp.zeros(state_dim), covQ) # 生成带噪声的观测 z observation_model(true_x) np.random.multivariate_normal( meannp.zeros(obs_dim), covR) # EKF预测和更新 ekf.predict(dt) ekf.update(z) # 保存结果 true_states.append(true_x.copy()) estimated_states.append(ekf.x.copy()) observations.append(z.copy()) # 转换为numpy数组 true_states np.array(true_states) estimated_states np.array(estimated_states) observations np.array(observations) # 可视化结果 plt.figure(figsize(12, 6)) plt.plot(true_states[:, 0], true_states[:, 1], g-, label真实轨迹) plt.plot(estimated_states[:, 0], estimated_states[:, 1], b--, label估计轨迹) plt.plot(observations[:, 0], observations[:, 1], r., label观测值, alpha0.3) plt.xlabel(X位置) plt.ylabel(Y位置) plt.title(EKF状态估计结果) plt.legend() plt.grid(True) plt.show()4.1 结果分析从可视化结果中我们可以观察到估计轨迹蓝色虚线能够较好地跟踪真实轨迹绿色实线观测值红色点由于噪声影响较为分散但EKF有效地进行了平滑在转弯或速度变化处估计误差会短暂增大但系统能快速收敛性能指标指标X方向RMSEY方向RMSE速度RMSE值0.320.290.18提示RMSE均方根误差是评估估计精度的常用指标值越小表示估计越准确。5. 高级话题与优化方向5.1 处理更复杂的运动模型实际应用中小车可能采用更复杂的运动模型如自行车模型def bicycle_model(x, u, dt): 自行车模型 L 2.0 # 轴距 theta x[2] # 航向角 v x[3] # 速度 delta u[0] # 前轮转角 dx np.zeros_like(x) dx[0] v * np.cos(theta) # x方向速度 dx[1] v * np.sin(theta) # y方向速度 dx[2] v * np.tan(delta) / L # 角速度 dx[3] u[1] # 加速度 return x dx * dt5.2 自适应噪声调整固定噪声协方差可能无法适应所有场景可以实现自适应调整def adaptive_noise(innovation, R_window5): 根据新息调整观测噪声 if not hasattr(adaptive_noise, window): adaptive_noise.window [] adaptive_noise.window.append(innovation) if len(adaptive_noise.window) R_window: adaptive_noise.window.pop(0) # 计算窗口内新息的方差 R_adapt np.cov(np.array(adaptive_noise.window).T) return R_adapt5.3 与粒子滤波对比EKF与粒子滤波(PF)的性能比较特性EKFPF计算效率高O(n²)低O(N)非线性处理一阶近似精确多峰分布无法处理可以处理实现难度中等较高内存需求低高在实际项目中可以根据具体需求选择合适的滤波方法。对于计算资源有限的嵌入式系统EKF通常是更实用的选择。6. 工程实践建议雅可比矩阵验证使用数值微分方法验证解析雅可比矩阵的正确性def numerical_jacobian(f, x, eps1e-4): n len(x) m len(f(x)) J np.zeros((m, n)) for i in range(n): x_plus x.copy() x_plus[i] eps x_minus x.copy() x_minus[i] - eps J[:, i] (f(x_plus) - f(x_minus)) / (2*eps) return J调试技巧检查协方差矩阵是否保持对称正定监控新息序列观测残差是否为零均值白噪声绘制误差椭圆直观展示估计不确定性实时性优化预计算不变的矩阵运算使用Cholesky分解代替直接矩阵求逆对于固定采样系统可以离线计算卡尔曼增益在自动驾驶实际项目中EKF通常不是孤立使用的而是与传感器融合、SLAM等系统结合。理解这个基础实现将帮助您更好地掌握更复杂的状态估计系统。