要点
-
@harmegiddoさんの以下の記事をもとに、トロッコ問題を例としたカルマンフィルタを実装していました（勉強のため）。
- 【カルマンフィルタの実装と理論】トロッコ問題で理解するカルマンフィルタ (実装済み)
- 初学者にも大変わかりやすくまとまっていました。超絶感謝。
- データ同化の計算ステップ内には以下の2つの分布が別々に計算されるのですが、もともとのコードではその区別が明確でないことが気になりました。
- 予測分布: t-1期までの情報から、t期の状態x_tの確率分布(平均と分散)を予想したもの
- フィルタ分布: 観測で入手されたt期の最新情報に基づき、予測分布を修正したもの
- フィルタ分布は予測分布と比べて1期分多くの情報に基づくため、推定精度の向上が期待される
- そこで、各タイムステップでの予測分布とフィルタ分布を明示したコードに書き換えて、予測分布とフィルタ分布の推定精度の差から、各タイムステップでの観測情報により推定精度が向上することを確認できるようにしてみました。
実行結果（コードはこの下です）
めんどくさかったんで軸名入れてませんが、横軸が時間、縦軸がトロッコの位置です。
また、緑が観測、黒が真値、青が予測分布に基づく推定値、赤がフィルタ分布に基づく推定値です。
これだけだとよくわからないので、真値に対する差も比べてみましょう。緑が観測と真値の差、青が予測分布に基づく推定値と真値の差、赤がフィルタ分布に基づく推定値と真値の差です。
青線と赤線を比べると、赤線の方がより小さい差となっていることがわかります。
つまり、観測前の予測分布に基づく推定値より、観測情報を加味したフィルタ分布に基づく推定値の方が精度が良い、ということですね。
最後にMSEによる比較も載せておきます。
やったぜ。
改造したコード
以下のコードはほとんど@harmegiddoさんのコードをベースにしています。
変更点としては、上述の点に加えて、いくつか行列の次元を修正したこと、外力項を導入したことです。
truck_prob_by_KF.py
# implementation of Kalman Filter for Truck problem
# original: https://qiita.com/g_rej55/items/618778dbb4bf286a985b
import numpy as np
import numpy.random as random
# generate norm
def norm(_loc=0.0, _scale=1.0, _size=(1)):
return random.normal(_loc, _scale, _size)
# --------------------------------------------------------- #
# universal setting
# --------------------------------------------------------- #
# time
global_time = 0
# set dt
dt = 0.1
# calc setting
calc_num = 500
end_time = calc_num * dt
# unit matrix
I = np.matrix([
[1, 0],
[0, 1]
])
# --------------------------------------------------------- #
# physical equations and settings
# --------------------------------------------------------- #
# kinematic equation
F = np.matrix([
[1, dt],
[0, 1]
])
# external force to state vector matrix
B = np.matrix([
[(dt**2) / 2],
[dt]
])
# external force component
f = np.array([0.01]) # hyp. of constant external force
# noize to state vector matrix (accidential acceralation)
G = np.matrix([
[(dt**2) / 2],
[dt]
])
# system noize parametres : v_t ~ N(mean_v, sigma_v)
mean_v = 0 # if dim(v_t) > 1, it becomes vector
sigma_v = 1 # if dim(v_t) > 1, it is co-var mtx.
# system noize covariance mtx
# note: cov = sigma**2 * G * G.T in the original article
# but because v_t is 1 dim, Q = sigma_v**2, not a co-ver mtx.
Q = (sigma_v**2)
# --------------------------------------------------------- #
# observation equations
# --------------------------------------------------------- #
# obs matrix
H = np.matrix([
1,
0
])
# obs noize : w_t ~ N(mean_w, sigma_w)
mean_w = 0 # if dim(w_t) > 1, it becomes vector
sigma_w = 1 # if dim(w_t) > 1, it becomes co-var mtx
# obs error covariance mtx or scalar
R = (sigma_w**2)
# --------------------------------------------------------- #
# set initial condition
# --------------------------------------------------------- #
# initial state vector
# note: ave and covmtx of x0/0 is known
x_true_t = np.matrix([
[0],
[0]
])
y_t = H * x_true_t + norm(mean_w, sigma_w)
# estimated dist of 1/0 can be calc.d from init cond.
x_ave_t_t = np.matrix([
[0],
[0]
])
V_t_t = np.matrix([
[0, 0],
[0, 0]
])
# for plotting
li_xtrue = []
li_obs_yt = []
li_estim_xavett1 = []
li_filter_xavett = []
li_t = []
# --------------------------------------------------------- #
# actual calculation
# --------------------------------------------------------- #
'''
term etc.
x_true_t : true value of state vector at t
ESTIMATION distribution: pdf of state vector x at time t
based on information 1 ~ t-1
x_ave_t_t1 : estimated value of state vector
as average of ESTIMATION distribution
V_t_t1 : estimated co-var mtx of ESTIMATION distribution
FILTER distribution: pdf of state vector x at time t
based on the information 1 ~ t
x_ave_t_t : estimated value of state vector
as average of FILTER distribution
V_t_t : estimated co-var mtx of FILTER distribution
'''
while global_time < end_time:
# 0. time count
global_time += dt
li_t.append(global_time)
# 1. renew the x_true_t
v_t = norm(mean_v, sigma_v)
# x_true_t = F * x_true_t + G * v_t # w/o external force f
x_true_t = F * x_true_t + G * v_t + B * f # with external force f
li_xtrue.append(x_true_t.tolist()[0][0])
# 2. estimated distribution of t/t-1
# x_ave_t_t, V_t_t are given (actually x_ave_t1_t1, V_t1_t1)
# F, Q, G are from model
# x_ave_t_t1 = F * x_ave_t_t # w/o external force f
x_ave_t_t1 = F * x_ave_t_t + B * f # with external force f
V_t_t1 = F * V_t_t * F.T + G * Q * G.T
li_estim_xavett1.append(x_ave_t_t1.tolist()[0][0])
# 3. observation
w_t = norm(mean_w, sigma_w)
y_t = H * x_true_t + w_t
li_obs_yt.append(y_t.tolist()[0][0])
# 4. filter distribution of t/t
# x_ave_t_t1, V_t_t1 and y_t are already calculated
# H, R are from model
K = V_t_t1 * H.T * np.linalg.inv(H * V_t_t1 * H.T + R) # R dim ?
x_ave_t_t = x_ave_t_t1 + K * (y_t - H * x_ave_t_t1)
V_t_t = V_t_t1 - K * H * V_t_t1
li_filter_xavett.append(x_ave_t_t.tolist()[0][0])
# --------------------------------------------------------- #
# performance check
# --------------------------------------------------------- #
MSE_obs = np.sum((np.array(li_obs_yt) - np.array(li_xtrue))**2)
MSE_estim = np.sum((np.array(li_estim_xavett1) - np.array(li_xtrue))**2)
MSE_filter = np.sum((np.array(li_filter_xavett) - np.array(li_xtrue))**2)
# if KF is effective, MSE_obs > MSE_estim > MSE_filter
# EOF.
参考文献
樋口知之編著「データ同化入門（シリーズ予測と発見の科学6）」