世界一分かりやすいカルマンフィルタの理論と導出と実装

カルマンフィルタは、1960年にカルマン博士が提案したアルゴリズムで、現在、制御工学や宇宙工学、通信工学、機械学習分野などで非常によく用いられているアルゴリズムです。特に機械学習の文脈では、時系列分析における線形ガウシアンな状態空間モデルの開放などにも利用されており、機械学習を学ぶエンジニアにとっても、このカルマンフィルタを学ぶ必要性は非常に大きくなっています。

今回は、このカルマンフィルタの理論と導出を一通り行い、その上でPythonを用いて実際に実装し、カルマンフィルタを用いた状態推定と将来予測を行います。

カルマンフィルタは統計学、制御工学、機械学習、通信、宇宙工学など様々な分野に登場し、基本的なアルゴリズムと言われることもありますが、カルマンフィルタによって得られる帰結を利用するだけでなく、カルマンフィルタが仮定する条件からその結論の導出法まで隅から隅まで理解しようとすると大変骨が折れて難解です。

カルマンフィルタを難解なものとしている理由としてはおそらく、下記の理由が考えられると思います。

カルマンフィルタが難しい理由

カルマンフィルタが扱う問題や目的がわかり難い
状態空間モデルや条件付き確率など、前提としてる知識が膨大
多変量を扱うので図示が難しいのに、数式が難解イメージできない
導出や証明に非常に長い式変形を必要とする
統計学や現在制御、機械学習といった様々なジャンルで扱われており、定式化の方法や表現などが分野ごとに異なり混乱する
機械学習の文脈で登場するのに、制御理論の観点からの解説が多く、機械学習とのつながりが分からない
etc …

そして、これらの理由をカバーできるような参考書もほとんど存在しないのが、現状だと思います。

このため、この記事では、カルマンフィルタの扱う問題をわかりやすく、前提としている知識をできる限り説明しながら、絵や図を交えて、定式化の方法を統一したり、混乱しそうなところがについては補足を交えながら、丁寧にカルマンフィルタのアルゴリズムを説明していきたいと思います。

カルマンフィルタは、現在の工学分野で非常に応用範囲が広いだけでなく、拡張カルマンフィルタ(Extended Kalman Filter, EKF)や粒子フィルタ(particle filter)など、実務で広く利用されるアルゴリズムを理解するためにもまずカルマンフィルタについて理解する必要があるので、頑張って理解しましょう。

本記事の作成においては、基礎からわかる時系列分析を参考にしました。

こちらの本は、私が参照してきたどの本よりも、直感的にカルマンフィルタについて解説されています。何か1冊、カルマンフィルタの本を購入しようか考えている人は、基礎からわかる時系列分析をおすすめします。

本記事の内容

カルマンフィルタの問題設定を理解
カルマンフィルタの導出を理解
カルマンフィルタを実際にPythonで実装する

カルマンフィルタの概要

まず最初にカルマンフィルタがどのような技術で、なぜ重要であるのかについて押さえていきましょう。カルマンフィルタ自体は、機械学習の分野では状態空間モデルと表現されることもあり、機械学習の有名な教科書であるPRML でもかなりの紙面を割いて解説されていますが、なかなかその必要性について理解するのが難しいのが現状です。

今回は具体例を示すのに、カルマンフィルタが開発されたきっかけでもある、宇宙船の位置推定の具体例を通して、カルマンフィルタの必要性を考えていきましょう。

カルマンフィルタの必要性

唐突ですが、地球から月に向かって、無人ロケットを打ち上げた場合を考えましょう。この時、無事に月に着陸するためには、ロケットの位置を正確に知る必要性があります。もしロケットの位置を正確に把握せずに月に着陸しようとした場合、月との距離を正確に算出することができず、ロケットは月に衝突して粉々になってしまうからです。

ではこのロケットの時刻$t$における本当に位置を知るにはどのようにしたら良いでしょうか？

ここで、カルマンフィルタは、このロケットのように移動する物体の正確な位置を知るための手法です。正確には、より普遍的なものなのですが、現状はこのように理解して問題ありません。

このような移動するロケットの位置を知るには、以下の方法が考えられると思います。

1つは、ロケットの運動方程式を解くものです。

今回、ロケットが一定の加速度$a$で、初速$v_0$、初期位置$x_0$で月に向かうとすると、運動方程式によって、時刻$t$のロケットの位置$x$は次のように書くことができます。

運動方程式によるロケットの位置の導出

x = \frac{1}{2}at^2 + v_0t + x_0

運動方程式や物理学に疎い人は、物理法則によってこのように解析的に導かれるんだな、くらいに理解しておけば良いでしょう。

もう1つは、何かしらのセンサーによって、直接的に、ロケットの位置を把握するというものです。

これは運動方程式を解かなくても、このようなレーダーを利用すればロケットの位置を知ることができるので、大変便利ですね。しかし、現実世界では、この2つの方法はどちらも問題があります。

それは、運動方程式のような物理法則を解く場合であっても、ロケットのエンジン出力は一定ではないので、現実的には厳密に一定の加速度$a$で進むことは難しく、多少加速度は変動するのが普通です。すると、実際のロケットの位置は、微妙にずれてくるのが普通です。運動方程式だけを頼りにしている場合は、このような実際の加速度のズレなどに当然ですが考慮できないため、計算で出したロケットの位置が実際とずれることがあります。

現実世界では、ロケットのエンジンだけでなく、微妙な空気抵抗があるかもしれませんし、地球の表層で強い風が吹いていて、微妙に横風によってロケットの軌道がずれてしまうかもしれません。このように運動方程式だけではうまくいかなそうなのはイメージが湧くかもしれません。

では、一方でセンサーを使う場合はどうでしょうか？

現実世界で運動方程式で厳密にロケットを予測するのが難しいなら、センサーやレーダーで直接計測すればいいじゃないか！と思う人もいるかもしれません。

それは半分正解で、半分不正解です。実際に、この世の中に存在するセンサーは、すべて誤差を含んでいます。例えば、体重計や血圧計も、体の状態を測るセンサーだと考えることができますが、これらのセンサーや測定機器全てに誤差が含まれています。

つまり本当の体重や本当の血圧に対して、微妙に値が異なっているんですね。通常はその誤差がかなり小さいので意識することはありませんが、無人ロケットが月に着陸するような状況では、このような誤差が命取りです。なので、誤差を含むセンサーでは正確な位置を知ることができないのです。

では、実際どうしたら、ロケットの位置を正確に知ることができるのでしょうか？

ここでやっと、カルマンフィルタの出番です。カルマンフィルタでは、このような状況で、ロケットの正確な位置を知るために、運動方程式とセンサーの値をうまく組み合わせることで、より正確なロケットの位置を知る手法です。

運動方程式でも誤差が出てしまうし、センサーでの測定でも誤差が出てしまう。このような状況でカルマンフィルタを用いることで、運動方程式、センサーをそれぞれ単独で使うよりも、正確にロケットの位置を知ることができるのです。

これがカルマンフィルタの必要性であり、カルマンフィルタとはなんぞや、ということの説明になります。

なんとなく、カルマンフィルタの必要性がわかりましたか？

ちなみに、ここまでの解説でもあったように、カルマンフィルタにおいては次のような前提条件や頭に入れておいておかなければならないことががあります。

カルマンフィルタの前提や理解しておくべきこと

運動方程式などの物理法則を用いてある物理量を測定しようとしても、外的要因などによって誤差が生じてしまう
センサーで直接測定しても誤差が生じてしまう
カルマンフィルちゃは、運動方程式や物理法則と、センサー測定値を組み合わせて、ある物理量を正確に知るための手法である
逆に言えば、カルマンフィルタは、ある物理量の運動方程式が既知であり、かつ、センサーによる測定ができている時にしか利用することができない

これらのことを、常に頭に叩き込んでおく必要性があります。特に最後の行で、カルマンフィルタは、運動方程式とセンサーの測定値両方がある時しか利用することができないんです。

機械学習の枠組みでカルマンフィルタや状態空間モデルを勉強している人は、このことが頭から抜け落ちてしまう傾向にあるので、ここは意識するようにしてください。

なので、機械学習の手法から見たとき、カルマンフィルタ単独ではほとんど使い物にならないということです。機械学習で何かしらの予測や異常検知、識別等のタスクをしようとする場合であっても、運動方程式が既知であるような場合というのはほとんどないからです。

カルマンフィルタはあくまでも、運動方程式とセンサーの測定値が既知である時に、これらを組み合わせることで、より誤差の少ない正しい真の値を得ることができる手法である。

と認識しておいた方が良いでしょう。

実はシステム同定という分野では、機会学習や統計的な手法によって測定値からカルマンフィルタにおける運動方程式を逆算しようとするものもありますが、これは応用のテーマであり、まずはカルマンふフィルタは、このようなものであると覚えておくのが良いでしょう。

ちなみにカルマンフィルタのテキスト等では、このセンサーの測定値を観測値や記号で$\bm{y}$で表現することが多いです。またこの運動方程式を行列形式で表現し、システム行列ということになっています。これらの詳しい解説は、この記事の後半で登場します。

カルマンフィルタの考え方

ここまででカルマンフィルタの概要や立ち位置について理解できたでしょうか。

繰り返しになりますが、カルマンフィルタは、ある物理量などの真の値が知るために、物理法則などの運動方程式を解いた結果とセンサーの測定値を組み合わせることで、より正確にある物理量を予測するための手法です。

ここまで散々触れてきたので、ここまでは大丈夫ですね。

ではカルマンフィルタは、この物理法則から導かれる値とセンサー値をどのように組み合わせるのでしょうか？？

実はカルマンフィルタの基本的な考え方はシンプルです。2つの値がどちらも微妙に本物の値からずれているのであれば、その平均ととったら、少なくとも片方を使うだけよりも、本当の値になるんじゃないか？

というものです。つまり平均を取るということですね。

ここで、世の中のテキストに倣って、時刻$t$における運動方程式によって導かれた予測値を$x_t$、センサーによる計測値を$y_t$と、記号で表現しましょう。

基本的にカルマンフィルタの考え方は、これらの両方の値を使いましょうということで、カルマンフィルタのよる予測値を$x_{kalman}$とすると、

x_{kalman} = \frac{x_ t + y_t}{2}

というようなイメージになります。上の式だと、運動方程式による値$x_t$とセンサーの値$y_t$の平均をとっているのがわかりますね。ちなみに、運用方程式によって予測された値$x_t$のことを、事前推定値と呼んだりします。センサーの値は、観測値と呼びます。一方、カルマンフィルタによって、予測された値$x_{kalman}$のことを、事後推定値と呼びます。

これは単純に用語の話ですが、ここまでのカルマンフィルタの前提や概要を理解していると、事前推定値、観測値、事後推定値が何を意味しているかは、イメージできると思います。

カルマンフィルタの学習で躓いている人の多くは、この用語の意味すら理解できずに諦めている人が多いので、ここまできちんと理解できるようにしましょう。

この用語は、以降のカルマンフィルタの数式の海を渡り抜くために、必ず理解してほしいので、下記に改めてまとめます。

カルマンフィルタで登場する重要な3つの用語

事前推定値 … 運動方程式によって計算された物理量
観測値 … センサーによって測定さえた値
事後推定値 … カルマンフィルタのよって計算された、事前推定値と観測値の平均
(正確には平均ではありませんが、この解説は続いて行います)

ここまでで、カルマンフィルタの簡単な考え方をまとめました。

カルマンフィルタは、運動方程式によって導出された事前推定値とセンサーの観測値の平均を取ることで、いい感じに誤差を無くします。（この値が事後推定値です）

しかし、実はカルマンフィルタが計算する事後推定値は、事前推定値と観測値の単純な平均ではありません。

しかし考え方はほとんど変わりません。

平均の場合、事前推定値と観測値を同等と見做して、それらの平均を取ることで事後推定値を出していました。しかし、実際には、運動方程式による事前推定値の方が正しいこともありますし、観測値の方が正しいこともあります。このため、事前推定値と観測値の方で、実際により近い方に寄せてしまおう、というのが、カルマンフィルタの本当のところの考え方です。

このことを、図で表現してみましょう。

まず、事前推定値と観測値の平均を取る場合は、このような、事前推定値と観測値の内分点を計算をしています。

ここで、事前推定値を$x_t$と観測値を$y_t$とした時、観測値と事前推定値の差は、$y_t – x_t$で表現できます。ここで、カルマンフィルタによる事後推定値$x_{kalman}$は、事前推定値と観測値の平均を取るとすると、事前推定値を基点に、$y_t – x_t$ だけ進めれ良いことになるので、

x_{kalman} = x_t + \frac{1}{2} (y_t - x_t) = \frac{x_t + y_t}{2}

となるわけです。

ここで、カルマンフィルタでは実際には、平均値ではなく、事前推定値と事後推定値のより正しい方に寄せてしまう考え方です。

今、事前推定値と観測値の正しさの割合が、$k-1 : 1$ であるとしましょう。

この、$k-1: 1 $というのは、かなり恣意的に思えるかもしれませんが、例えば、$6: 5$であっても、$11:6 $であっても、スケールを整えることで、$k-1: 1 $の形にできますよね。

$k-1$の形はかなり違和感がありますが、あまり重要ではないので、ひとまずこのように表現するとします。すると、平均の時と同じように、このように考えることができます。

このように、事前推定値と観測値のどちらがより正しそうかという値を用いて、事後推定値を

\begin{equation} x_{kalman} = x_t + \frac{k-1}{k}(y_t - x_t) \end{equation}

と表現することができます。

ここで、$\frac{k-1}{k}$の部分、つまり、

水色で網掛けがされた部分のことをカルマンゲインと呼び、事前推定値あるいは観測値のどちらをどれだけ重要視するかの指標で、事後推定値を表現するのが、カルマンフィルタです。

と、ここまでが、カルマンフィルタのイメージです。

ここまで述べてきたことをまとめると、カルマンフィルタは、運動方程式から予測される事前推定値とセンサーなどから測定される観測値を利用する。事前推定値にも観測値にも誤差が含まれているので、その誤差を良い感じに打ち消すために、カルマンゲインを用いて、事前推定値あるいは観測値の信頼できる方の値を重視して、事後推定値を計算する手法

とまとめることができます。

ここまで、なんとなく、カルマンフィルタのイメージができましたでしょうか？

カルマンフィルタの導出(1次元の場合)

続いて、これらのカルマンフィルタのイメージを持ちながら、厳密な数式に対峙してきましょう。

カルマンフィルタは多次元の場合と1次元の場合が両方あります。PRMLや現代制御論の教科書では、多次元のカルマンフィルタの導出を扱います。しかし実際問題として、多次元のカルマンフィルタの導出を行うのは、多変量ガウス分布に関連する、複雑な行列計算を延々と繰り返すことになり、初学者は100%脱落します。カルマンフィルタが難しい所以は、多変量カルマンフィルタの導出が初学者にとっては、本当に鬼のように難しいところだと思います。

なので、まず、1次元のカルマンフィルタを導出することこの章では目指していきたいと思います。

多次元のカルマンフィルタも1次元のカルマンフィルタも扱っている次元や式変形の難易度が違うだけで考え方は一緒なので、まずは1次元のカルマンフィルタから学んでいきましょう。

ちなみに、カルマンフィルタの導出はかなり難関ですが、カルマンフィルタから得られた結論を利用するだけなら比較的簡単なので、安心してください。

1次元カルマンフィルタの導出

では早速1次元カルマンフィルタの導出に入っていきましょう。

最初の前提で述べたように、カルマンフィルタでは、運動方程式とセンサーの値が既知である場合を前提とします。また運動方程式やセンサーには誤差がそれぞれ入っているとしています。

今、時刻$t-1$の状態から、次の時刻$t$における変化を次のように表現します。

1次元カルマンフィルタの状態方程式と観測方程式

状態方程式

\begin{equation} \begin{split} x_t &= A x_{t-1} + v_n \end{split} \end{equation}

観測方程式

\begin{equation} \begin{split} y_t &= Bx_t + w_n \end{split} \end{equation}

ここで、(2)式は状態方程式、(3)式は観測方程式と呼ばれています。

そして、(2)式と(3)式で登場するのが線形変換になります。

カルマンフィルタにおける問題設定

カルマンフィルタは次の線形な状態方程式と観測方程式で表現できる、状態空間モデルにおいて、得られる観測値から状態$\hat{\bm{x}}_{t|t}$を推定する問題である。

カルマンフィルタリング問題の状態遷移式

\begin{equation} \begin{split} \bm{x_t} = F_t \space \bm{x_t} + G_t \bm{w_t} \\ \bm{y_t} = H_t \bm{x_t} + \bm{v_t} \end{split} \end{equation}

具体的には、実際の状態を$\bm{x}_{t}$とした時、$E[(\| \bm{x}_{t} – \hat{\bm{x}}_{t|t} \| )^2 | \bm{Y}_{1:t})]$を最小化するような$\hat{\bm{x}}$を求める問題である。この問題がカルマンフィルタリング問題であり、それを解決するアルゴリズムがカルマンフィルタである。

ここで(1)式に登場する$\bm{w_t}$、$\bm{v_t}$ はそれぞれ状態ノイズ、観測ノイズであり、それぞれ平均0、分散共分散が $\bm{Q_t}$、$\bm{R_t}$のノイズです。

\begin{equation} \begin{split} \bm{w_t} \sim \mathcal{N}(0, \bm{Q_t}) \\ \bm{v_t} \sim \mathcal{N}(0, \bm{R_t}) \end{split} \end{equation}

カルマンフィルタによって得られるアルゴリズム

問題設定がおわったところで、カルマンフィルタによって得られるアルゴリズムについて先に掲載します。カルマンフィルタでは、主に次の5ステップによって、現在の状態量を推定するアルゴリズムです。

カルマンフィルタのアルゴリズムの全体像

カルマンフィルタは最初の2ステップは予測のアルゴリズムになります。

1. 事前状態の推定

\begin{equation} \bm{\hat{x}}_{t:t-1} = F_t \bm{\hat{x}}_{t-1:t-1} \end{equation}

カルマンフィルタの最初の1ステップ目は、最初の状態の予測値、つまり時刻$t-1$まで得られるデータを用いて予測した状態$\bm{\hat{x}}_{t-1:t-1}$ から、次の時刻の状態を予測します。

ここで、意識して欲しいのは、$\bm{\hat{x}}_{t-1:t-1}$と$\bm{\hat{x}}_{t:t-1}$の違いです。

同じ推定値ですが、この両者の意味はカルマンフィルタにおいて決定的に重要です。この違いを理解できてない段階では、カルマンフィルタは全く理解できないはずなので、この違いをしっかり抑えましょう。

まず、$\bm{\hat{x}}_{t:t-1}$ですが、これは$t-1$までの観測値から$t$の状態を予測した予測値になります。事前状態推定値と呼ばれることもある量です。

これから説明しますが、カルマンフィルタのアルゴリズムでは、事前状態推定値を推定した上で、実際の観測値を予測し、事後状態推定値を出すという流れになっています。このため、観測値を得る前の状態推定値と、観測値を得た後の状態推定値があることを、理解してください。（といっても、現段階ではそういうものだと思うしかないです。）

事前状態推定値は、下記のように様々な書き方をされています。

\begin{split} & \bm{\hat{x}}_{t:t-1} \\ & \bm{\hat{x}}({t:t-1}) \\ & \hat{X}^{-}_t \end{split}

最後の$X^{-}_t$といった表記も参考書や論文に非常によく登場します。この表現を見た瞬間に、「ああ、$& X^{-}_t$は事前状態推定値を表していて、時刻$t-1$までの観測値を用いて時刻$t$の状態を推定した値なんだな」と思えるようになる必要性があります。

一方、事後状態推定値は下記のような様々な書き方をされます。

\begin{split} & \bm{\hat{x}}_{t:t} \\ & \bm{\hat{x}}({t:t}) \\ & \hat{X_t} \end{split}

この記事でも、できる限り簡単な表記にするため、$X^{-}_t$や$X_t$と表すことにします。

(3)式も上記の表現に書き直すと、下記のようになります。

\begin{equation} \hat{X}^{-}_t = F_t \hat{X}_{t-1} \end{equation}

2. 事前誤差共分散行列の推定

先ほどまでで、カルマンフィルタの事前状態値を導出するステップまで来ました。次は、事前誤差共分散行列を求めます。カルマンフィルタでは、事前誤差共分散行列を$P$で表現します。

そもそも、この事前誤差共分散行列とはなんだ？という話ですがこれは、条件付き確率$p(\bm{x}_t | \bm{Y}_{1:t})$における、分散共分散行列である。もっというなら、状態方程式によって状態量の平均は先ほどのステップ1で推定しました。そして、今回は、状態方程式によって、その分散共分散を更新するステップです。

要は、最初の1ステップ、２ステップで、$p(\bm{x}_t | \bm{Y}_{1:t}) \sim \mathcal{N}$ の正規分布における、平均と分散共分散を出しているということです。

カルマンフィルタリング問題では、システム行列が線形で誤差分布がガウス分布だと仮定しており、条件付き確率$p(\bm{x}_t | \bm{Y}_{1:t})$もガウス分布になる。ガウス分布の場合は、1次モーメント(平均)と2次モーメント(分散共分散行列)がわかれば形状は1つに定まる。

カルマンフィルタで登場する、分散共分散が$P$というのは、状態量$p(\bm{x}_t | \bm{Y}_{1:t})$の事前ガウス分布における分散共分散が$P$が表現されていると思えば良い。ちなみに、事前誤差共分散行列とあるが、名前が少し違う事後誤差共分散行列もある。

今回の記事では、事前誤差共分散行列を$P^{-1}_{t}$と表現し、事後誤差共分散行列を$P_{t}$と表現する。事後誤差共分散行列を$P_{t}$ の導出方法は(4)式で行う予定である。

\begin{equation} P_t^{-1} = F_tP_{t-1}F_t^T + G_t Q_t G_t^T \end{equation}

3. カルマンゲイン行列の計算

\begin{equation} G_t = P_{t}^{-1}H_t ^T(H_t P_{t}^{-1} H_t ^T + R_t )^{-1} \end{equation}

4. 事後状態推定値の導出

\begin{equation} \hat{X}_t = \hat{X}_{t}^{-1} + G_t(Y_k - H_t \hat{X}_{t}^{-1}) \end{equation}

5. 事後誤差共分散行列の計算

\begin{equation} P_t =(I - G_t H_t)P_t^{-1} \end{equation}

カルマンフィルタは、初期値を与えた後は、(4)→(5)→(6)→(7)→(8)→(4)→(5)→(6) … と繰り返し実行し、事後分布を更新していき、状態を推定していくアルゴリズムになります。ここまでが、カルマンフィルタの全体像になります。

ここからは、なぜ、(4)~(8)の5式が導出されるのか、その証明から導出までやっていきましょう。

状態空間モデルにおける、予測・フィルタリング・平滑化

先ほどの、カルマンフィルタにおいては、得られる観測値から状態$\hat{\bm{x}}_{t|t}$を推定する問題だと書きました。ここで、現時点の時刻が$t$だとする時、時刻$t$より先の$t+1$の状態を推定することを予測、時刻$t$の状態を予測することをフィルタリング、時刻$t-1$など、過去の状態を予測することを、平滑化といいます。

まず、予測、フィルタリング、平滑化の分布の条件付き確率の式を示します。これらはカルマンフィルタリング問題だけでなく、一般的な状態空間モデルを前提とした確率モデルにおいて成り立つ式になります。ここで、現在の時刻を$t$とし、時刻$t$まで得られた観測値を$\bm{Y}_{1:t}$とします。

状態の予測分布の定式化

状態の予測は、時刻$t$までに得られたデータ$\bm{Y}_{1:t}$から将来の状態$\bm{x}_{1:t}$を予測する問題です。これらは条件付き確率を用いて、(3)式のように表現できます。

\begin{equation} \begin{split} p(\bm{x}_{t+1} | \bm{Y}_{1:t}) &= \int_{- \infin}^{\infin} p(\bm{x}_{t+1} , \bm{x}_{t} | \bm{Y}_{1:t}) d \bm{x}_{t} \\ &= \int_{- \infin}^{\infin} p(\bm{x}_{t+1} | \bm{x}_{t}, \bm{Y}_{1:t}) p( \bm{x}_{t} | \bm{Y}_{1:t}) d \bm{x}_{t} \\ &= \int_{- \infin}^{\infin} p(\bm{x}_{t+1} | \bm{x}_{t}) p( \bm{x}_{t} | \bm{Y}_{1:t}) d \bm{x}_{t} \\ \end{split} \end{equation}

三行目の式変形はマルコフ過程による仮定を取り込んでいます。

状態のフィルタリング分布の定式化

状態のフィルタリング分布は、時刻$t$までに得られたデータ$\bm{Y}_{1:t}$から現在の状態$\bm{x}_{1:t}$を予測する問題です。これらも、

\begin{equation} \begin{split} p(\bm{x}_t | \bm{Y}_{1:t}) &= p(\bm{x}_t | \bm{y}_t, \bm{Y}_{1:t-1}) \\ &= \frac{p(\bm{x}_t, \bm{y}_t| \bm{Y}_{1:t-1})}{p(\bm{y}_t| \bm{Y}_{1:t-1})} \\ &= \frac{p(\bm{y}_t| \bm{x}_t, \bm{Y}_{1:t-1}) p(\bm{x}_t | \bm{Y}_{1:t-1}) }{p(\bm{y}_t| \bm{Y}_{1:t-1})} \\ &= \frac{p(\bm{y}_t| \bm{x}_t) p(\bm{x}_t | \bm{Y}_{1:t-1}) }{p(\bm{y}_t| \bm{Y}_{1:t-1})} \\ \end{split} \end{equation}

状態の平滑化分布の定式化

平滑化分布とは、現在の時刻$t$から得られる観測値$\bm{Y}_{1:t}$から、過去の時刻の状態を推定する分布のことです。平滑化分布の条件付き確率も求めることができますが、一旦カルマンフィルタのアルゴリズムを説明する上では、予測分布とフィルタリング分布が分かっていれば問題はないので、平滑化分布の定式化は別の記事で説明したいと思います。

カルマンフィルタの導出

ここまでで一般的な状態空間モデルにおけるフィルタリング分布と予測分布の定式化を(3)式と(4)式で行いました。

これらの条件付き確率において成り立つ式は、状態空間モデルのどのようなモデルにおいても成り立つ式です。一方で、(3)式と(4)式はなかなか複雑な式をしており、解析的に解くことは一般的に難しいはずです。

しかし、カルマンフィルタでは、(1)式と(2)式で表される状態方程式、および観測方程式にある仮定を導入することで、予測分布およびフィルタリング分布の予測式である(3)式と(4)式の計算を簡易化しています。ここで入れる過程は、(1)式における状態遷移の項$\bm{F_t}$がベクトルである過程。つまり状態遷移が線形変換によっておこる過程が一つ目の過程です。

そして2つ目の過程が、(2)式と(3)式で表されるノイズの分散$\bm{Q_t}$、$\bm{R_t}$がガウス分布に従うという過程です。(そもそもそれを過程したのがカルマンフィルタリング問題なのですが）

まず、(4)式のフィルタリング分布の構成要素から考えていきましょう。

まず、$\bm{y}_t| \bm{x}_t)$ですが、これは、観測方程式から、$\bm{x_t}$が$H_t$による変換を受けて、さらにその誤差分散として$\bm{R_t}$が入るので、下記のようになる。

\begin{equation} \begin{split} p(\bm{y}_t| \bm{x}_t) \propto exp \biggr\{ - \frac{1}{2} (\bm{y_t} - H_t \bm{x_t})^T \bm{R_t}^{-1}(\bm{y_t} - H_t \bm{x_t}) \biggr\} \end{split} \end{equation}

続いて、フィルタリング分布における、$p(\bm{x} \ bm{Y}{t-1})$であるが、これは

\begin{equation} \begin{split} p(\bm{x}_t| \bm{Y}_{t-1}) \propto exp \biggr\{ - \frac{1}{2} (\bm{x_t} - \bm{\hat{x}_{t|t-1}})^T \hat{\Sigma}^{-1}_{t|t-1}(\bm{x_t} - \bm{\hat{x}_{t|t-1}}) \biggr\} \end{split} \end{equation}

と表現できる。

参考文献

本記事を執筆するにあたっては、下記の資料・書籍を参考にさせていただきました。

電子情報通信学会知識ベース 1群(信号・システム) 5編 (信号理論) 6章カルマンフィルタ西山清
Advanced Python 時系列解析島田直希
基礎からわかる時系列分析