Skip to content
状态估计与 AI阅读约 7 分钟
EKFUKFNonlinear FilteringState Estimation

从极坐标雷达观测的“香蕉形”非高斯分布出发,直观对比扩展卡尔曼滤波(EKF)与无迹卡尔曼滤波(UKF)的均值与协方差传播精度。

非线性状态估计:从 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)的自适应噪声缩放,通常能显著降低航迹发散概率。

讨论区开启了自动与人工双重审核,文明交流的评论会在短时间内展示。

Built with VitePress