カルマンフィルタ

(3)

u_k-1 および v_k は、雑音ベクトルと呼ばれ、運動の揺らぎや観測系の雑音などを表します。z_k は、観測ベクトルと呼ばれ、実際にセンサで観測されたデータ（あるいはそれを変換したもの）を表します。信号源追跡では、信号源の状態 x_k の推定値を、式(1)と式(2)、および観測値 {z_1,...z_k}から求めます。

それでは、２つの方程式を、もう少しくわしく見ていきましょう。ターゲットが移動しない場合は、観測方程式(1)のみを用いて、信号源の位置を推定します。式(1)は、観測系（センサの種類や信号の種類）に依存します。一方、移動するターゲットに対して、観測方程式(1)を用いて信号源の位置を推定する場合は、観測値を短い区間（ブロック）に区切り、ブロック内では、ターゲットの移動は小さいと仮定して、式(1)を解くことになります。しかし、ブロック長をあまり長くすることができないため、方程式を解くためのデータ数が少なくなり、推定分散が大きくなるなど、推定の精度が低下する場合があります。また、信号源が休止したり、観測系に突発的な雑音が発生することにより、推定した運動の軌跡が大きく乱れることもあります。このため、カルマンフィルタでは、プロセス方程式(2)を新たに追加して、安定に軌跡を求められるよう工夫されています。式（2)は、運動のモデルと呼ばれ、ターゲットの移動をモデル化したものです。時刻 k と k-1 の関係を表していることから、１次のマルコフモデル (First-order Markov model)と呼ばれます。例えば、位置 θ_k について、式（2)の例を書いてみると、次式のようになります。

(4)

ここで、ΔT は、単位時間（時刻 k と k-1 の時間）を表します。この式は、等速度運動をしていることを表しています。図１は、等速度運動している信号源の例です。式(1)と(2)の役割は、カーナビにおけるジャイロセンサとGPSセンサの機能に例えるとわかりやすいでしょう。通常、カーナビで車の位置を求めるには、GPSの情報を使います。しかし、車がトンネルに入ってしまったような場合は、GPSの情報は使えません。そこで、カーナビでは、ジャイロセンサで車の加速度や速度を測り、式(4)と同様の方法で、トンネル中での車の位置を推定しています。つまり、観測方程式(2)→GPS、プロセス方程式(1)→ジャイロ、という役割分担です。

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

カルマンフィルタは、線形最小二乗平均法(Linear minimum mean square error, LMMSE)を用いた推定手法です。LMMSE法では、次式の左辺の事後確率を最大とするよう、状態ベクトルを推定します。この場合の事後確率とは、過去から現在までのデータ Z_1:k=[z₁,...,z_k] が与えられた後の状態ベクトル x_k の確率です。

(5)

式(5)の右辺は、左辺をベイズの定理によって、分解したものです。第２項の p(x_k|Z_1:k-1) は、1時刻前までのデータで現在の状態ベクトルを推定した場合の確率で、予測確率密度と呼ばれます。この項は、プロセス方程式(1)に基づいて、計算します。一方、第１項の p(z_k|x_k) は、状態ベクトルが与えられた場合のデータの確率であり、尤度(Likelihood)と呼ばれます。この項は、観測方程式(2)に基づいて計算します。第２項の計算を「予測」(Prediction)、第１項に第２項をかける計算を「更新」(Update)と呼びます。カルマンフィルタは、この予測と更新を各時刻ごとに交互に行いながら、推定値

を逐次的に計算するアルゴリズムです。

カルマンフィルタでは、計算を簡単にするため、式(5)の確率分布をガウス分布(Gaussian distribution)であると仮定しています。こうすることにより、確率密度全体を推定しなくても、平均値ベクトル（＝状態ベクトルの推定値

）と、次式で定義される共分散行列を更新すればよいことになります。

(6)

カルマンフィルタのアルゴリズムを表１にまとめます。導出や記号の詳細などは、参考文献[1]を参照してください。式(A1)と(A2)が上述の予測に、式(A4)と(A5)が更新のプロセスを表します。式(A3)のG_k はカルマンゲインと呼ばれます。・^Hはエルミート転置を表します（実数の場合は通常の転置・^Tと同じです）

(7)

このことから、運動のモデルを等速度運動とする場合は、F行列は次式のようになります。

(8)

H行列

H行列は、観測値と内部状態の関係を表します。例えば、電波や音波を発する信号源を観測する場合、信号源の位置と観測値とは、式(2)のような線形の関係にはありません。このような場合は、適当な他のアルゴリズムを使って観測値を事前に変換する必要があります。詳しくは、参考文献[1]を参照してください。この解説では、内部状態の一部（信号源の位置）に雑音が付加したものが直接観測できる場合を扱います。この場合、H行列は式(10)のようになります。

Q行列とK行列

Q行列とK行列は、雑音の共分散行列であり、次式で定義されます。

(9)

カルマンフィルタでは、 u_k-1 および v_k は、ガウス分布に従うと仮定します。通常、雑音の共分散行列は未知ですが、システムパラメタとして、適当な値を設定することにより、サンプルプログラムの例のように、式(1)と式(2)の重みを変えることができます。例えば、簡単な例として、式(10)のように単位行列に係数がかかった形とした場合を考えます。σ_u² を相対的に大きくした場合、プロセス方程式には、大きなパワー（分散）の雑音がのっていると仮定することになるため、信頼度が低いと考えられ、重みが低下します。この場合、観測方程式(2)が優先されます。逆に、σ_u² を相対的に小さくした場合、プロセス方程式の雑音が小さく、信頼度が高いため、運動のモデルを優先した軌跡推定が行われます。図３と４を比較してみてください。

サンプルプログラム

このサンプルには、FilterPy (https://filterpy.readthedocs.io/en/latest/index.html)のインストールが必要です。

このサンプルでは、軌跡の真値をサイン波とし、これにガウス雑音が乗ったものを観測値として、カルマンフィルタによる軌跡の推定を行います。カルマンフィルタの行列パラメタは、以下のようにしました。

(10)

図２はカルマンフィルタによる推定値、図３はカルマン平滑化(smoother)による推定値です。

"""

カルマンフィルタを用いたサイン波のトラッキング

Ver. 2.1, 2019/6/9, F.Asano

"""

import numpy as np

import matplotlib.pyplot as plt

from filterpy.kalman import KalmanFilter

# Parameters

N = 100 # サンプル数

ObservationNoiseVariance = 10.0 # 観測雑音の分散

TransitionNoiseVariance = 0.001 # プロセス雑音の分散

dim_x = 2 # 状態ベクトルの次元

dim_z = 1 # 観測値の次元

mode = 'filter'

#mode = 'smoother'

# 軌跡の真値

t = np.linspace(0, 3*np.pi, N)

s = np.sin(t) # 軌跡の真値

b = np.zeros(N-1)

for n in range(N-1):

b[n] = s[n+1]-s[n] # 速度の真値

# 観測値

rnd = np.random.RandomState(0)

u = 0.5*rnd.randn(N)

z = s + u # 観測値

# カルマンフィルタ

kf = KalmanFilter (dim_x=dim_x, dim_z=dim_z)

kf.H = np.array([[1, 0]])

kf.F = np.array([[1, 1], [0, 1]])

kf.R = ObservationNoiseVariance

kf.Q = TransitionNoiseVariance * np.eye(dim_x)

kf.P = np.eye(dim_x)

kf.x = np.array([[0.], # Position

[0.]]) # Velocity

if mode == 'filter':

xs = np.zeros((N,dim_x))

for n in range(N):

kf.predict()

kf.update(z[n])

xs[n,:] = kf.x.T

elif mode == 'smoother':

Hs = [kf.H for tt in range (N)]

Fs = [kf.F for tt in range (N)]

Rs = [kf.R for tt in range (N)]

Qs = [kf.Q for tt in range (N)]

(mu, cov, _, _) = kf.batch_filter(z, Rs=Rs, Fs=Fs, Hs=Hs, Qs=Qs,

Bs=None, us=None, update_first=False)

(xs, Ps, Ks, _) = kf.rts_smoother(mu, cov, Fs=Fs)

# 速度の推定値のグラフ

plt.figure(figsize=(10, 6))

plt.plot(t, s, color='m', label='true')

plt.scatter(t, z, marker='x', color='b', label='observation')

plt.plot(t, xs[:, 0], linestyle='-', marker='o', color='r',

label='estimate')

plt.legend(loc='lower right')

plt.xlim(xmin=0, xmax=t.max())

plt.xlabel('Time')

plt.title('Trajectory_' + mode)

# 速度の推定値のグラフ

plt.figure(figsize=(10, 6))

plt.plot(t[0:N-1], b, color='m', label='true')

plt.plot(t, xs[:, 1], linestyle='-', marker='o', color='g',

label='estimate')

plt.legend(loc='upper right')

plt.xlim(xmin=0, xmax=t.max())

plt.xlabel('Time')

plt.title('Velocity_' + mode)

plt.show()

参考文献

• 1. 浅野太、「音のアレイ信号処理」、コロナ社 2011

  2. S. Haykin, "Kalman filter and neural networks," Wiley Inter-science, 2001