カルマンフィルタに関するメモ

はじめに

カルマンフィルタ本体は決して難解なものではなく、いたって単純です。一方で、「カルマンフィルタを設計すること」はおそらく大変複雑であり、私の手におえそうにはないためここでは割愛します。というわけで、この記事ではカルマンフィルタを勉強した私が、いまいちわからなかった点について、言葉で平易にまとめています。おそらく、数学ができる人や、きちんとした教科書でカルマンフィルタを勉強した人にとっては、意味のない文章です。勉強したけど、なんだかよく分からなかった人（私）が対象です。

仕組み

カルマンフィルタの仕組みは次のようになっています。ここで、観測値をy(t)、隠れ変数ないし内部状態をX(t)、その分散共分散行列をP(t)とします。

• 初期値X(0)、P(0)を決める。
• 次のステップにおける予測値X^(t+1)とP^(t+1)を計算する（予測）。
• 観測方程式とX^(t+1)から、観測値の推定値y^(t+1)を求める。
• 観測方程式とP^(t+1)から、観測値の共分散行列S(t+1)を推定する。
• 観測方程式とP^(t+1)、S(t+1)からカルマンゲインを計算。
• X^(t+1)をカルマンゲインと観測値y(t)とy^(t)の観測誤差を求めてX(t+1)を補正する。
• P^(t+1)をカルマンゲインと観測方程式を用いて補正する。

これらの計算ができるのは、システムの不確実性Qや、観測の不確実性Rが所与であるため。

import numpy as np
import matplotlib.pyplot as plt
from numpy.random import multivariate_normal
from scipy.linalg import inv, det

class Model:

    def __init__(self, F, Q, H, R):
        self.F = F
        self.Q = Q
        self.H = H
        self.R = R
        self.Qmean = np.zeros(Q.shape[0])
        self.Rmean = np.zeros(R.shape[0])

    def sample(self, x):
        q = multivariate_normal(self.Qmean, self.Q)
        r = multivariate_normal(self.Rmean, self.R)
        x = self.F @ x + q
        y = self.H @ x + r
        return x, y

class KalmanFilter:

    def __init__(self, model):        
        self.sys_eq = model.F
        self.sys_noise = model.Q
        self.obs_eq = model.H
        self.obs_noise = model.R

    def predict(self, x, P):
        F, Q = self.sys_eq, self.sys_noise
        return F @ x, F @ P @ F.T + Q

    def filt(self, x, P, y, l):
        H, R = self.obs_eq, self.obs_noise
        # 予測、X^(t+1)とP^(t+1)を計算する。
        x, P = self.predict(x, P)
        # 観測値の期待値を計算する。
        f = H @ x
        # 観測値の誤差分さ共分散を計算する。
        S = H @ P @ H.T + R
        # カルマンゲインを計算する。
        K = P @ H.T @ inv(S)
        # X^(t+1)を真のX(t+1)のy(t)による「推定値」に寄せる。
        x = x + K @ (y - f)
        # P^(t+1)を真のP(t+1)のy(t)による「推定値」に寄せる。
        P = P - K @ H @ P               
        # tにおける対数尤度を計算する。
        l += np.log(det(S)) + np.log(np.sum((y-f)*(y-f)) / det(S))
        
        return x, P, l

F = np.array([[1]])
Q = np.array([[1]])
H = np.array([[1]])
R = np.array([[2]])

kf = KalmanFilter(Model(F,Q,H,R))
T = 150

xs = np.cumsum(np.random.normal(0.0, 1.0, T))
ys = xs + np.random.normal(0.0, 2.0, T)

xhat = np.zeros((T,1))
Phat = np.zeros((T,1))
Phat[0][0] = 0.5

likelihood = 0.0

for t in range(T-1):
    xhat[t+1], Phat[t+1], likelihood = kf.filt(xhat[t], Phat[t], ys[t], likelihood)

plt.plot(ys, '--', label='observed')
plt.plot(xs, linewidth=2, label='true X')
plt.plot(np.squeeze(xhat), label='X^')
plt.legend()
plt.show()
    
print("Likelihood={}".format(-0.5 * (T * np.log(2*np.pi) + likelihood)))

普通に調べると数式しか出てこないのですが、ある程度の誤解及び不正確さを許容しつつ言葉で説明するとこんな感じです。私のような「三流ぷろぐらまー」はどんな風に動いているか分からないと、自信をもってプログラムを書けませんので、ざっくりとした説明をメモ代わりに残します。

コードを書くという視点から見ると、カルマンフィルタの変数はx(t)とP(t)となります。システムの状態及び分散共分散行列が、関数の入力及び出力になるようなイメージです。カルマンゲインや、観測誤差、観測誤差の共分散行列は計算途中で使用しますが、次のステップでは新たに計算しなおすので、一時変数のようなものです。

さて、なんでこんなに七面倒くさい感じになっているのか？ということについて、個人的に理解できた範囲で説明します。

観測方程式

そもそも、なぜ、観測方程式などというものを想定するのか？ということです。おそらく、普通に考えてまず疑問に思うのがここだと思います。だって、システムの内部状態にノイズ（システムノイズと呼ばれる成分）が含まれているのだから、別に観測方程式で新たな誤差を想定する必要などないように思えます。ところが、このように観測とシステムの時間発展を区別して考えることこそが、カルマンフィルタの肝であり、便利なゆえんです。

仮に、完璧な計算というものが存在するとします。このとき、システムノイズ及び観測ノイズの分散は、両方ともゼロになると考えられます。そのため、統計的推定など行う必要すらありません。計算結果＝現実の値となるため、統計が入り込む余地がないからです。神様が、宇宙で野球のボールを投げたら、（神様なので観測誤差が０であり、現代物理学によるとボールの軌道は一般相対性理論によって計算できることから）軌道は完璧に予測可能なはずです。

でも、これは現実的な場合ではありません。

たとえば、キリスト教徒によると人間には自由意思があります。そのため、たとえ神様であれ、人間の行動は完全には予測できないはずです（なぜなら、不確実性が伴わないのなら、実際に行われた行動以外に行動が存在しないことになるので、選択の余地がなく、自由意思もないと考えられるから）。そのため、仮にある人間、太郎君がいたとして、太郎君の将来的な行動に関しては、全知全能の神でも完全には予測できないと考えられます。

さて、太郎君の時刻tにおける行動X(t)から予測される、次の時間ステップにおける行動の予測をX(t+1)=F(X(t))+v(t)と書くことにします。一方で、神様は完全な観察者であると考えられるため観測誤差はゼロです。ただ、神の認識は人間と異なるので、神は太郎君の実際の行動X(t+1)を直接見るのではなく、何らかの変換を受けたy(t+1)=H(X(t+1))を見るとします（映画マトリックスで、タンクがマトリックスの画面を見ているような感じ）。さて、この時、Fの形とvの分散が既知だとして、神様は、カルマンフィルタによって観測結果y(t+1)から何らかの知識を引き出すことができるでしょうか？答えは、否です。神様にとってはF(X)が既知なので、y(t+1)に直接現れないX(t+1)の分散共分散行列に関しては計算が可能ですが、これは飽くまでX(t+1)をy(t+1)から逆算した段階で初めて更新可能になる情報ですので、今ある知識が形を変えたものに過ぎません。神様が太郎君に自由意思を認めている、というのはこういう意味です。

何が言いたいのかというと、カルマンフィルタはシステム方程式の不確実性については、何ら新しい知識をもたらすものではないということです。これが、カルマンフィルタが、フィルタと呼ばれるゆえんなのではないかと思います。システムノイズv(t)は、神が人間に与えたもう自由意思と同じで、システム設計者（アーキテクトというシャレで神様と掛けています）が、システムに許す自由度のようなものです。

さて、神様はさておき、太郎君の友人、次郎君が太郎君の行動を予測する場合について考えます。このとき、次郎君は人間なので、事実としての太郎君の行動を完全に知ることはできません。神様と異なり、観測方程式はy(t) = H(X(t)) + w(t)という形になります。このとき、カルマンフィルタを用いることで、次郎君はw(t)に関する不確実性を利用して、X(t)の値を推定することができるようになります。これは、飽くまで次郎君がX(t)をじかに知ることができないため、発生する不確実性です。これが減っても、太郎君の自由意思が減る（？）わけではありません。

カルマンフィルタの仕組みが分かっている人にとっては当たり前のことを、なんでこんなに長々と説明するのかというと、私が勉強したとき、カルマンフィルタの動作のうち、どの部分が「統計的推測」になっているのかが、いまいち不明だったからです。ここで説明したように、システム方程式はノイズv(t)を含みますが、そもそも、状態であるX(t+1)はノイズを加味して計算されるため、そこには減少させることが可能な「誤差」はありません。v(t)はシステムの本質的な不確実性であり、カルマンフィルタによって減少させることのできる不確実性ではない、ということがいまいちピンと来ていませんでした。

システム方程式のv(t)が、たとえ自分が神様だとしても、減少させることのできないX(t)の不確実性を表現したものである一方、y(t)に付随する誤差w(t)は統計的観察によって減少させることができます。それにより、X(t+1)の推定値であるX^(t+1)と、システムの不確実性を織り込んだ実現値X(t+1)の誤差を減少させるのが、カルマンフィルタの仕事ということです。このX^(t+1)とX(t+1)は、全知全能の神様に関しては一致しますが、人間である我々にとっては常に一致するとは限りません——なぜならば、観察できるy(t)にw(t)という誤差が付きまとうから、我々にとって知りえるのはX(t+1)ではなく、実はX^(t+1)に過ぎないためです。

システム方程式と観測方程式の違い

よって、カルマンフィルタによって、何らかの統計的推測を行いたい場合、それはシステム方程式ではなく、観測方程式に入っていなければなりません。よく、時系列に説明変数を入れて回帰分析する、というようなモデルを見ますが、その手のモデルで決まって回帰の計算が観測方程式に含まれているのには、上記のような理由があると考えられます。はじめは、単に計算上、回帰係数βを状態X(t)に含めたいからそうしているだけなのかな？と思っていたのですが、そうではなくて、時変のARモデルなども同じで、通常のARとは定式化が完全に異なります。通常、ARプロセスそのものが隠れ状態x(t)として扱われますが、時変ARの場合、AR係数a0(t)...ai(t)が隠れ変数になります。

たとえば、AR過程について言うと、この二つの定式化の違いは、次のようなものになります。

• 通常のAR過程：AR係数aを所与として、x(t)には何らかの自由度を仮定する。そのうえで、x(t)の推定値x^(t)と真値x(t)の誤差を観測値y(t)とy^(t)の誤差w(t)を通して減少させるため、フィルタリングを行う。よって、^x(t)とx(t)の誤差に関する知識をy(t)とy^(t)の誤差によって得るが、システムノイズv(t)に相当する分散は、x(t)の本質的な自由度として残る。

• 時変ARの場合：x(t)を所与として、a(t)には何らかの自由度（この場合、時変であるということ）を仮定する。そのうえでa(t)の推定値a^(t)と真値a(t)との誤差を、観測値y(t)とy^(t)の誤差w(t)を通して減少させるため、フィルタリングを行う。a^(t)とa(t)の誤差に関する知識をy(t)とy^(t)の誤差によって得るが、システムノイズv(t)に相当する分散は、a(t)の本質的な自由度として残る。

つまり、最初の想定ではシステムがAR過程であるのに対して、後の想定では観測過程がAR過程であると言っていることになると思われます。ようするに、前者はプロセスがAR過程であるとき、xの不確実性をy(t)から評価している一方、後者はAR過程を観察しているという想定でaの不確実性をy(t)から評価しているということになります。よって、後者の場合AR過程「そのもの」に関しては何ら統計的推定は行われていませんし、尤度を計算した場合も、求められるのはAR過程のパラメータa(t)に関する尤度です。観測値y(t)には自由度がないので、y(t)が「厳密な」AR過程（語法矛盾か？）でなければ、推定結果は全然ダメということになり得ます。

神様がどうのと言う、安っぽいたとえ話をしたのは、この辺のイメージが自分で理解できなかったので、メモとして後で思いだしやすいようにするためです。

尤度の計算

ということで、カルマンフィルタの予測一期分の尤度は次のように計算されます（疑似コード）。ただし、実際のコードでは、この式で計算した尤度を時系列分掛け算すると（同時確率を求めるので、掛け算になる）数値誤差が発生してしまうので、対数化して足し算に変換しています（いわゆる対数尤度）。

1/np.sqrt(2*pi*np.det(S)) * exp(- (y - f)**2 / (2*np.det(S)))

＜追記＞
実際に実装していて、もうちょい詳しく調べたので追記します。尤度の計算ですが、実際に実装する場合、対数尤度の計算をするようです。そのため、各地点における予測値（時点tのY(t)でフィルタする前の予測値の方）をf、観測値をy、yの予測誤差の分散共分散行列をSとして、コードは下記のよう（な感じ、動作確認はしてない）になります。

e = y - f
likelihood[t] = np.log(np.det(S)) + e.T @ np.linalg.inv(S) @ e

で、最後に全体の尤度を計算する場合は、データ数をN、観測方程式の次元をydimとすると、次のような（感じ）になると思います。

likelihood_T = -0.5 * (ydim*N*np.log(2*np.pi) + np.sum(likelihood))

時系列全体の尤度なので、全部かけ合わせることになるのですが、そうするとアンダーフローを起こすので対数尤度の計算をします。実際の最適化をする場合、こいつを最小（尤度なんだから最大なんだけど、大抵、最適化ルーチンは「最小化」するので符号を逆にして最小化問題にする）にするパラメータをscipy.optimize.minimizeなんかで探すとよいようです（今やっているところ、もしかしたら記事にするかも）。定数のところで、データ数と観測モデルの次元数をかけているのは、（正規分布の）対数尤度の計算をするとき1/np.sqrt(2×np.pi)みたいなのが出てくるからです。logをとるとこいつが残るのですが、先ほど説明したように時系列の尤度は、yの各成分ごとの尤度をデータ点ごとに計算して全部掛ける、みたいな計算なのでこのようになるようです（掛け算はlogを取ると足し算なので、計N×ydim個分の定数項が出てきます）。そんな感じです。

＜ここまで＞

変数の意味は、サンプルコードと同じです。この式の形を見て、ピンとこない人もいるかもしれませんが、これは正規分布の確率密度関数と同じ形です。なぜかというと、カルマンフィルタを適用する際は、観測方程式の誤差w(t)が正規分布に従う（ガウス型）であると想定することが多いからです（小耳にはさんだのですが、不偏推定量が欲しいのでなければ、厳密にガウス分布でなくともよいようです。ただそうすると、フィルタにバイアスがかかったりとかするのかも。未調査）。ようするに、w(t)を正規分布と仮定して、観測値y(t)と予測値y^(t)の差がどのくらい「尤もらしい」かを計算しているだけです。分散は、カルマンフィルタで求めた分散共分散行列Sを使います。

この式を見てわかる通りxとかFについては、何ら統計的な推定は行っていません。だから、カルマンフィルタの尤度関数を求めよ、という場合、観測方程式の形だけわかればよく、システム方程式については気にしなくてよいのです。システム方程式の不確実性関しては、xの分散共分散行列と所与のシステムノイズQから順次計算されます。

結論

カルマンフィルタが、実際に「何を」フィルタしているのか？ピンとこないうすぼんやりさん（私のこと）向けに、私が分かっている範囲で日本語による説明を書きました。教科書を見ても、数式とわずかな説明があり、後は頑張れや！という感じだったので、あえてくどくどしい説明を書いてみました。まぁ、色々と勘違いしている可能性はあると思いますが、多少なりとも理解の助けになればと思います。