カルマンフィルター
重みは以下のように与えられる。
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、β = 2 、κ = 0 といった値がよく用いられる。
更新
予測値と共分散には、上と同様に観測値のノイズの平均と共分散項が加えられる。
ここでは時間発展モデル
x k = f ( x k − 1 , u k , w k ) {\displaystyle {\boldsymbol {x}}_{k}=f({\boldsymbol {x}}_{k-1},{\boldsymbol {u}}_{k},{\boldsymbol {w}}_{k})}
と、観測モデル
z k = h ( x k , v k ) {\displaystyle {\boldsymbol {z}}_{k}=h({\boldsymbol {x}}_{k},{\boldsymbol {v}}_{k})}
を考える。どちらも微分可能であれば線形である必要はない。関数 f は前の状態から推定値を与え、関数 h は観測値を与える。どちらの関数も直接共分散を求めることはできず、偏微分行列(ヤコビアン)を用いる必要がある。
原理としては、非線形モデルを現在の推定値の回りで線形化する。そのためにそれぞれの時刻で、ヤコビアンを計算する。すなわち、
予測
x ^ k | k − 1 = f ( x ^ k − 1 | k − 1 , u k , 0 ) {\displaystyle {\hat {\boldsymbol {x}}}_{k|k-1}=f({\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k},0)}
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}}}
推定値と観測値の相関行列