はじめに
カルマンフィルタは行列計算でひとまず実装ができてしまうので、なんだかよくわからないけど動かすことができます。 だいたい、結構ライブラリも揃っているので、そもそも行列計算なんて自分で書かない場合もしばしばあります。
というわけで、こいつが一体何をしているのかを理解していきましょう。
カルマンフィルタの概要
カルマンフィルタを使うようなケース
カルマンフィルタは端的に言えば、知りたいが直接見ることはできない時系列データ $x _ t$ を 実際に観測した $y _ t$ から逐次的に推定していくアルゴリズムです。ざっくりと数式を書いてしまえば、
$$ \begin{align} x _ {t} &= F _ t x _ {t - 1} + u _ t + w _ t \\ y _ {t} &= H _ t x _ {t} + v _ t \end{align} $$
という流れで時系列データが生成されていると考えます。ここで $u _ t$ は時系列データ $x _ t$ に影響を及ぼすような意図的な入力であり、 $w _ t$ は意図的でない外乱です。また、 $F _ t$ は仮に意図的な入力も外乱もないとすれば、 $x _ t$ がひとりでに変化していくであろう様子を表したなにかです（ $x _ t$ がベクトルなら $F _ t$ は行列だし、スカラーならスカラーである）。
一方二番目の式は $v _ t$ が観測時混入するノイズ（センサーへのノイズだと思えばよいでしょう）であり、 $H _ t$ が物理的な観測を表したものです（例えば知りたい$x _ t$ は温度なのだが、観測しているのは電位差である…とか、まあ計測に応じて生ずる物理的な変換であったりスケールの変換を意味するものになります）。
こんなふうな時系列データで、実際に観測できるのが $y _ t$ だけのとき、本当に知りたい $x _ t$ をどうやって復元（しかも各時刻逐次的に！！）するのかを考えた時に使えるのがカルマンフィルタになります。
例えば制御問題であるならば、 しばしば $y _ t$ を思い通りの値に至らしめたいという思いがあります。どうやら $y _ t$ が $x _ t$ に依存しているらしいので、 $u _ t$ によって $x _ t$ を変化させ、それを介して $y _ t$ をコントロールしてやることを考えたりします。ところが、$x _ t$ が本当に自分の与えた $u _ t$ でいい感じになっているかは直接見れない（制御したい $y _ t$ は見れるのだが、$x _ t$ を介してしか制御できない）ので $x _ t $ の振る舞いを把握する方法が必要です。
そこでカルマンフィルタで $y _ t$ から $x _ t$ を逆に推定して、次の$u _ {t + 1}$ を決定しようとしたりします（一般にオブザーバーと呼ばれる仕組みであり、今やろうとしているのは状態フィードバックである。仮に $x _ t$ も $y _ t$ も直接センシングできるなら、こんな小細工は不要である。センシングした $x _ t$ で次の $ u _ {t+1}$ を上手に構成してやり、最終的に $y _ {t+1}$ が目標値に近づくようにしてやればいい）。
制御問題になると、状態フィードバック制御器の設計はどうするんですか？とか他の問題が出てきてしまうので、今回は単に兎にも角にも $y _ t$ から $x _ t$ を復元してやるという問題を考えます。すごくシンプルに、なんだか信頼のおけないセンサーで $y _ t$ を観測したので、ノイズ $v _ t$ が乗る前の $x _ t$ を復元してやりたいという問題です。
というわけで、以降は $u _ t$ は無いものとします。意図的に $x _ t$ を操作するようなことはせず、単に自然に時間的に変動している $x _ t$ を $y _ t$ から復元します。また、$y _ t$ を計測した際には $x _ t$ の物理的な単位の変換は行われておらず、スケールも特に変わっていないとします。すなわち対象としている時系列データは
$$ \begin{align} x _ {t} &= F _ t x _ {t - 1} + w _ t \\ y _ {t} &= x _ {t} + v _ t \end{align} $$
となりました。すごくシンプルにかけましたね。
カルマンフィルタがやっていること
カルマンフィルタがやっていることは至って単純です。
とにかく $x _ t$ は観測できていないのですが、だいたい $\hat x _ {t}$ くらいであろうと予想を付けます（どうやって予想をつけるのかはあとで）。さて、それに対して $y _ t$ という観測が得られたとします。もしも $v _ t$ というノイズが邪魔をしていなければ
$$ y _ t = x _ t $$
のはずですが、現実はそうではありません。ノイズが乗る前が知りたいのです。かといって $v _ t$ がどんなものであったのかは分かりません。なので、$x _ t$ を復元するために我々が考えるのは、予想を付けた $\hat x _ t$ と $y _ t$ の間くらい（間とは限らんが）の値を適当に取ることにします（！！？！？）。すなわち
$$ x _ t \simeq x' _ t = T _ t y _ t + (I - T _ t) \hat x _ t $$
ということにしてしまいます。なんといい加減な…！ と思わないでください。そもそも $\hat x _ t$ が大外れしていないとして、また $y _ t$ もノイズが乗っているものの計測に不備がなかったとすれば、それぞれ相応に $x _ t$ に近いものになっているはずです。 $T _ t$ をもしも上手に決めてやることができれば、 それぞれ単体の値を使ってしまうよりは、$x _ t$ に近づくかもしれないと思えるはずです（当然 $T _ t$ をデタラメに決めれば離れることも想定できる）。
カルマンフィルタの流れ
流れとしてはこうです。
ひとまず天下り的に
$ x _ t $ の目処 $ \hat x _ t$ をつける。その後、目処付した値とノイズを含んだ観測値のどちらにより高い重みをつけるのかを決める $T _ t$ を計算する。実際に$x _ t$ にノイズ $v _ t$ が乗った $y _ t$ を観測して、上述の式で $x _ t$ の推定値 $x' _ t $ を計算する。
というのが時刻 $t$ で行われることです。で、次の時刻 $t + 1$ で行われるのは
$ x _ {t + 1} $ の目処を
$$ \hat x _ {t + 1} = F _ {t + 1} x' _ t $$
とつける（注意： $F _ {t + 1}$ は $x _ t$ を放っておいたらひとりで変化していく様子を表したものである。もしも放っておいたら何も変化しない場合は単なる単位行列になる。これは物理的特性などから既にモデル化されているものとする。 $x' _ t $ は前回の $x _ t$ の推定値である！すなわち、この目処付は、前回の推定がそれなりにあたっていた場合に、 $w _ t$ を無視して、自然に時間がたったらこうなっているはずである！という値を算出している）。
その後、目処付した値とノイズを含んだ観測値のどちらにより高い重みをつけるのかを決める $T _ {t+1}$ を計算する。実際に$x _ {t+1}$ にノイズ $v _ {t+1}$ が乗った $y _ {t+1}$ を観測して、上述の式で $x _ {t + 1}$ の推定値 $x' _ {t + 1} $ を計算する。
以下同様でございます。 ということで、一番最初に時刻は 目処付 $\hat x _ 0$ をほとんど当てずっぽうに初期化することになります（当てずっぽうは言い過ぎだが、ピタリとは当たらないだろう）。
そうすると、あとは $T _ t$ ってどうやって求めるんですか…？というところが重要になるわけですが、
を下から読んでいけば分かります（丸投げ…）。 あと、Wikipediaの式見ると良いです（丸投げ…）。
