### 实现扩展卡尔曼滤波（EKF）的Python代码 扩展卡尔曼滤波（EKF）是卡尔曼滤波器的非线性扩展，通过线性化系统模型来进行状态估计。EKF 的核心思想是使用泰勒展开对非线性函数进行近似，然后应用标准卡尔曼滤波器的预测和更新步骤。 以下是一个基于二维目标跟踪的 EKF 实现，假设系统状态包括位置和速度，观测输入为位置信息。 ```python import numpy as np from numpy.linalg import inv class ExtendedKalmanFilter: def __init__(self, initial_state, initial_covariance, process_noise, measurement_noise): self.state = np.array(initial_state) # 初始状态 [x, y, vx, vy] self.P = np.array(initial_covariance) # 初始协方差 self.Q = np.array(process_noise) # 过程噪声协方差 self.R = np.array(measurement_noise) # 测量噪声协方差 def predict(self, dt): # 状态转移函数（非线性） F = np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ]) self.state = F @ self.state # 预测状态 self.P = F @ self.P @ F.T + self.Q # 预测协方差 def update(self, z): # 观测函数（非线性） def h(x): return np.array([x[0], x[1]]) # 只观测位置 # 计算雅可比矩阵 H = np.array([ [1, 0, 0, 0], [0, 1, 0, 0] ]) y = z - h(self.state) # 计算残差 S = H @ self.P @ H.T + self.R # 残差协方差 K = self.P @ H.T @ inv(S) # 计算卡尔曼增益 self.state += K @ y # 更新状态 self.P = (np.eye(len(self.P)) - K @ H) @ self.P # 更新协方差 def get_state(self): return self.state # 示例：使用EKF进行目标跟踪 if __name__ == "__main__": # 初始状态 [x, y, vx, vy] initial_state = [0, 0, 1, 1] # 初始协方差矩阵 initial_covariance = np.eye(4) # 过程噪声协方差 process_noise = np.eye(4) * 0.01 # 测量噪声协方差 measurement_noise = np.eye(2) * 0.1 ekf = ExtendedKalmanFilter(initial_state, initial_covariance, process_noise, measurement_noise) # 模拟观测数据 measurements = [ [1.1, 1.0], [2.0, 1.9], [3.1, 3.0], [4.0, 4.1] ] for z in measurements: ekf.predict(dt=1.0) ekf.update(z) print("估计状态:", ekf.get_state()) ``` 上述代码中，预测阶段基于状态转移模型更新状态和协方差[^1]，更新阶段则利用观测值对预测结果进行修正[^2]。在非线性情况下，状态转移和观测函数需要通过线性化处理，例如使用雅可比矩阵计算[^3]。 ### 应用场景 - **目标跟踪**：如自动驾驶中的车辆跟踪，使用激光雷达或毫米波雷达获取观测值[^4]。 - **传感器融合**：将来自不同传感器的数据融合，提高状态估计的精度。 - **机器人定位**：在机器人导航中，EKF 可用于融合里程计和 GPS 数据。 ### 扩展方向 - 引入更复杂的非线性模型，例如考虑加速度或角速度。 - 使用真实传感器数据进行测试，例如从 ROS 系统中获取激光雷达或IMU数据。 - 将 EKF 替换为无迹卡尔曼滤波（UKF），避免雅可比矩阵计算。