UKF
UKF
- KF 系列求解:
Kalman filter
需要线性模型EKF
通过泰勒展开线性化- 更好的方式线性化 ->
Unscented Transform
->UKF
- 计算一组(所谓的)sigma 点
- 从变换和加权的 sigma 点计算高斯
Unscented Transform
- 计算一系列的 Sigma 点
- 每个Sigma点有一个权重
- 通过非线性函数转换 Sigma 点
- 权重点计算高斯
Sigma and weight
Sigma 点
- 选择
χ
[
i
]
{\chi^{[i]}}
χ[i],
w
[
i
]
{w^{[i]}}
w[i] 使得:
- ∑ i w [ i ] = 1 {\sum_i w^{[i]} = 1} ∑iw[i]=1
- μ = ∑ i w [ i ] χ [ i ] { \mu = \sum_i w^{[i]}\chi^{[i]}} μ=∑iw[i]χ[i]
- ∑ = ∑ i w [ i ] ( χ [ i ] − μ ) ( χ [ i ] − μ ) T {\sum = \sum_i w^{[i]}(\chi^{[i]}-\mu)(\chi^{[i]}-\mu)^T} ∑=∑iw[i](χ[i]−μ)(χ[i]−μ)T
- 没有唯一的解决方案
- 如何选择Sigma点
- 第一个Sigma点也是均值 χ [ 0 ] = μ {\chi^[0] = \mu} χ[0]=μ
- χ [ i ] = μ + ( ( n + λ ) ∑ ) i {\chi^[i] = \mu + (\sqrt{(n+\lambda)\sum})_i} χ[i]=μ+((n+λ)∑)i for i=1,…,n
- χ [ i ] = μ − ( ( n + λ ) ∑ ) i − n {\chi^[i] = \mu - (\sqrt{(n+\lambda)\sum})_{i-n}} χ[i]=μ−((n+λ)∑)i−n for i=1+n,…,2n
- 矩阵平方根
- 定义 S S S, ∑ = S S \sum=SS ∑=SS
- 通过对角化计算:
- ∑ = V D V − 1 = ( d 11 ⋯ 0 ⋮ ⋱ ⋮ 0 ⋯ d n n ) = V ( d 11 ⋯ 0 ⋮ ⋱ ⋮ 0 ⋯ d n n ) ( d 11 ⋯ 0 ⋮ ⋱ ⋮ 0 ⋯ d n n ) V − 1 {\sum=VDV^{-1}= \begin{pmatrix} d_{11} & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & d_{nn} \end{pmatrix} = V\begin{pmatrix} \sqrt{d_{11}} & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & \sqrt{d_{nn}} \end{pmatrix} \begin{pmatrix} \sqrt{d_{11}} & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & \sqrt{d_{nn}} \end{pmatrix} V^{-1}} ∑=VDV−1=⎝⎜⎛d11⋮0⋯⋱⋯0⋮dnn⎠⎟⎞=V⎝⎜⎛d11⋮0⋯⋱⋯0⋮dnn⎠⎟⎞⎝⎜⎛d11⋮0⋯⋱⋯0⋮dnn⎠⎟⎞V−1
- 因此可以定义: S = V D 1 / 2 V − 1 {S=VD^{1/2}V^{-1}} S=VD1/2V−1
Cholesky Matrix
平方根法- 矩阵平方根的替代定义: L , ∑ = L L T {L, \sum=LL^T} L,∑=LLT
- L , ∑ {L,\sum} L,∑ 有相同的特征向量
- Sigma 点可以但不必位于 ∑ {\sum} ∑
- 如何设置权重
- w m [ 0 ] = λ n + λ {w_m^{[0]}=\frac{\lambda}{n+\lambda}} wm[0]=n+λλ
- w c [ 0 ] = w m [ 0 ] + ( 1 − α 2 + β ) {w_c^{[0]}=w_m^{[0]}+(1-\alpha^2+\beta)} wc[0]=wm[0]+(1−α2+β)
- w c [ i ] = w m [ i ] + 1 2 ( n + λ ) {w_c^{[i]}=w_m^{[i]}+\frac{1}{2(n+\lambda)}} wc[i]=wm[i]+2(n+λ)1 for i=1,…,2n
- 选择
χ
[
i
]
{\chi^{[i]}}
χ[i],
w
[
i
]
{w^{[i]}}
w[i] 使得:
UKF Algorithm
-
Prediction
-
χ t − 1 = ( μ t − 1 , μ t − 1 + ( n + λ ) ∑ t − 1 , μ t − 1 − ( n + λ ) ∑ t − 1 ) {\chi_{t-1}=(\mu_{t-1},\ \ \mu_{t-1}+\sqrt{(n+\lambda)\sum_{t-1}},\ \ \mu_{t-1}-\sqrt{(n+\lambda)\sum_{t-1}})} χt−1=(μt−1, μt−1+(n+λ)∑t−1, μt−1−(n+λ)∑t−1)
-
χ ˉ t ∗ = g ( u t , χ t − 1 ) {\bar{\chi}_t^* = g(u_t,\chi_{t-1})} χˉt∗=g(ut,χt−1)
-
μ t ˉ = ∑ i = 0 2 n w m [ i ] χ ˉ t ∗ [ i ] {\bar{\mu_t}=\sum_{i=0}^{2n}w_m^{[i]}\bar{\chi}_t^{*[i]}} μtˉ=∑i=02nwm[i]χˉt∗[i]
-
Σ ˉ t = ∑ i = 0 2 n w c [ i ] ( χ ˉ t ∗ [ i ] − μ t ˉ ) ( χ ˉ t ∗ [ i ] − μ t ˉ ) T + R t {\bar{\Sigma}_t=\sum_{i=0}^{2n}w_c^{[i]}(\bar{\chi}_t^{*[i]}-\bar{\mu_t})(\bar{\chi}_t^{*[i]}-\bar{\mu_t})^T+R_t} Σˉt=∑i=02nwc[i](χˉt∗[i]−μtˉ)(χˉt∗[i]−μtˉ)T+Rt
-
-
Correction
- χ t ˉ = ( μ t ˉ , μ t ˉ + ( n + λ ) ∑ t − 1 , μ t ˉ − ( n + λ ) ∑ t − 1 ) {\bar{\chi_{t} }=(\bar{\mu_{t}},\ \ \bar{\mu_{t}}+\sqrt{(n+\lambda)\sum_{t-1}},\ \ \bar{\mu_{t}}-\sqrt{(n+\lambda)\sum_{t-1}})} χtˉ=(μtˉ, μtˉ+(n+λ)∑t−1, μtˉ−(n+λ)∑t−1)
- z t ˉ = h ( χ t ˉ ) {\bar{z_t}=h(\bar{\chi_t})} ztˉ=h(χtˉ)
- z t ^ = ∑ i = 0 2 n w m [ i ] z ˉ t [ i ] {\hat{z_t}=\sum_{i=0}^{2n}w_m^{[i]}\bar{z}_t^{[i]}} zt^=∑i=02nwm[i]zˉt[i]
- S t = ∑ i = 0 2 n w c [ i ] ( χ ˉ t [ i ] − μ ˉ t ) ( χ ˉ t [ i ] − μ ˉ t ) T {S_t=\sum_{i=0}^{2n}w_c^{[i]}(\bar{\chi}_t^{[i]}-\bar{\mu}_t)(\bar{\chi}_t^{[i]}-\bar{\mu}_t)^T} St=∑i=02nwc[i](χˉt[i]−μˉt)(χˉt[i]−μˉt)T
- K t = ∑ ˉ t x , z S t − 1 {K_t = \bar{\sum}_t^{x,z}S_t^{-1}} Kt=∑ˉtx,zSt−1
- ∑ t = ∑ ˉ t − K t S t K t T {\sum_t =\bar{\sum}_t - K_tS_tK_t^T} ∑t=∑ˉt−KtStKtT
UT/UKF/EKF Summary
-
UT/UKF
- 无迹卡尔曼作为线性化的替代方案
- UT 是比泰勒展开更好的近似值
- UT 使用 sigma 点传播
- UT中的自由参数
- UKF 在预测和校正步骤中使用 UT
-
UKF VS EKF
- 线性模型的结果与 EKF 相同
- 非线性模型比 EKF 更好的近似
- 差异通常“有点小”
- UKF 不需要雅可比行列式
- 相同的复杂度类
- 比 EKF 稍慢
- 仍然受限于高斯分布