カルマンフィルター
非線形カルマンフィルター > Unscented カルマンフィルター

非線形性の強いとき、拡張カルマンフィルターの性能は悪い。理由は平均値だけが非線形性に反映されるからである。unscented カルマンフィルターは、シグマ点とよばれる代表点を平均値の回りで用いて、推定値の共分散を計算する。こうすることにより、真の平均と共分散により近い値が得られることがモンテカルロ法や、テイラー展開によって示される。しかも解析的にヤコビアンを計算する必要がなくなるという利点がある。これは複雑なモデルでは有利である。
予測
拡張カルマンフィルターと同様、 unscented カルマンフィルターの予測手続きは更新手続きと別であり、更新手続きに線形カルマンフィルターや拡張カルマンフィルターを用いたり、その逆を行うことも可能である。推定値と共分散には、予測ノイズの平均と共分散項が加えられる。
{\boldsymbol {x}}_{k-1|k-1}^{a}=[{\hat {\boldsymbol {x}}}_{k-1|k-1}^{\textrm {T}}\quad \mathrm {E} ({\boldsymbol {w}}_{k}^{\textrm {T}})\ ]^{\textrm {T}}
P_{k-1|k-1}^{a}={\begin{bmatrix}&P_{k-1|k-1}&&0&\\&0&&Q_{k}&\end{bmatrix}}
シグマ点 2L+1 個は、付け加えた項から計算される。ここに L は付け加えた状態項の次元である。
| 0 | 1 | 2 |
| --- | --- | --- |
|  \chi _{k-1|k-1}^{0} |  ={\boldsymbol {x}}_{k-1|k-1}^{a} |  |
|  \chi _{k-1|k-1}^{i} |  ={\boldsymbol {x}}_{k-1|k-1}^{a}+\left({\sqrt {(L+\lambda )P_{k-1|k-1}^{a}}}\right)_{i} |  i=1,\ldots L\,\! |
|  \chi _{k-1|k-1}^{i} |  ={\boldsymbol {x}}_{k-1|k-1}^{a}-\left({\sqrt {(L+\lambda )P_{k-1|k-1}^{a}}}\right)_{i-L} |  i=L+1,\dots {}2L\,\! |
シグマ点は関数 f で時間発展する。
\chi _{k|k-1}^{i}=f(\chi _{k-1|k-1}^{i})\quad i=0..2L
予測値と共分散は重み付き平均で求められる。
{\hat {\boldsymbol {x}}}_{k|k-1}=\sum _{i=0}^{2L}W_{s}^{i}\chi _{k|k-1}^{i}
P_{k|k-1}=\sum _{i=0}^{2L}W_{c}^{i}\ [\chi _{k|k-1}^{i}-{\hat {\boldsymbol {x}}}_{k|k-1}][\chi _{k|k-1}^{i}-{\hat {\boldsymbol {x}}}_{k|k-1}]^{\textrm {T}}
重みは以下のように与えられる。
W_{s}^{0}={\frac {\lambda }{L+\lambda }}

 W_{c}^{0}={\frac {\lambda }{L+\lambda }}+(1-\alpha ^{2}+\beta )

 W_{s}^{i}=W_{c}^{i}={\frac {1}{2(L+\lambda )}}

 \lambda =\alpha ^{2}(L+\kappa )-L\,\!
α = 10-3、β = 2 、κ = 0 といった値がよく用いられる。
更新
予測値と共分散には、上と同様に観測値のノイズの平均と共分散項が加えられる。
{\boldsymbol {x}}_{k|k-1}^{a}=[{\hat {\boldsymbol {x}}}_{k|k-1}^{\textrm {T}}\quad \mathrm {E} ({\boldsymbol {v}}_{k}^{\textrm {T}})\ ]^{\textrm {T}}
P_{k|k-1}^{a}={\begin{bmatrix}&P_{k|k-1}&&0&\\&0&&R_{k}&\end{bmatrix}}
シグマ点 2L+1 個は、付け加えた項から計算される。ここに L は付け加えた状態項の次元である。
| 0 | 1 | 2 |
| --- | --- | --- |
|  \chi _{k|k-1}^{0} |  ={\boldsymbol {x}}_{k|k-1}^{a} |  |
|  \chi _{k|k-1}^{i} |  ={\boldsymbol {x}}_{k|k-1}^{a}+\left({\sqrt {(L+\lambda )P_{k|k-1}^{a}}}\right)_{i} |  i=1..L\,\! |
|  \chi _{k|k-1}^{i} |  ={\boldsymbol {x}}_{k|k-1}^{a}-\left({\sqrt {(L+\lambda )P_{k|k-1}^{a}}}\right)_{i-L} |  i=L+1,\dots {}2L\,\! |
もし、予測手続きも unscented カルマンフィルターで行われていたならば、以下のような変形も可能である。
\chi _{k|k-1}:=[\chi _{k|k-1}\quad \mathrm {E} ({\boldsymbol {v}}_{k}^{\textrm {T}})\ ]^{\textrm {T}}\pm {\sqrt {(L+\lambda )R_{k}^{a}}}
ここに、
R_{k}^{a}={\begin{bmatrix}&0&&0&\\&0&&R_{k}&\end{bmatrix}}
である。シグマ点は関数 h で観測値に変換される。
\gamma _{k}^{i}=h(\chi _{k|k-1}^{i})\quad i=0..2L
重み付き平均で、観測値とその共分散を推定する。
{\hat {\boldsymbol {z}}}_{k}=\sum _{i=0}^{2L}W_{s}^{i}\gamma _{k}^{i}
P_{z_{k}z_{k}}=\sum _{i=0}^{2L}W_{c}^{i}\ [\gamma _{k}^{i}-{\hat {\boldsymbol {z}}}_{k}][\gamma _{k}^{i}-{\hat {\boldsymbol {z}}}_{k}]^{\textrm {T}}
推定値と観測値の相関行列
P_{x_{k}z_{k}}=\sum _{i=0}^{2L}W_{c}^{i}\ [\chi _{k|k-1}^{i}-{\hat {\boldsymbol {x}}}_{k|k-1}][\gamma _{k}^{i}-{\hat {\boldsymbol {z}}}_{k}]^{\textrm {T}}
を用いて unscented カルマンゲイン
K_{k}=P_{x_{k}z_{k}}P_{z_{k}z_{k}}^{-1}
を計算する。以下は線形の場合と同様である。
{\hat {\boldsymbol {x}}}_{k|k}={\hat {\boldsymbol {x}}}_{k|k-1}+K_{k}({\boldsymbol {z}}_{k}-{\hat {\boldsymbol {z}}}_{k})
P_{k|k}=P_{k|k-1}-K_{k}P_{z_{k}z_{k}}K_{k}^{\textrm {T}}