UKFslam

UKF

UKF

• KF 系列求解：
  • Kalman filter 需要线性模型
  • EKF通过泰勒展开线性化
  • 更好的方式线性化 -> Unscented Transform -> UKF
    • 计算一组（所谓的）sigma 点
    • 从变换和加权的 sigma 点计算高斯
• Unscented Transform
  • 计算一系列的 Sigma 点
  • 每个Sigma点有一个权重
  • 通过非线性函数转换 Sigma 点
  • 权重点计算高斯

Sigma and weight

• Sigma 点
  • 选择 ${\chi }^{[i]}$，$w^{[i]}$ 使得：
    • ${\sum }_i{w}^{[i]}=1$
    • $\mu ={\sum }_i{w}^{[i]}{\chi }^{[i]}$
    • $\sum ={\sum }_i{w}^{[i]}({\chi }^{[i]}-\mu )({\chi }^{[i]}-\mu {)}^T$
  • 没有唯一的解决方案
  • 如何选择Sigma点
    • 第一个Sigma点也是均值 ${\chi }^{[}0]=\mu$
    • ${\chi }^{[}i]=\mu +(\sqrt{(n+\lambda )\sum }{)}_i$ for i=1,…,n
    • ${\chi }^{[}i]=\mu -(\sqrt{(n+\lambda )\sum }{)}_{i-n}$ for i=1+n,…,2n
  • 矩阵平方根
    • 定义 $S$，$\sum =SS$
    • 通过对角化计算：
      • $\sum =VD{V}^{-1}=\left(\begin{array}{ccc}{d}_{11}& \cdots & 0\\ ⋮& \ddots & ⋮\\ 0& \cdots & d_{nn}\end{array}\right)=V\left(\begin{array}{ccc}\sqrt{d_{11}}& \cdots & 0\\ ⋮& \ddots & ⋮\\ 0& \cdots & \sqrt{d_{nn}}\end{array}\right)\left(\begin{array}{ccc}\sqrt{d_{11}}& \cdots & 0\\ ⋮& \ddots & ⋮\\ 0& \cdots & \sqrt{d_{nn}}\end{array}\right)V^{-1}$
      • 因此可以定义： $S=V{D}^{1/2}{V}^{-1}$
    • Cholesky Matrix 平方根法
      • 矩阵平方根的替代定义：$L,\sum =L{L}^T$
      • $L,\sum$ 有相同的特征向量
      • Sigma 点可以但不必位于 $\sum$
  • 如何设置权重
    • $w_m^{[0]}=\frac{\lambda }{n+\lambda }$
    • $w_c^{[0]}=w_m^{[0]}+(1-{\alpha }^2+\beta )$
    • $w_c^{[i]}=w_m^{[i]}+\frac{1}{2(n+\lambda )}$ for i=1,…,2n

UKF Algorithm

• Prediction

  • ${\chi }_{t-1}=({\mu }_{t-1},{\mu }_{t-1}+\sqrt{(n+\lambda ){\sum }_{t-1}},{\mu }_{t-1}-\sqrt{(n+\lambda ){\sum }_{t-1}})$

  • ${\bar{\chi }}_t^{\ast }=g(u_t,{\chi }_{t-1})$

  • $\overline{{\mu }_t}={\sum }_{i=0}^{2n}{w}_m^{[i]}{\bar{\chi }}_t^{\ast [i]}$

  • ${\bar{\Sigma }}_t={\sum }_{i=0}^{2n}{w}_c^{[i]}({\bar{\chi }}_t^{\ast [i]}-\overline{{\mu }_t})({\bar{\chi }}_t^{\ast [i]}-\overline{{\mu }_t}{)}^T+R_t$

• Correction

  • $\overline{{\chi }_t}=(\overline{{\mu }_t},\overline{{\mu }_t}+\sqrt{(n+\lambda ){\sum }_{t-1}},\overline{{\mu }_t}-\sqrt{(n+\lambda ){\sum }_{t-1}})$
  • $\overline{z_t}=h(\overline{{\chi }_t})$
  • $\widehat{z_t}={\sum }_{i=0}^{2n}{w}_m^{[i]}{\bar{z}}_t^{[i]}$
  • $S_t={\sum }_{i=0}^{2n}{w}_c^{[i]}({\bar{\chi }}_t^{[i]}-{\bar{\mu }}_t)({\bar{\chi }}_t^{[i]}-{\bar{\mu }}_t{)}^T$
  • $K_t={\sum ^{ˉ}}_t^{x,z}{S}_t^{-1}$
  • ${\sum }_t={\sum ^{ˉ}}_t-K_t{S}_t{K}_t^T$

UT/UKF/EKF Summary

• UT/UKF

  • 无迹卡尔曼作为线性化的替代方案
  • UT 是比泰勒展开更好的近似值
  • UT 使用 sigma 点传播
  • UT中的自由参数
  • UKF 在预测和校正步骤中使用 UT

• UKF VS EKF

  • 线性模型的结果与 EKF 相同
  • 非线性模型比 EKF 更好的近似
  • 差异通常“有点小”
  • UKF 不需要雅可比行列式
  • 相同的复杂度类
  • 比 EKF 稍慢
  • 仍然受限于高斯分布