タグ別アーカイブ: カルマンフィルター

アンサンブルカルマンフィルターのメモ

最終更新日: 2013年11月03日

アンサンブルカルマンフィルターについて少し整理します。

アンサンブルカルマンフィルターのアルゴリズムは、カルマンフィルターのアルゴリズムを元に作成されています。そのため、まず、カルマンフィルターのアルゴリズムを復習して、それからアンサンブルカルマンフィルターのアルゴリズムを説明します。

カルマンフィルターのまとめ

カルマンフィルターは、観測を伴う線形の動的システムにおいて、観測値を利用しながらシステムの状態を時々刻々推定していくアルゴリズムで、推定値と推定誤差の共分散行列の動的な変化を計算していきます。

\(n\) 次元の確率変数ベクトルの系列 {\(x_0\), \(x_1\), …} が次式(状態方程式)によって定められるとします。

\[
x_{k+1} = A_k x_k + v_k, \quad k = 0, 1, …
\tag{1.1}
\]

ただし、\(A_k\) は \(n\)×\(n\) 次の非確率行列で、\(v_k\) は外乱を表す確率変数ベクトルです。\(v_k\) の平均値は 0 ベクトルであり、共分散行列 \(Q_k\) は既知であるとします。

\(x_k\) に対して、

\[
y_k = H_k x_k + w_k, \quad k = 0, 1, …
\tag{1.2}
\]

となる\(m\)次ベクトル \(y_k\) が時刻 \(k\) に得られるとします。
この式は観測方程式と呼ばれます。ただし、
\(H_k\) は \(m\)×\(n\) 次の非確率行列、
\(w_k\) は観測ノイズを表す\(m\)次の確率変数ベクトル
です。\(w_k\) の平均値は 0 ベクトルであり、
その共分散行列 \(R_k\) は正則で既知であるとします。

\(x_0\) の平均値 \(\bar{x}_0\)、共分散行列 \(P_0\) は既知であるとします。

以上の設定のもとで、時刻 \(k\) までの観測ベクトル

\[
Y_k = (y_0, y_1, …, y_k)^{\rm T}
\]

が与えられたときの \(x_k\), \(x_{k+1}\) の線形最小二乗推定 \(\hat{x}_{k|k}\), \(\hat{x}_{k+1|k}\) を求める問題を考えます。

\(x_k\), \(x_{k+1}\) の \(\hat{x}_{k|k}\), \(\hat{x}_{k+1|k}\) による推定誤差の共分散行列を \(P_{k|k}\), \(P_{k+1|k}\) と表すことにします。

逐次的に考えます。まず、\(\hat{x}_{k|k-1}\), \(P_{k|k-1}\) が得られていると仮定します。\(k\) = 0 のときは、

\[
\hat{x}_{0|-1} = \bar{x}_0, \quad P_{0|-1} = P_0
\]

と置けばいいので、この仮定は満たされています。

時刻 \(k\) に観測値 \(y_k\) が得られると、

\[
K_k = P_{k|k-1}H_k{}^{\rm T}(H_k P_{k|k-1} H_k{}^{\rm T} + R_k)^{-1} \tag{1.3}
\]

を計算して、

\[
\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (y_k – H_k \hat{x}_{k|k-1}) \tag{1.4}
\]
によって、観測値 \(y_k\) が得られた後の \(x_k\) の線形最小分散推定値が計算されます。

(1.3)式で定められる行列 \(K_k\) はカルマンゲインと呼ばれます。カルマンゲインを求めるために逆行列を計算する必要がありますが、\(m=1\) のときは、\((H_k P_{k|k-1} H_k{}^{\rm T} + R_k)^{-1}\) は単なる逆数です。

(1.4)式によって \(x_k\) を推定したときの推定誤差の共分散行列 \(P_{k|k}\) は、

\[\begin{eqnarray}
P_{k|k} &=& (I-K_k H_k) P_{k|k-1} (I-K_k H_k)^{\rm T} + K_k R_k K_k{}^{\rm T} \tag{1.5}\\
&=& (I-K_k H_k) P_{k|k-1} \tag{1.6}
\end{eqnarray}\]

によって計算できます。(1.5)式は、(1.4)式右辺の \(y_k\) に (1.2)式を代入し、

\[
P_{k|k} = E[(x_k – \hat{x}_{k|k})(x_k – \hat{x}_{k|k})^{\rm T}]
\]

の定義に従って計算して、\(w_k\) が時刻 \(k\) より前の確率変数系列と無相関であるという仮定から導かれます。

\(\hat{x}_{k|k}\), \(P_{k|k}\) が得られたら、予測値 \(\hat{x}_{k+1|k}\)、\(P_{k+1|k}\) は、(1.1)式に対応して、

\[\begin{eqnarray}
\hat{x}_{k+1|k} &=& A_k \hat{x}_{k|k} \tag{1.7}\\
P_{k+1|k} &=& A_{k+1} P_{k|k} A_{k+1}{}^{\rm T} + Q_k \tag{1.8}
\end{eqnarray}
\]

によって計算されます。

アンサンブルカルマンフィルターのアルゴリズム

アンサンブルカルマンフィルターのアルゴリズムは、「アンサンブルカルマンフィルタ – Wikipedia」 に要約されています。「アンサンブルカルマンフィルタ – Wikipedia」 では、フィルタリングだけでなく、スムージングの方法も記載されています。(「アンサンブルカルマンフィルタ – Wikipedia」 を 2013年10月27日に参照しました。)
このページを参考にして、アンサンブルカルマンフィルターのアルゴリズムを以下に要約します。

時刻 \(k\) における状態方程式を
\[
x_k = f_k (x_{k-1}, v_k) \tag{2.1}
\]
とします。\(v_k\) は状態遷移のときの外乱を表しています。

時刻 \(k\) における観測は、
\[
y_k = H_k x_k + w_k \tag{2.2}
\]
によって得られるとします。\(w_k\) は観測ノイズを表していて、平均値は、0 で、その共分散行列 \(R_k\) は既知であるとします。

時刻 \(k-1\) にそれまでの観測値を使って、状態量 \(x_{k-1}\) を推定する \(N\) 個のアンサンブル\(\{x_{k-1|k-1}^{(i)}\}_{i=1}^N\) が与えられているとします。

まず予測。アンサンブルメンバー \(x_{k-1|k-1}^{(i)}\) を状態方程式(2.1)式によって更新し、予測分布のアンサンブルを得ます。すなわち、
\[
x_{k|k-1}^{(i)} = f_k (x_{k-1|k-1}^{(i)}, v_k^{(i)}) \tag{2.3}
\]
によって、予測分布のアンサンブル\(\{x_{k|k-1}^{(i)}\}_{i=1}^N\) を求めます。\(v_{k}^{(i)}\) を得るために乱数を発生する必要があります。予測値は、
\[
\hat{x}_{k|k-1} = \frac{1}{N}\sum_{i=1}^N x_{k|k-1}^{(i)} \tag{2.4}
\]
として求めます。

つぎにフィルタリング。まず、
\[\begin{eqnarray}
x^{\prime}{}_{k|k-1}^{(i)} = x_{k|k-1}^{(i)} – \hat{x}_{k|k-1} \tag{2.5} \\
\hat{P}_{k|k-1} = \frac{1}{N-1} \sum_{i=1}^N x^{\prime}{}_{k|k-1}^{(i)} x^{\prime}{}_{k|k-1}^{(i)}{}^{\rm T} \tag{2.6} \end{eqnarray}\]
として、共分散行列 \(P_{k|k-1}\) の推定値 \(\hat{P}_{k|k-1}\) を求めます。

次に、観測方程式の中のノイズ \(w_k\) に対して、\(w_k^{(i)}\), \(i=1, …, N\) を発生させて、
\[\begin{eqnarray}
\bar{w}_k &=& \frac{1}{N} \sum_{i=1}^N w_{k}^{(i)} \tag{2.8} \\
w^{\prime}{}_k^{(i)} &=& w_{k}^{(i)} – \bar{w}_k \tag{2.9} \\
\hat{R}_k &=& \frac{1}{N-1} \sum_{i=1}^N w^{\prime}{}_k^{(i)}w^{\prime}{}_k^{(i)}{}^{\rm T} \tag{2.10}
\end{eqnarray}\]
によって、\(\hat{R}_k\) を生成します。\(R_k\) は既知であるのに、このようにして、\(\hat{R}_k\) を求める理由については、つぎの「アンサンブルカルマンフィルターの解説」の中で述べます。

こうして求めた \(\hat{P}_{k|k-1}\), \(\hat{R}_k\) を用いて、カルマンゲインを
\[
\hat{K}_k = \hat{P}_{k|k-1}H_k^{\rm T}(H_k\hat{P}_{k|k-1}H_k^{\rm T} + \hat{R}_k)^{-1} \tag{2.11}
\]
によって求め、アンサンブルメンバーを次式で更新します。
\[
x_{k|k}^{(i)} = x_{k|k-1}^{(i)} + \hat{K}_k(y_k + w_k^{(i)} – H_k x_{k|k-1}^{(i)}) \tag{2.12}
\]
この式の右辺に \(w_k^{(i)}\) が含まれている点に注意してください。(1.8)式右辺と比べて、この \(w_k^{(i)}\) が含まれているところがアンサンブルカルマンフィルターの肝要な点です。なぜ、この項があるのかは、つぎの「アンサンブルカルマンフィルターの解説」で記述します。

アンサンブルカルマンフィルターの解説

状態方程式によるアンサンブルメンバーの更新

アンサンブルメンバーを (2.3)式によって更新するときは、状態の遷移を表現する式によって更新します。

カルマンフィルターでは状態方程式は (1.1)式のように線形の式が前提になっていますが、アンサンブルカルマンフィルターでは、\(x_{k-1}\) から \(x_k\) への推移を記述する (2.1)式の式形については特に指定されていません。\(f_k\) は、状態量の動的推移を一ステップだけ計算する電子計算機のプログラムを表していると考えても構いません。実際、アンサンブルカルマンフィルターが使われる場合、実現象をシミュレーションするプログラムがここで使われる場合が多いでしょう。

確率的外乱 \(v_k\) を模擬発生する必要があるので、乱数を発生させるテクニックが必要です。もちろん、モデルとしては、確率的外乱 \(v_k\) が無い形の状態方程式を考えることも可能です。

観測方程式中のノイズを発生させるのはなぜか

アンサンブルカルマンフィルターでは、観測方程式中の \(w_k\) を \(N\) 個発生させます。この値は、(2.10)式で \(\hat{R}_k\) を計算するのに使われるだけでなく、(2.11)式でアンサンブルメンバーを更新するときにも使われることに注意する必要があります。\(w_k^{(i)}\) を発生させるのは、むしろ、(2.12)式右辺で使うためです。

カルマンフィルターの (1.8)式では、この \(w_k^{(i)}\) に相当する項がありません。

(2.12)式に \(w_k^{(i)}\) がある理由を理解するために、
\[\begin{eqnarray}
\hat{x}_{k|k} &=& \frac{1}{N} \sum_{i=1}^N x_{k|k}^{(i)} \tag{3.1} \\
x^{\prime}{}_{k|k}^{(i)} &=& x_{k|k}^{(i)} – \hat{x}_{k|k} \tag{3.2} \\
\hat{P}_{k|k} &=& \frac{1}{N-1} \sum_{i=1}^N x^{\prime}{}_{k|k}^{(i)} x^{\prime}{}_{k|k}^{(i)}{}^{\rm T} \tag{3.3}
\end{eqnarray}\]
と置くことにして、\(\hat{P}_{k|k}\) を計算してみます。

(3.1)式右辺に、(2.12)式を代入して、
\[
\hat{x}_{k|k} = \hat{x}_{k|k-1} + \hat{K}_k (y_k + \bar{w}_k – H_k \hat{x}_{k|k-1})
\]
が得られるので、
\[\begin{eqnarray}
x^{\prime}{}_{k|k}^{(i)} &=& x^{\prime}{}_{k|k-1}^{(i)} + \hat{K}_k (w^{\prime}{}_k^{(i)} – H_k x^{\prime}{}_{k|k-1}^{(i)}) \\
&=& (I – \hat{K}_k H_k) x^{\prime}{}_{k|k-1}^{(i)} + \hat{K}_k w^{\prime}{}_k^{(i)} \tag{3.4}
\end{eqnarray}\]
が得られます。これを (3.3)式右辺に代入して、
\[\begin{eqnarray}
\hat{P}_{k|k} &=& \frac{1}{N-1}\sum_{i=1}^N \Bigl((I – \hat{K}_k H_k) x^{\prime}{}_{k|k-1}^{(i)} + \hat{K}_k w^{\prime}{}_k^{(i)}\Bigr) \\
&\quad& \quad\quad \times \Bigl((I – \hat{K}_k H_k) x^{\prime}{}_{k|k-1}^{(i)} + \hat{K}_k w^{\prime}{}_k^{(i)}\Bigr)^{\rm T} \\
&=& (I-\hat{K}_k H_k)\hat{P}_{k|k-1}(I-\hat{K}_k H_k)^{\rm T} + \\
&\quad& \quad + \hat{K}_k \hat{R}_k \hat{K}_k^{\rm T} \\
&\quad& \quad + (I-K_k H_k) \Bigl(\frac{1}{N-1}\sum_{i=1}^N x^{\prime}{}_{k|k-1}^{(i)}w^{\prime}{}_k^{(i)}{}^{\rm T}\Bigr)\hat{K}_k^{\rm T} \\
&\quad& \quad + \hat{K}_k\Bigl( \frac{1}{N-1} \sum_{i=1}^{N} w^{\prime}{}_k^{(i)}x^{\prime}{}_{k|k-1}^{(i)}{}^{\rm T}\Bigr)(I-\hat{K}_k H_k)^{\rm T} \tag{3.5}
\end{eqnarray}\]

観測ノイズ \(w_k\) は \(x_k\) とは独立なので、(3.5)式最終辺の第3項、第4項は \(N\) が大きいと 0 に収束します。よって、
\[
\hat{P}_{k|k} \simeq (I-\hat{K}_k H_k)\hat{P}_{k|k-1}(I-\hat{K}_k H_k)^{\rm T} + \hat{K}_k \hat{R}_k \hat{K}_k^{\rm T} \tag{3.6}
\]
が得られます。これで、カルマンフィルターのフィルタリングの後の共分散行列の更新の式 (1.5) に対応する式になりました。

(3.6)式の右辺の第2項は、(2.12)式右辺に \(w_k^{(i)}\) を入れたために出て来たことに注意してください。

アンサンブルカルマンフィルターの最初の提案では、(2.12)式右辺の \(w_k^{(i)}\) が考慮されなかったようです。

観測値がスカラーのときのカルマンゲインの計算

観測値がスカラーのときのカルマンゲインの計算は簡単です。カルマンゲインの計算式の右辺では、\(\hat{P}_{k|k-1}\) を用いて計算するように見えますが、一旦 \(\hat{P}_{k|k-1}\) を計算してから、カルマンゲインを計算するのではなくて、つぎのようにします。

まず、
\[\begin{eqnarray}
a_i &=& H_k x^{\prime}{}_{k|k-1}^{(i)}, \quad i = 1,…, N \\[6pt]
b &=& H_k \hat{P}_{k|k-1} H_k{}^{\rm T} + \hat{R}_k \\
&=& H_k \frac{1}{N-1} \sum_{i=1} ^N x^{\prime}{}_{k|k-1}^{(i)} x^{\prime}{}_{k|k-1}^{(i)}{}^{\rm T} H_k^{\rm T} + \hat{R}_k \\
&=& \frac{1}{N-1}\sum_{i=1}^N \Bigl(H_k x^{\prime}{}_{k|k-1}^{(i)}\Bigr)\Bigl(H_k x^{\prime}{}_{k|k-1}^{(i)}\Bigr)^{\rm T} + \hat{R}_k \\
&=& \frac{1}{N-1}\sum_{i=1}^N (a_i)^2 + \hat{R}_k
\end{eqnarray}\]
と置きます。\(a_i\) はスカラーです。
\[\begin{eqnarray}
\hat{P}_{k|k-1} H_k^{\rm T} &=& \Bigl(\frac{1}{N-1}\sum_{i=1}^N x^{\prime}{}_{k|k-1}^{(i)} x^{\prime}{}_{k|k-1}^{(i)}{}^{\rm T}\Bigr)\; H_k^{\rm T} \\
&=& \frac{1}{N-1}\sum_{i=1}^N x^{\prime}{}_{k|k-1}^{(i)} \Bigl(H_k x^{\prime}{}_{k|k-1}^{(i)}\Bigr)^T \\
&=& \frac{1}{N-1} \sum_{i=1}^N a_i x^{\prime}{}_{k|k-1}^{(i)}
\end{eqnarray}\]
なので、カルマンゲイン \(\hat{K}_k\) (\(n\)次列ベクトル) は、

\[
\hat{K}_k = \hat{P}_{k|k-1}H_k^{\rm T}(H_k\hat{P}_{k|k-1}H_k^{\rm T} + \hat{R}_k)^{-1} = \frac{1}{(N-1)b} \sum_{i=1}^N a_i x^{\prime}{}_{k|k-1}^{(i)}
\]

として求められます。共分散行列 \(\hat{P}_{k|k-1}\) を計算する必要はありません。

状態量が属する制約領域があるときの処理について

状態量が物理量である場合は、その値は非負であるべきであるとか、ダムの貯水量である場合は、ある範囲に含まれているべきであるとか、状態量が意味があるために、ある制約領域に含まれていなければいけない場合があります。

状態量の時間更新の式 (2.3) 式では、当然、状態量が制約領域に含まれるように計算されるでしょう。問題は、フィルタリングの更新式 (2.12)式です。

実際に適用するときには、(2.12) 式であらたに得られたアンサンブルメンバーは制約領域の外に出るかもしれません。あらたに得られたアンサンブルメンバーの適合性を検査して、非負であるべきなのに負の値になったりしたら、0 に置き換えるなどの処理を個別にすることになるでしょう。

参考文献