カルマンフィルター（その２）

このページでは、カルマンフィルターのより一般的な表現について考えてみる。

行列によるモデルの表現

状態を$n$次元ベクトル $\boldsymbol{x}$ で表し、独立な$d$成分の揺らぎ$\boldsymbol{w}$が重畳されながら時間発展するような状態モデルを考える： $$ \boldsymbol{x}(t+\Delta) = F \boldsymbol{x}(t) + G \boldsymbol{w}(t) \tag{1} $$ ここで、$F$は$n$行$n$列の正方行列、$G$は$n$行$d$列の行列である。 揺らぎ$\boldsymbol{w}(t)$は時間的に無相関で、各成分の平均は0とし、共分散行列($d$行$d$列）を$Q$と置く: $$ Q_{ij}(t) = \left\langle w_i(t) w_j(t) \right\rangle \tag{2} $$

次に、$m$成分からなる観測量 $\boldsymbol{z}$を考え、$m$行$n$列の行列$H$によって状態ベクトル $\boldsymbol{x}$ を線形変換した値に $m$次元の誤差ベクトル$\boldsymbol{v}$を加えた観測モデル $$ \boldsymbol{z}(t) = H \boldsymbol{x}(t) + \boldsymbol{v}(t) \tag{3} $$ を考える。 ここで、誤差は時間的には独立で、平均は0、その共分散行列（$m$行$m$列）を$R$と置く: $$ R_{ij}(t) = \left\langle v_i(t) v_j(t) \right\rangle \tag{4} $$

トロッコ問題の例では $$ \boldsymbol{x}(t) = \begin{pmatrix} x(t)\\ v(t) \end{pmatrix} ,\ \ \ F = \begin{pmatrix} 1 & \Delta \\ 0 & 1 \\ \end{pmatrix} $$ $$ G = \begin{pmatrix} 0 \\ \sqrt{\Delta} \end{pmatrix} ,\ \ \ \boldsymbol{w}(t) = \begin{pmatrix} a(t) \end{pmatrix} $$ $$ H = \begin{pmatrix} 1 & 0 \end{pmatrix} ,\ \ \ \boldsymbol{v}(t) = \begin{pmatrix} b(t) \end{pmatrix} $$ にそれぞれ対応する。

カルマンフィルターのアルゴリズム

導出は専門書を参照することにして、上記のように行列で記述された状態モデルと観測モデルに対するカルマンフィルターの計算アルゴリズムは以下の通りとなる。

初期設定

状態ベクトル$\boldsymbol{x}$と分散行列$S$の初期状態を設定する。

カルマンゲインを求め、観測量をもとに予測値を修正する

観測ベクトル$\boldsymbol{z}(t)$、 状態ベクトルの予測値 $\hat{\boldsymbol{x}}(t|t-\Delta)$、および 状態モデルの共分散行列の予測値 $\hat{S}(t|t-\Delta)$ から、カルマンゲイン（$n$行$m$列の行列）を $$ K = \hat{S}(t|t-\Delta) H^\top \left(H \hat{S}(t|t-\Delta) H^\top + R(t) \right)^{-1} \tag{5} $$ から求める。

状態ベクトルと共分散行列を修正する

観測量をもとに値を修正する。 状態ベクトルは $$ \boldsymbol{\tilde{x}}(t) = \hat{\boldsymbol{x}}(t|t-\Delta) + K \left( \boldsymbol{z}(t) - H \hat{\boldsymbol{x}}(t|t-\Delta) \right) \tag{6} $$ 共分散については $$ \tilde{S}(t) = \left( I - K H \right) \hat{S}(t|t-\Delta) \tag{7} $$ となる（$I$は単位行列）。

$\Delta$先の予測値を求める

次のステップの状態ベクトルを $$ \hat{\boldsymbol{x}}(t+\Delta|t) = F \boldsymbol{\tilde{x}}(t) \tag{8} $$ で、共分散を $$ \hat{S}(t+\Delta|t) = F \tilde{S}(t) F^\top + G Q G^\top \tag{9} $$ で推定する。

icon-teacher　解説: カルマンゲインの導出

過去の状態から予測した状態ベクトルを$\hat{\boldsymbol{x}}$、現在の状態ベクトルを$\boldsymbol{x}$、 現在の観測ベクトルを$\boldsymbol{z}$とすると、 現在の状態ベクトルと、カルマンゲイン$K$を使って修正された予測ベクトルとの距離（スカラー量）の平均 $$ d(K) = \left\langle \| \boldsymbol{x} - \left( \hat{\boldsymbol{x}} + K (\boldsymbol{z} - H \hat{\boldsymbol{x}}) \right) \| \right\rangle \tag{10} $$ が最小となるように$K$を決めればよい。

ここで二乗距離 $$ d(K) = \left\langle \left( \boldsymbol{x} - \left( \hat{\boldsymbol{x}} + K (\boldsymbol{z} - H \hat{\boldsymbol{x}}) \right) \right)^\top \left( \boldsymbol{x} - \left( \hat{\boldsymbol{x}} + K (\boldsymbol{z} - H \hat{\boldsymbol{x}}) \right) \right) \right\rangle \tag{11} $$ を採用し、観測モデル $\boldsymbol{z} = H \boldsymbol{x} + \boldsymbol{v}$ を代入して、式を整理する。

次いで、 $$ \frac{\partial d(K)}{\partial K_{ij}} = 0 \tag{12} $$ となるような$K$を求めればよい。

観測誤差ベクトルの各成分は平均0で他の量（$\boldsymbol{X}$とする）と独立であるから、 $$ \left\langle \boldsymbol{X} \cdot \boldsymbol{v} \right\rangle = \left\langle \boldsymbol{X} \right\rangle \cdot \left\langle \boldsymbol{v} \right\rangle = 0 \tag{13} $$ となること、定義から $$ \hat{S} = \left\langle (\boldsymbol{x} - \hat{\boldsymbol{x}}) (\boldsymbol{x} - \hat{\boldsymbol{x}})^\top \right\rangle \tag{14} $$ $$ R = \left\langle \boldsymbol{v} \boldsymbol{v}^\top \right\rangle \tag{15} $$ を用いて、式(12) を整理する。

その際に、行列の微分についての関係式 $$ \frac{\partial \left( (K \boldsymbol{a})^\top K \boldsymbol{a} \right)}{\partial K} = 2 K \boldsymbol{a} \boldsymbol{a}^\top $$ $$ \frac{\partial \left(\boldsymbol{a}^\top K \boldsymbol{b} \right)}{\partial K} = \boldsymbol{a} \boldsymbol{b}^\top $$ $$ \frac{\partial f(K)}{\partial K^\top} = \left( \frac{\partial f(K)}{\partial K} \right)^\top $$ を用いると、式(12)は $$ -2 \hat{S} H^\top + 2 K \left( H \hat{S} H^\top + R \right) = 0 $$ とまとめられるから、右から $H \hat{S} H^\top + R$ の逆行列を作用させると、(5)式のカルマンゲインを得る。

icon-pc　練習： 共分散行列

カルマンゲインを使って、補正された共分散行列 $$ \tilde{S} = \left\langle (\boldsymbol{x} - \tilde{\boldsymbol{x}}) (\boldsymbol{x} - \tilde{\boldsymbol{x}})^\top \right\rangle $$ を与える式(7)を確認してみなさい。

NumPyを使った実装の例

行列とベクトルによる表式で記述した「トロッコ問題」をNumPyを使ってPythonに直訳してみた例を以下に示す： 

# coding: utf-8

import random
import math
import numpy as np
import matplotlib.pyplot as plt

dt = 0.1
F = np.array([[1, dt],
              [0,1]])
G = np.array([[0],
              [math.sqrt(dt)]])
H = np.array([[1,0]])

x = np.zeros((2,1))
xt = np.zeros((2,1))

S = np.zeros((2,2))

R = np.array([[1.0]])
Q = np.array([[1.0]])

x[0,0]=0
x[1,0]=1

xt[0,0]=-1
xt[1,0]=2

S[0,0]=1
S[1,1]=1

T=[]
X=[]
XT=[]
ZT=[]

t=0
while t<10:
    D = np.linalg.pinv(H.dot(S.dot(H.T)) + R)
    K = S.dot(H.T).dot(D)

    # observation
    zt = H.dot(x) + np.random.multivariate_normal([0], Q, 1).T

    x2 = xt + K.dot(zt - H.dot(xt))
    S2 = (np.eye(2) - K.dot(H)).dot(S)

    T.append(t)
    X.append(x[0,0])
    XT.append(x2[0,0])
    ZT.append(zt[0,0])

    xt = F.dot(x2)
    S = F.dot(S2.dot(F.T)) + G.dot(Q.dot(G.T))
    
    # update state
    x = F.dot(x) + G.dot( np.random.multivariate_normal([0], R, 1).T )

    t = t + dt        

plt.plot(T,X, color='blue', linewidth=1.0, label='State Model')
plt.plot(T,XT, 'o', color='orange', linewidth=1.0, label='Estimation')
plt.plot(T,ZT, '*',color='green',label='Obvervation')
# plt.errorbar(T,XT, yerr=XTERR, capsize=5, fmt='o', color='red', ecolor='orange',label='Estimate')
plt.xlabel('T')
plt.ylabel('X')
plt.grid(True)
plt.legend()
plt.show()