主题
非线性状态估计:从 EKF 雅可比线性化到 UKF 无迹变换
经典卡尔曼滤波(KF)在线性高斯假设下能给出最小均方误差意义下的最优解,但真实机器人与雷达追踪系统中,状态转移(如恒定转弯率运动)与传感器观测(如极坐标下的距离-方位角 (r, θ))几乎都是强非线性的。
为什么一阶线性化会“偏心”?
假设雷达测得目标距离 r = 10 m、方位角 θ = 90°,当角度存在较大不确定性 σθ 时,真实目标在笛卡尔平面 (x, y) = (r cos θ, r sin θ) 上的分布会弯曲成经典的 “香蕉形”。
- EKF 的做法:在均值点处对非线性函数做一阶泰勒展开(计算雅可比矩阵 J),相当于用一条切线近似整条曲线,均值不会随弯曲而下移,协方差也容易出现过度自信。
- UKF 的做法:与其近似非线性函数,不如直接近似概率分布。通过构造 2n+1 个确定性 Sigma 点,将每个点直接代入真实非线性函数传播,再加权重构出二阶甚至三阶精度的均值与协方差。
INTERACTIVE LAB · EKF vs. UKF NONLINEAR TRANSFORM
极坐标转笛卡尔坐标:雅可比线性化 vs. 无迹 Sigma 点采样
真实均值 (9.11 m) EKF 均值 (10.00 m, 偏差 0.89 m) UKF 均值 (9.16 m, 偏差 0.05 m) UKF 5个 Sigma 点
核心数学形式对比
EKF First-Order Jacobian vs. UKF Sigma-Point Propagation
Py ≈ Jh Px JhT, Jh = ∂h/∂x |x̂(1) EKF 一阶协方差传播
χ0 = x̂, χi = x̂ ± √(n + λ) · (√Px)i(2) UKF Sigma 点采样
ȳ = Σ Wi(m) h(χi), Py = Σ Wi(c) (h(χi) − ȳ)(h(χi) − ȳ)T(3) 无迹重构均值与协方差
工程选型对照
| 维度 | 扩展卡尔曼 (EKF) | 无迹卡尔曼 (UKF) | 工程建议 |
|---|---|---|---|
| 线性化精度 | 一阶泰勒展开 | 二阶以上统计矩 | 强非线性、大角度噪声优先选 UKF |
| 导数需求 | 需推导/求雅可比矩阵 J | 无需解析求导(黑盒可用) | 复杂动力学模型用 UKF 更省维护成本 |
| 计算复杂度 | 单次传播 O(n3) | 2n+1 个 Sigma 点并行传播 | 低维实时端侧(n ≤ 15)两者均毫秒级 |
| 数值稳定性 | 协方差矩阵易失正定 | 配合平方根 SR-UKF 极稳 | 嵌入式建议采用 Cholesky 分解实现 |
实践经验:在毫米波雷达目标跟踪与视觉惯性里程计(VIO)中,当目标近距离高速切向运动导致方位角误差放大时,将 EKF 替换为 UKF 或在 EKF 中引入基于新息(Innovation)的自适应噪声缩放,通常能显著降低航迹发散概率。