### 无迹卡尔曼滤波（UKF）简介 无迹卡尔曼滤波（UKF）是一种适用于非线性系统的状态估计方法，与扩展卡尔曼滤波（EKF）不同，UKF 不需要对非线性函数进行线性化，而是通过 **无迹变换（Unscented Transform）** 来近似非线性变换后的均值和协方差。这种方法避免了雅可比矩阵的计算，提高了数值稳定性，尤其适用于高维或强非线性系统[^1]。 ### UKF 的核心步骤 1. **预测步骤**：通过无迹变换生成 sigma 点，将这些点通过非线性过程模型传播，然后计算预测的均值和协方差。 2. **更新步骤**：将预测的 sigma 点与观测模型结合，计算卡尔曼增益，并更新状态估计和协方差。 ### Python 实现 UKF 的示例 以下是一个基于恒定速度模型的二维 UKF 实现，用于跟踪目标的位置和速度。该示例使用 `filterpy` 库中的 `UnscentedKalmanFilter` 类进行实现。 ```python import numpy as np from filterpy.kalman import UnscentedKalmanFilter as UKF from filterpy.kalman import MerweScaledSigmaPoints # 状态转移函数（非线性） def f_cv(x, dt): """ 恒定速度模型的状态转移函数 x = [x, y, vx, vy] """ F = np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ]) return F @ x # 观测函数（只观测位置） def h_cv(x): return x[:2] # 初始化 UKF def create_ukf(dt, std_acc, std_pos): points = MerweScaledSigmaPoints(n=4, alpha=0.1, beta=2., kappa=-1) ukf = UKF(dim_x=4, dim_z=2, fx=lambda x, dt: f_cv(x, dt), hx=h_cv, dt=dt, points=points) # 初始状态协方差 ukf.x = np.array([0., 0., 1., 1.]) # 初始状态 [x, y, vx, vy] ukf.P *= 0.1 # 初始状态不确定性 # 过程噪声 ukf.Q = np.diag([std_acc**2, std_acc**2, std_acc**2, std_acc**2]) # 加速度噪声 # 观测噪声 ukf.R = np.diag([std_pos**2, std_pos**2]) # 位置测量噪声 return ukf # 示例：使用 UKF 进行目标跟踪 if __name__ == "__main__": dt = 1.0 # 时间步长 std_acc = 0.1 # 过程噪声标准差 std_pos = 0.3 # 测量噪声标准差 ukf = create_ukf(dt, std_acc, std_pos) # 模拟观测数据 measurements = [ np.array([1.1, 1.0]), np.array([2.0, 1.9]), np.array([3.1, 3.0]), np.array([4.0, 4.1]) ] for z in measurements: ukf.predict(dt=dt) ukf.update(z) print("估计状态:", ukf.x) ``` ### 代码说明 - **状态转移函数 `f_cv`**：表示恒定速度模型的状态转移关系，输入当前状态和时间步长 `dt`，输出下一时刻的状态。 - **观测函数 `h_cv`**：仅返回位置信息 `[x, y]`。 - **Sigma 点生成**：使用 `MerweScaledSigmaPoints` 生成 sigma 点，这是 UKF 的核心部分，用于捕捉状态分布的统计特性。 - **UKF 初始化**：设置初始状态、协方差、过程噪声和观测噪声。 ### UKF 优势 - **无需雅可比矩阵**：相比 EKF，UKF 不需要计算雅可比矩阵，避免了线性化误差。 - **适用于非线性系统**：在强非线性系统中表现优于 EKF。 - **更稳定的协方差更新**：通过无迹变换，UKF 能更准确地估计状态的均值和协方差。 ### 应用场景 - **自动驾驶中的目标跟踪**：使用雷达或激光雷达数据进行车辆或行人跟踪。 - **机器人定位**：融合 IMU、GPS 和里程计数据进行状态估计。 - **传感器融合**：将来自不同传感器的数据进行非线性融合，提高系统鲁棒性。