5 概率机器人 Probabilistic Robotics 无迹卡尔曼滤波算法

1 前言

• 前文介绍了扩展卡尔曼滤波，它对非线性函数线性化的方法是泰勒展开式，但这不是唯一线性化的方法
• 本文介绍的无迹卡尔曼滤波(unscented Kalman filter, UKF)是应用无迹变换进行线性化
• 无迹变换要比泰勒展开式更加准确，所以对于非线性滤波效果更好

2 通过无迹变换实现线性化

• 这里不做公式推导和证明，仅给出算法所需无迹变换实现线性化的公式
• 无迹变换是通过从高斯分布中提取$\sigma$点(${\chi }^{[i]}$)，再对这些$\sigma$点进行非线性变换，具体方法见第3部分
• 对于$n$维高斯分布要提取$2n+1$个$\sigma$点(${\chi }^{[i]}$)
• 这些$\sigma$点(${\chi }^{[i]}$)位于均值附近，对称分布:
  $$\begin{gathered} \begin{array}{rl}{\chi }^{[0]}& =\mu \\ {\chi }^{[i]}& =\mu +{\left(\sqrt{(n+\lambda )\Sigma }\right)}_{i}i=1,\cdots ,n\\ {\chi }^{[i]}& =\mu -{\left(\sqrt{(n+\lambda )\Sigma }\right)}_{i-n}i=n+1,\cdots ,2n\end{array} \\ \text{式中：}\lambda ={\alpha }^2(n+k)-n(\alpha ,k\text{:为尺度参数，决定σ点距离均值的远近}) \\ \text{注意：(n+λ)Σ 是矩阵开平方，例如：B =A⇒ATA=B, matlab函数为sqrtm()} \end{gathered}$$
• 有两个权重：
  • $w_m^{[i]}$:用于计算均值
  • $w_c^{[i]}$:用于计算方差
    $$\begin{array}{rl}{w}_m^{[0]}& =\frac{\lambda }{n+\lambda }\\ w_c^{[0]}& =\frac{\lambda }{n+\lambda }\\ w_m^{[i]}& \end{array}$$