julialang でカルマンフィルタを書く

最近カルマンフィルタの勉強を少ししており、行列を含む数式を素直にプログラムできる言語があるといいなと感じていました。Python を長年書いてきたものの、毎回 numpy.array を用意したり numpy 関数を呼んだりするのが少し面倒になってきました。

そこで今回は、scientific computation に強みがあると言われ、最近 v1.0 の出た Julia 言語（julialang）を使って簡単なカルマンフィルタを実装してみたいと思います。julialang は数日前に使い始めたばかりなので、パフォーマンス最適化などは考えずにやっていきます。

カルマンフィルタとは

確率システム理論の分野では、次のような設定で問題を考えることがあります：関心のあるシステム A があり、それを観測する機構 B によって、間接的にシステムの状態を知ることができる。特に、A は確率的にゆらぎ（ノイズ）をもったダイナミクスに従い、観測機構 B に対してもノイズが入る状況を考える。

この時、観測機構 B から得られたデータ $y$ に基づきシステム A の状態量 $x$ を推定するのですが、特に $x$ および $y$ の時間発展が線形方程式で記述できる場合の推定手法をカルマンフィルタと言います。一例としては惑星探査機の軌道推定や生体分子のダイナミクス観測等があり、現代制御理論の金字塔として広範な分野に適用されてきた手法です。

まずはグラフから

まずカルマンフィルタが何をやっているのか目で見てわかるようにするため、Julia による数値計算の結果を出してみます。
f:id:yoshidabenjiro:20181008013512p:plain
この図は、システム A としてある粒子の位置 $x$ と速度 $v$ を考えた時、システムダイナミクスの解 $x$ (solution)、観測量 $y$ (observation)、システムにノイズが入っていないとした場合の解 (deteministic)、そしてカルマンフィルタによる最適推定値 $\hat x$ (estimation) の4系列をプロットしたものです。 $x$ と $y$ については実際に系のダイナミクスを数値的に解いて時系列を生成しました。

カルマンフィルタを使ってやりたいのは、いま手元に観測量（黄色の系列）があったとして、生の状態（水色の系列）をできるだけ精度良く復元することです。その結果が緑色の系列ということになります。これを見ると、観測量、かなり系の状態から逸脱した値をとってますね。一方で推定された状態量はそこそこ生の状態量と近そうな感じです*1。

カルマンフィルタの式

システムダイナミクス

今回は、（離散ではなく）連続時間を扱い、微小時間区間を $dt$ として、次のような確率微分方程式でシステムを記述します：
\begin{align}
dx(t) = Ax(t)dt + Gdw(t),\\
dy(t) = Hx(t)dt + Rdv(t).\\
\end{align}ここで $x$ はシステム A の状態を表すベクトル量、 $y$ は観測量で、 $A, G, H, R$ は適当な次元の行列です（一般には時間依存していてもよいですが、今回は非依存としておきます）。また、 $dw$ および $dv$ は分散が $dt$ のウィーナ過程（ガウシアンノイズ）です。 $w, v$ は独立で、それぞれの共分散は $Q, I$ であるとしましょう。（本当は伊藤積分の考え方や解の存在条件についても書くべきだとは思うのですが、ここではスキップです。）

少し仰々しく見えるかもしれないですが、まず、どちらの式も $x$ の一次の項だけを含んでいるので線形な系で、各々第一項が系の確定的な振る舞いを記述しており、第二項によってノイズが追加されているという感じです。数値計算にあたっては、第二項を単に正規分布に従う乱数として書くことになります。

カルマンフィルタ

この時、データ $y$ に基づいた $x$ の最適な推定値 $\hat x$ と推定誤差を表す分散行列 $P$ の時間発展は次のように書けます：
\begin{align}
d\hat x = A\hat x dt + PH^T(RR^T)^{-1}(dy-H\hat x dt),\\
\dot P = AP + PA^T + GQG^T - PH^T(RR^T)^{-1}HP\\
\end{align}これがカルマンフィルタです。一見複雑なのですが、 $\hat x$ についての式をよく見てみると、まず第一項は $\hat x$ が自分自身に比例するという確定的な振る舞いを表しています。そこに第二項が加わっており、これは推定の誤差である $P$ に比例しているため、精度良く推定ができている状態では第一項に対して補正をあまり加えない、という妥当な挙動が読み取れます。

この記事では、パラメータはこんな感じで設定します*2：
\begin{equation}
A=\begin{pmatrix} 0 & 1\\-1.5 & -1\end{pmatrix}, G=\begin{pmatrix} 0\\0.45\end{pmatrix}, H=\begin{pmatrix}1 & 0\end{pmatrix}, R=0.3.
\end{equation}システムの状態は2次元で、物体の位置と速度に対応する量を考える設定です。

Julia による数値計算

さて、本題です。Julia で 1. サンプル（システムおよび観測量）系列を生成、2. 観測量から最適推定値を計算するカルマンフィルタ、の2つを実装しました。そのコア部分をここに書いていきます。

サンプル系列の生成

まず、サンプル系列の生成、つまりシステムダイナミクスを解くコードは、こんな感じです。

dw() = randn()*sqrt(dt) # ノイズを表現する関数を定義
dv() = randn()*sqrt(dt)

x = zeros(n_steps, dim_x)
y = zeros(n_steps, dim_y)
x[1, :] = x0 # インデックスは1から始まる
y[1, :] = y0

for i in 1:(n_steps-1)
    dx = A*x[i, :]*dt + G*dw() # 行列×ベクトルの積
    dy = H*x[i, :]*dt + R*dv()
    x[i+1, :] = x[i, :] + dx
    y[i+1, :] = y[i, :] + dy
end

Julia での関数定義は function ブロックを使って（Python の def のように）実行できますが、上にあるように、f(x) = x みたいに数式っぽく書くこともできます。dw() は引数なし、ということですね。

また Julia では特別なライブラリを使わなくとも、デフォルトで zeros (全ての要素が 0 の行列を生成) や randn (正規乱数を生成) といった関数が使えます。

さらに、* が行列と行列（ベクトル含む）の積を表す点が地味に便利で、例えば numpy を使った場合よりも記述がかなりきれいになると思います（numpy だと * は要素ごとの積となるため、行列×行列とか行列×ベクトルの積は np.dot を使ったりしますね）。

そして、ループ計算が速い、という点が何よりも特長で、上のように何も考えずにループを入れてしまってもいったん問題ない、というところは Python と大きく異なりますね。

カルマンフィルタの実装

for i in 1:(n_steps-1)
    cur_x = x[i, :]
    cur_P = P[i, :, :]
    dx = A*cur_x*dt + cur_P*H'*inv(R*R')*(dy[i, :] - H*cur_x*dt)
    P_dot = A*cur_P + cur_P*A' + G*G' - cur_P*H'*inv(R*R')*H*cur_P
    x[i+1, :] = cur_x + dx
    P[i+1, :, :] = cur_P + P_dot .* dt
end

が実装のコア部分です。H' は行列 $H$ の転置（正確には随伴）、inv は逆行列を求める関数です。上の方に書いたカルマンフィルタの式と見比べると、ほぼそのまま記述されていることが分かりますよね。行列計算等に必要な処理はネイティブ実装されているおかげで、かなり直感的なコーディングができるようになっている感じがします。

そのほか

サンプル系列の生成からカルマンフィルタの計算、結果の図を保存するまでを一貫してできるコードを
blog/kalman_filter at master · dlnp2/blog · GitHub
に置きました。いちおう、観測量を与えればカルマンフィルタが実行されるような関数を定義してあるので、ライブラリとして使えなくはないと思います。ただし今回の記事向けに定数行列をモジュール内定義しているので、適宜変更が必要です。

本当は Python でも同様のコードを書いて比較するべきだったのですが、力尽きたので止めました。。

*1:なお、ピンクの系列はノイズが無いとした際の位置の変動を表しており、確かに不規則な変動の入っていないきれいな曲線になっています。値はゼロに向かって減衰振動しているように見えますね。実際、この系列を支配している常微分方程式は \begin{equation} \ddot x + \dot x + 1.5x = 0 \end{equation}で、解は $x \propto \exp(-\frac{t}{2})\exp(i\omega t)$ ( $\omega$ はある定数) となるため、ある周期で振動しており、その振幅は時間が 2 を超えると急激に小さくなっていきます。

*2:『確率システム入門』（大住晃）6.4節より。ちなみにこの本は知名度高くないかもしれないですが、確率過程解析の基礎からカルマンフィルタ、最適制御まで丁寧に書かれており、演習問題と解答もついているので、大変おすすめです。

luggage baggage

Machine learning, data analysis, web technologies and things around me.