UKFslam

UKF

UKF

• KF Series solution ：
  • Kalman filter Need a linear model
  • EKF Linearization by Taylor expansion
  • A better way to linearize -> Unscented Transform -> UKF
    • Calculate a set of （ So-called ）sigma spot
    • From transformation and weighted sigma Point calculation Gauss
• Unscented Transform
  • Calculate a series of Sigma spot
  • Every Sigma The point has a weight
  • Through nonlinear function transformation Sigma spot
  • Weight focus calculation Gauss

Sigma and weight

• Sigma spot
  • choice ${\chi }^{[i]}$,$w^{[i]}$ bring ：
    • ${\sum }_i{w}^{[i]}=1$
    • $\mu ={\sum }_i{w}^{[i]}{\chi }^{[i]}$
    • $\sum ={\sum }_i{w}^{[i]}({\chi }^{[i]}-\mu )({\chi }^{[i]}-\mu {)}^T$
  • There is no single solution
  • How to choose Sigma spot
    • first Sigma The point is also the mean ${\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
  • Square root of matrix
    • Definition $S$,$\sum =SS$
    • By diagonalization ：
      • $\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}$
      • Therefore, we can define ： $S=V{D}^{1/2}{V}^{-1}$
    • Cholesky Matrix Square root method
      • Alternative definition of square root of matrix ：$L,\sum =L{L}^T$
      • $L,\sum$ Have the same eigenvector
      • Sigma The point can, but does not have to be located in $\sum$
  • How to set the weight
    • $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

  • Unscented Kalman as an alternative to linearization
  • UT Is a better approximation than Taylor's expansion
  • UT Use sigma Point propagation
  • UT Free parameters in
  • UKF Use... In the prediction and correction steps UT

• UKF VS EKF

  • The results of the linear model are consistent with EKF identical
  • Nonlinear model ratio EKF A better approximation
  • The difference is usually “ It's a little small ”
  • UKF You don't need a Jacobian determinant
  • The same complexity class
  • Than EKF Slightly slower
  • Still limited by Gaussian distribution