扩展卡尔曼滤波估计车辆运动信息 注释完整，细节描述完整，是学习卡尔曼滤波的很好案例
这个轮胎压过减速带的时候，后视镜上的挂件突然晃得厉害。我盯着仪表盘上跳动的速度值，突然好奇车载电脑到底怎么算出这些动态参数的——直到后来接触到扩展卡尔曼滤波（EKF），才发现这货简直是车辆状态估计的劳模。
咱们先来看个简化版的车辆运动模型。假设咱们需要跟踪四个关键状态：x方向位置、y方向位置、航向角θ，还有速度v。这时候系统方程可能会写成这样：
def state_transition(state, dt): x, y, theta, v = state new_x = x + v * np.cos(theta) * dt # 简单匀速圆周运动模型 new_y = y + v * np.sin(theta) * dt new_theta = theta + (v * dt) / wheelbase # 假设有轴距参数 new_v = v # 暂不考虑加速度 return np.array([new_x, new_y, new_theta, new_v])但现实中的传感器数据可不会这么听话。比如方向盘转角传感器突然给个60度，这时候车辆运动轨迹明显呈现非线性——这就是标准卡尔曼滤波hold不住的场景了。EKF的聪明之处在于，它把非线性函数在当前估计点处一阶泰勒展开，用雅可比矩阵实现局部线性化。
来看个实际的雅可比矩阵计算片段。假设我们的观测模型包含GPS位置和陀螺仪角速度：
def jacobian_h(state): x, y, theta, v = state return np.array([ [1, 0, 0, 0], # x观测 [0, 1, 0, 0], # y观测 [0, 0, 1, 0] # 航向角观测 ]) # 状态转移雅可比矩阵（考虑转向角影响） def jacobian_f(state, delta, dt): _, _, theta, v = state return np.array([ [1, 0, -v * np.sin(theta) * dt, np.cos(theta) * dt], [0, 1, v * np.cos(theta) * dt, np.sin(theta) * dt], [0, 0, 1, dt / wheelbase], [0, 0, 0, 1] ])重点看第三行那个dt/wheelbase，这个项反映了转向角变化对航向的影响。实际调试时会发现，当车辆低速大角度转向时，这个线性近似可能会产生明显误差，这时候可能需要更频繁的更新周期。
实测中处理IMU数据时遇到过有趣的现象——当车辆经过减速带时，z轴加速度会产生高频噪声。这时候在预测步骤中加入运动学约束就特别重要：
# 预测步骤代码片段 def predict(self, delta, dt): # 计算雅可比矩阵 F = self.jacobian_f(self.state, delta, dt) # 状态预测 self.state = self.state_transition(self.state, delta, dt) # 协方差预测 self.P = F @ self.P @ F.T + self.Q # Q为过程噪声协方差 # 处理非对称噪声的特殊情况 if abs(delta) > np.radians(30): # 大转向角时增加过程噪声 self.Q[2,2] *= 2 # 航向角噪声增强这里有个工程实践中的小技巧：当检测到方向盘转角超过30度时，主动增大过程噪声的协方差。这相当于告诉滤波器："现在车辆运动状态变化剧烈，别太相信之前的模型"，实测中能有效防止滤波器发散。
最后分享一个调试时发现的"鬼影"问题：在长直道行驶时，GPS信号波动会导致估计位置出现正弦振荡。后来发现是观测噪声矩阵设置不合理，修改后加入速度相关项得以解决：
# 观测噪声矩阵优化 R = np.diag([3.0**2, 3.0**2, 0.1**2]) # 原始设置 R_optimized = np.diag([(3 + 0.2*abs(v))**2, (3 + 0.2*abs(v))**2, 0.1**2]) # 速度相关噪声这个改动背后的物理意义很直观——车速越快，GPS定位误差的波动范围自然越大。这种动态调整噪声的策略，比固定参数更能适应真实道路场景。
在停车场实测时，看着滤波后的轨迹曲线完美贴合真实路径，那种成就感就像第一次拆开发动机看清内部构造。EKF的优雅之处在于，它用数学工具巧妙处理了现实世界中的不确定性，就像老司机凭感觉就能判断车身位置——只不过这次，我们用矩阵运算实现了这种直觉。
