buq’s blog

覚えておきたいけど覚えておけなさそうなことを書きます?

カルマンフィルタの導出

カルマンフィルタのベイズじゃない説明の意味がわからなかったので，ベイズっぽく導出した．それのメモ．

状態変数が $X_t$ であり，観測量が $Z_t$ がである次の離散時間システムを考える：

$X_t = AX_{t-1} + Bu_{t-1} + W_{t-1}$

$Z_t = HX_{t} + V_{t}$

ここで $u$ は系への入力であり， $w, v$ はそれぞれ平均が零，共分散行列が $Q, R$ である正規分布に従うノイズである．ノイズは全て独立とする．（確率変数と行列を大文字でかいてある．）

例えば $x$ が地表の点で， $A, H$ がともに単位行列であれば，場所の観測にノイズが乗っていて，かつ速度ベクトルを自分で決められるときの位置の推定問題である．

平均 $x_0$ , 分散 $P$ の確率変数 $X_0$ を初期値とする上記のシステムで， $p(X_1|Z_1)$ を求めることができればよい．すなわち， $Z_1 = z_1$ を観測したときの $X_1$ を求めればOK．

これは条件つき確率の定義に従って分母を無視することで簡単に計算できる． $p(X_1|Z_1) = p(X_1 = x_1 | Z_1 = z_1) \propto p(AX+Bu+W = x_1 \cap V = z_1-Hx_1)$
$AX+Bu+W$ と $V$ は独立な正規分布だから，確率は積をとればOK．定数項を上手に無視して， $x_1$ に関するところだけピックアップし，行列版の平方完成をすれば，条件付きの分布が正規分布となり，その平均および分散が

平均: $(\Pi^{-1} + H^\top R^{-1} H)^{-1} (\Pi^{-1}\xi + H^\top R^{-1} z_1)$
分散: $(\Pi^{-1} + H^\top R^{-1} H)^{-1}$

となることがわかる．ここで $\Pi = APA^{\top}+Q, \xi=Ax_0+Bu_0$ ．

おまけ：ここで1次元の問題を考えて， $A=H=1, P=p, Q=q, R=r$ とすると，

平均: $((p+q)^{-1} + r^{-1} )^{-1} ((p+q)^{-1}\xi + r^{-1} z_1)$
分散: $((p+q)^{-1} + r^{-1} )^{-1}$

平均は精度（分散の逆数）で重み付けした2つの推定の内分となり，精度は2つの推定の精度の和になる．