カルマンフィルター(2) - 多次元の場合

カルマンフィルター 統計学

はじめに

　本エントリーでは、一般の多次元のカルマンフィルターについてまとめます。カルマンフィルターの概要や式の解釈については以前のエントリー「カルマンフィルター(1) - 1次元の場合」で投稿させて頂きましたので、こちらもご覧頂ければと思います。

　本稿では、多次元の場合の予測モデルと更新モデルについて式の導出を行います。また、実装上課題となる数値安定性を解決するために一般的に用いられているUD分解による定式化についても捕捉します。

カルマンフィルターのモデル

1次元の場合と同様に状態遷移モデルから予測モデルと更新モデルを記述します（図１）。

図１：状態遷移モデルのグラフ
状態量:$\bf{y_t}$、観測:$\bf{z_t}$および制御入力：観測:$\bf{u_t}$はそれぞれ$n$次元、$m$次元の$k$次元のベクトルとすると、図１の状態遷移モデルは以下で定式化できます。

$$ \begin{align} \bf{y_t} &= { \bf A_t y_{t-1} + B_t u_t+G_tw_t } \tag{1} \\\ \bf{z_t} &= { \bf H_t y_t+ v_t } \tag{2} \end{align} $$ $$ \begin{align} {\bf w_t} & \sim N(0,{ \bf Q}_t) \tag{3} \\\ {\bf v_t} & \sim N(0,{ \bf R}_t) \tag{4} \end{align} $$

ここで、${ \bf A_t } \in \mathbb{R}^{n \times n},{ \bf B_t } \in \mathbb{R}^{n \times k}$および${ \bf H_t } \in \mathbb{R}^{m \times n}$はそれぞれ予測モデルと更新モデルの線形変換を表す係数行列です。$\bf{G}_t \in \mathbb{R}^{n \times n}$はノイズのモデル式で${ \bf w_t } \in \mathbb{R}^{n}, { \bf v_t } \in \mathbb{R}^{m}$はそれぞれ式(3),(4)の通り分散共分散行列がそれぞれ$\bf{Q}_t$と$\bf{R}_t$の多正規分布に従うノイズです。また、$\bf{y_t},\bf{z_t},\bf{u_t}$と$\bf{w}_t, \bf{v}_t$および$\bf{w}_t$と$\bf{v}_t$同士はtによらず独立であるとします。

また、この後の計算の記述を簡潔にするために時刻:$t$において得られている情報：$I_t$を以下の様に定義します。

$$ I_t \equiv \{ { \bf z_1, z_2, z_3,..., z_t, u_1,u_2,...,u_t } \} \tag{5} $$

これらのモデルの式と情報：$I_t$を用いて予測モデルと更新モデルの式を導出していきます。実用上は、状態量がどの値であるとすべきかを示す：”平均”とその際にどれくらいの信頼度、つまりばらつきがあるのかを示す”分散”この2つが必要な値となります。なので、予測モデルと更新モデルからこの"平均"と”分散”についての式を導出します。

予測モデル

予測ステップは時刻t-1までに得た情報:$I_{t-1}$と制御入力:$\bf{u}_t$で予測された$\bf{y}_t$で、つまり$I_{t-1}$と$u_t$の実現値を得た場合の$\bf{y}_t$の条件付き確率となります。よって平均と分散はそれぞれ式(6),(7)で表すことができます。

$$ \begin{align} { \bf \hat{y}_{t|t-1}} &= E[{ \bf y_t}| I_{t-1},{ \bf u_t}] \tag{6} \\\ { \bf \Sigma_{t|t-1}} &= Var[{ \bf y_t}|I_{t-1},{ \bf u_t}] \tag{7} \end{align} $$

式(1)-(4)と、ノイズ$\bf{w}_t, \bf{v}_t$と各変数が$t$に依らず独立である事を利用し、式(6),(7)から平均と分散をそれぞれ計算します。

平均：

$$ \begin{align} {\bf \hat{y}}_{t|t-1}&=E[{\bf A_t y_{t-1}+ B_t u_t + G_t w_t }|I_{t-1},{\bf u}_t] \\\ &= E[{\bf A_t y_{t-1}}|I_{t-1}] + E[{\bf B_t u_t }|I_{t-1}]+ E[{\bf G_t w_t }|I_{t-1},{\bf u}_t] \\\ &= {\bf A_t }E[ {\bf y}_{t-1}|I_{t-1}] + {\bf B_t u_t } \\\ &= {\bf A_t }{\bf \hat{y}}_{t-1|t-1} + {\bf B_t u_t } \end{align} \tag{8} $$

分散：

$$ \begin{align} {\bf\Sigma_{t|t-1}}&=Var[{\bf A_t y_{t-1} + B_t u_t +G_tw_t } |I_{t-1},\bf{u_t}] \\\ &= Var[{ \bf A_t y_{t-1} }| I_{t-1} ] + Var[{ \bf B_t u_t} |I_{t-1}]+ Var[{\bf G_tw_t}|I_{t-1},\bf{u_t}] \\\ &= { \bf A_t } Var[{ \bf y_{t-1} }|I_{t-1}] { \bf A_t}^T + { \bf G_t Q_t {G_t}^T }\\\ &= { \bf A_t } { \bf \Sigma }_{t-1|t-1}{ \bf A_t}^T + { \bf G_t Q_t {G_t}^T } \end{align} \tag{9} $$

まとめると、平均と分散を求める予測モデルの式は式(10)になります。

$$ \begin{align} \bf{\hat{y}_{t|t-1}}&=A_t \bf{\hat{y}_{t-1|t-1}} + B_t \bf{u_t} \\\ \bf{\Sigma}_{t|t-1}&=A_t\bf{\Sigma}_{t-1|t-1}{A_t}^T + G_t Q_t {G_t}^T \end{align} \tag{10} $$

更新モデル

更新モデルでは、時刻$𝑡$に得られている情報の全て：$I_t$を使って更に精度を高めるべく、予測値：$y_{t|t−1}$の分布から更新値：$y_{t|t}$の平均と分散を求めます。

$$ \begin{align} { \bf \hat{y}}_{t|t}&=E[ { \bf y_t}|I_{t}] \tag{11} \\\ { \bf \Sigma}_{t|t} &= Var[{ \bf y}_t|I_t] \tag{12} \end{align} $$

式(11)と(12)を展開していきます。 ここでこちらのエントリーでまとめた多次元正規分布の条件付き確率の平均、分散の関係式を用います。

多次元正規分布の条件付き期待値・分散(2) - 多変数の場合 - BIOCV’s blog

平均：

$$ \begin{align} {\bf \hat{y}_{t|t}}&=E[{ \bf y_t }|I_{t-1},{\bf u_t, z_t}] \\\ &= E[{\bf y_{t|t-1} }|{\bf z}_t] \\\ &= {\bf \hat{y}_{t|t-1} + CS^{-1}(z_t - H_t \hat{y}_{t|t-1}) } \\\ &= {\bf \hat{y}_{t|t-1} + K_t (z_t - H_t \hat{y}_{t|t-1}) } \end{align} \tag{14} $$

ここで$C$と$S$はそれぞれ分散共分散行列であり、

$$ \begin{align} C &=Cov({ \bf y_{t|t-1},z_t }) \\\ &=Cov({ \bf y_{t|t-1},H_t y_{t|t-1}+ v_t } ) \\\ &=E[{ \bf (y_{t|t-1} - \hat{y}_{t|t-1})(H_t y_{t|t-1} - H_t \hat{y}_{t|t-1})^T } ] \\\ &=E[{ \bf (y_{t|t-1} - \hat{y}_{t|t-1})(y_{t|t-1} -\hat{y}_{t|t-1})^TH_t^T } ] \\\ &=E[{ \bf (y_{t|t-1} - \hat{y}_{t|t-1})(y_{t|t-1} -\hat{y}_{t|t-1})^T } ] \bf{ H_t^T }\\\ &={ \bf \Sigma_{t|t-1}H_t^T} \tag{15} \\\ \newline S&= Var({ \bf z_t } ) \\\ &= Var({ \bf H_t y_{t|t-1}+ v_t } ) \\\ &= { \bf H_t \Sigma_{t|t-1} H_t^T + R_t } \tag{16} \end{align} $$

よってカルマンゲイン：$K_t$は、

$$ { \bf K_t =\Sigma_{t|t-1}H_t^T S^{-1} } \tag{17} $$

となる。

分散：

$$ \begin{align} { \bf \Sigma_{t|t}} &= Var[{ \bf y_t } |I_t]\\\ & = Var({ \bf H_t y_{t|t-1}+ v_t } | { \bf z_t } ) \\\ & = {\bf \Sigma_{t|t-1} - \Sigma_{t|t-1}H_t^TS^{-1}H_t \Sigma_{t|t-1} } \\\ & = {\bf (I -K_t H_t)\Sigma_{t|t-1} } \tag{18} \end{align} $$

まとめると更新式は式(19)で表されます。

$$ \begin{align} { \bf S }&={ \bf H_t \Sigma_{t|t-1} H_t^T + R_t } \\\ { \bf K_t } &= { \bf \Sigma_{t|t-1}H_t^T S^{-1} } \\\ { \bf \hat{y}_{t|t} } &={ \bf \hat{y}_{t|t-1} + K_t( z_t - H_t \hat{y}_{t|t-1})} \\\ { \bf \Sigma_{t|t} } &={ \bf (I -K_t H_t)\Sigma_{t|t-1} } \end{align} \tag{19} $$

数値安定性

アプリケーションにかかわらず、式(10)と式(19)をステップ毎に計算することで時刻$t$における状態量$y_t$の平均と分散を知ることが出来ます。しかし、これらを愚直に実装すると数値誤差によって計算が不安定になってしまうと言う課題があります。より具体的には、分散は半正定値(positive semidefinite: PSD)であることが数学的に求められますが、数値誤差が蓄積するとこれが満たされなくなり計算が不安定になることがあります。 これを解決するために、式(10)と式(19)において分散：$\bf{\Sigma}_{t|t}$を直接予測・更新するのではなくこれを分解した行列を予測・更新する手法が提案され、実用上はこの方法が用いられています。

UDカルマンフィルター

分散：$\bf{\Sigma}_{t|t}$は半正定値行列なので、コレスキー分解によって上三角行列：$\bf{S}$によって以下の様に分解することが出来ます。

$$ { \bf \Sigma_{t|t} }= SS^T \tag{20} $$

さらに、$\bf{S}$は対角成分が全て$\sqrt{\quad}$を含む形と上三角行列の積の形に分解でき、

$$ { \bf S } = U\sqrt{\bf{D}} \tag{21} $$

という形で表すことができます。 ここで状態量が3次元の場合を例にすると、$\bf{U}$と$\bf{D}$はそれぞれ以下のような成分を持つ行列です。

$$ S= U\sqrt{\bf{D}}=\begin{bmatrix} 1 & \frac{s_{12}-\frac{s_{13}s_{23}}{s_{33}}}{s_{22}-\frac{s^2_{23}}{s_{33}}} & \frac{s_{13}}{s_{33}} \\\ 0 & 1 & \frac{s_{23}}{s_{33}} \\\ 0 & 0 & 1\end{bmatrix} \begin{bmatrix} \sqrt{s_{11} -\frac{s^2_{13}}{s_{33}}-\frac{(s_{12}-\frac{s_{13}s_{23}}{s_{33}})^2}{s_{22}-\frac{s^2_{23}}{s_{33}}}} & 0 & 0 \\\ 0 & \sqrt{s_{22} - \frac{s^2_{23}}{s_{33}}} & 0 \\\ 0 & 0 & \sqrt{s_{33}} \end{bmatrix} \tag{22} $$

ここで$s_{ij}$は${ \bf \Sigma_{t|t}}$の第$(i,j)$成分を表しています。

この$\bf{U}$と$\sqrt{\bf{D}}$を用いると${ \bf \Sigma_{t|t}}$は、

$$ { \bf \Sigma_{t|t} = SS^T = U\sqrt{\bf{D}}\sqrt{\bf{D}}U^T = UDU^T } \tag{23} $$

のように分解できます。

この分解を利用したUDカルマンフィルターでは、${ \bf \Sigma_{t|t} }$を直接扱うのではなく、この$\bf{U}$と$\bf{D}$を予測と更新によって伝搬します。$\bf{U}$と$\bf{D}$それぞれを数値計算する際に$\bf{U}$の下三角部分や$\bf{D}$の非対角成分を厳密に０として扱うことが出来るので、$\bf{\Sigma}_{t|t}$は半正定値性が保たれ数値的に安定に出来るというものです。

この場合、式(10)と式(19)に式(22)を代入して更に式変形して、予測モデルと更新モデルを${ \bf \Sigma_{t|t} }$ではなく、${ \bf U_{t|t} }$と${ \bf D_{t|t} }$が登場する形に変形します。${ \bf U_{t|t} }$と${ \bf D_{t|t} }$がわかれば式(23)によって毎時刻$t$の${ \bf \Sigma_{t|t} }$は計算することができます。詳細な式変形についてはこちらの文献が分かりやすかったので興味がある方はhttps://arxiv.org/pdf/2203.06105をご参照下さい。

まとめ

　前回のエントリー「カルマンフィルター(1) - 1次元の場合」と合わせてカルマンフィルターの概要と線型モデルの場合の一般の多次元状態量の場合のモデルの式を導出しました。また、実用上は本稿でも紹介したUD分解等によって数値安定な方法で実装されたものを用いることについても説明しました。

　本稿では式(1)・(2)が根底にあるモデルとした場合の予測式・更新式を導きました。式(1)・(2)は見ての通り、状態量や制御入力、観測に対して線形モデルで表される場合に限定された話をしています。しかし、実用上の多くの場合では線形性が仮定できないことの方が多いです。モデルが非線形の場合を扱うためには拡張カルマンフィルターがよく用いられます。こちらについては続くエントリーにて説明したいと思います。

関連記事

  • 一次元の場合のカルマンフィルター

biocv.hateblo.jp

biocv.hateblo.jp

参考文献