Unscented カルマンフィルター
出典: フリー百科事典『ウィキペディア（Wikipedia）』 (2022/02/12 16:55 UTC 版)
「カルマンフィルター」の記事における「Unscented カルマンフィルター」の解説
非線形性の強いとき、拡張カルマンフィルターの性能は悪い。理由は平均値だけが非線形性に反映されるからである。unscented カルマンフィルターは、シグマ点とよばれる代表点を平均値の回りで用いて、推定値の共分散を計算する。こうすることにより、真の平均と共分散により近い値が得られることがモンテカルロ法や、テイラー展開によって示される。しかも解析的にヤコビアンを計算する必要がなくなるという利点がある。これは複雑なモデルでは有利である。 予測 拡張カルマンフィルターと同様、 unscented カルマンフィルターの予測手続きは更新手続きと別であり、更新手続きに線形カルマンフィルターや拡張カルマンフィルターを用いたり、その逆を行うことも可能である。推定値と共分散には、予測ノイズの平均と共分散項が加えられる。 x k − 1 | k − 1 a = [ x ^ k − 1 | k − 1 T E ( w k T ) ] T {\displaystyle {\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 = [ P k − 1 | k − 1 0 0 Q k ] {\displaystyle P_{k-1|k-1}^{a}={\begin{bmatrix}&P_{k-1|k-1}&&0&\\&0&&Q_{k}&\end{bmatrix}}} シグマ点 2L+1 個は、付け加えた項から計算される。ここに L は付け加えた状態項の次元である。 χ k − 1 | k − 1 0 {\displaystyle \chi _{k-1|k-1}^{0}} = x k − 1 | k − 1 a {\displaystyle ={\boldsymbol {x}}_{k-1|k-1}^{a}} χ k − 1 | k − 1 i {\displaystyle \chi _{k-1|k-1}^{i}} = x k − 1 | k − 1 a + ( ( L + λ ) P k − 1 | k − 1 a ) i {\displaystyle ={\boldsymbol {x}}_{k-1|k-1}^{a}+\left({\sqrt {(L+\lambda )P_{k-1|k-1}^{a}}}\right)_{i}} i = 1 , … L {\displaystyle i=1,\ldots L\,\!} χ k − 1 | k − 1 i {\displaystyle \chi _{k-1|k-1}^{i}} = x k − 1 | k − 1 a − ( ( L + λ ) P k − 1 | k − 1 a ) i − L {\displaystyle ={\boldsymbol {x}}_{k-1|k-1}^{a}-\left({\sqrt {(L+\lambda )P_{k-1|k-1}^{a}}}\right)_{i-L}} i = L + 1 , … 2 L {\displaystyle i=L+1,\dots {}2L\,\!} シグマ点は関数 f で時間発展する。 χ k | k − 1 i = f ( χ k − 1 | k − 1 i ) i = 0..2 L {\displaystyle \chi _{k|k-1}^{i}=f(\chi _{k-1|k-1}^{i})\quad i=0..2L} 予測値と共分散は重み付き平均で求められる。 x ^ k | k − 1 = ∑ i = 0 2 L W s i χ k | k − 1 i {\displaystyle {\hat {\boldsymbol {x}}}_{k|k-1}=\sum _{i=0}^{2L}W_{s}^{i}\chi _{k|k-1}^{i}} P k | k − 1 = ∑ i = 0 2 L W c i [ χ k | k − 1 i − x ^ k | k − 1 ] [ χ k | k − 1 i − x ^ k | k − 1 ] T {\displaystyle 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 = λ L + λ {\displaystyle W_{s}^{0}={\frac {\lambda }{L+\lambda }}} W c 0 = λ L + λ + ( 1 − α 2 + β ) {\displaystyle W_{c}^{0}={\frac {\lambda }{L+\lambda }}+(1-\alpha ^{2}+\beta )} W s i = W c i = 1 2 ( L + λ ) {\displaystyle W_{s}^{i}=W_{c}^{i}={\frac {1}{2(L+\lambda )}}} λ = α 2 ( L + κ ) − L {\displaystyle \lambda =\alpha ^{2}(L+\kappa )-L\,\!} α = 10-3、β = 2 、κ = 0 といった値がよく用いられる。 更新 予測値と共分散には、上と同様に観測値のノイズの平均と共分散項が加えられる。 x k | k − 1 a = [ x ^ k | k − 1 T E ( v k T ) ] T {\displaystyle {\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 = [ P k | k − 1 0 0 R k ] {\displaystyle P_{k|k-1}^{a}={\begin{bmatrix}&P_{k|k-1}&&0&\\&0&&R_{k}&\end{bmatrix}}} シグマ点 2L+1 個は、付け加えた項から計算される。ここに L は付け加えた状態項の次元である。 χ k | k − 1 0 {\displaystyle \chi _{k|k-1}^{0}} = x k | k − 1 a {\displaystyle ={\boldsymbol {x}}_{k|k-1}^{a}} χ k | k − 1 i {\displaystyle \chi _{k|k-1}^{i}} = x k | k − 1 a + ( ( L + λ ) P k | k − 1 a ) i {\displaystyle ={\boldsymbol {x}}_{k|k-1}^{a}+\left({\sqrt {(L+\lambda )P_{k|k-1}^{a}}}\right)_{i}} i = 1.. L {\displaystyle i=1..L\,\!} χ k | k − 1 i {\displaystyle \chi _{k|k-1}^{i}} = x k | k − 1 a − ( ( L + λ ) P k | k − 1 a ) i − L {\displaystyle ={\boldsymbol {x}}_{k|k-1}^{a}-\left({\sqrt {(L+\lambda )P_{k|k-1}^{a}}}\right)_{i-L}} i = L + 1 , … 2 L {\displaystyle i=L+1,\dots {}2L\,\!} もし、予測手続きも unscented カルマンフィルターで行われていたならば、以下のような変形も可能である。 χ k | k − 1 := [ χ k | k − 1 E ( v k T ) ] T ± ( L + λ ) R k a {\displaystyle \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 = [ 0 0 0 R k ] {\displaystyle R_{k}^{a}={\begin{bmatrix}&0&&0&\\&0&&R_{k}&\end{bmatrix}}} である。シグマ点は関数 h で観測値に変換される。 γ k i = h ( χ k | k − 1 i ) i = 0..2 L {\displaystyle \gamma _{k}^{i}=h(\chi _{k|k-1}^{i})\quad i=0..2L} 重み付き平均で、観測値とその共分散を推定する。 z ^ k = ∑ i = 0 2 L W s i γ k i {\displaystyle {\hat {\boldsymbol {z}}}_{k}=\sum _{i=0}^{2L}W_{s}^{i}\gamma _{k}^{i}} P z k z k = ∑ i = 0 2 L W c i [ γ k i − z ^ k ] [ γ k i − z ^ k ] T {\displaystyle 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 = ∑ i = 0 2 L W c i [ χ k | k − 1 i − x ^ k | k − 1 ] [ γ k i − z ^ k ] T {\displaystyle 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 {\displaystyle K_{k}=P_{x_{k}z_{k}}P_{z_{k}z_{k}}^{-1}} を計算する。以下は線形の場合と同様である。 x ^ k | k = x ^ k | k − 1 + K k ( z k − z ^ k ) {\displaystyle {\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 T {\displaystyle P_{k|k}=P_{k|k-1}-K_{k}P_{z_{k}z_{k}}K_{k}^{\textrm {T}}}
※この「Unscented カルマンフィルター」の解説は、「カルマンフィルター」の解説の一部です。
「Unscented カルマンフィルター」を含む「カルマンフィルター」の記事については、「カルマンフィルター」の概要を参照ください。
- Unscented カルマンフィルターのページへのリンク
