カルマンフィルタの基礎 第6回
前回は状態変数が1次元の状態モデルでカルマンフィルタの実践を行った
今回は、状態変数は複数で、入力と出力は1つのSISOのカルマンフィルタを実装する
カルマンフィルタ(SISO)
今回は、下記の状態空間モデルを考える
カルマンフィルタの実装(ベクトル)
, をベクトル/スカラとして扱うか、すべて行列として扱うか、で実装が変わる
とりあえず、ベクトル/スカラとして扱う
import matplotlib.pyplot as plt import numpy as np import pandas as pd A_ = np.array([[0, -0.7], [1, -1.5]]) b_ = np.array([[0.5], [1.]]) c_ = np.array([[0.], [1.]]) sigma_nu2 = 1. sigma_omega2 = 0.1
def kf_siso(A_, b_, c_, sigma_nu2, sigma_omega2, y_, xhat, P_): xhatm = A_ @ xhat Pm = A_ @ P_ @ A_.T + sigma_nu2 * b_ @ b_.T g_ = Pm @ c_ / ((c_.T @ Pm @ c_)[0][0] + sigma_omega2) xhat_new = xhatm + g_ * (y_ - (c_.T @ xhatm)[0][0]) P_new = (np.eye(A_.shape[0]) - g_ @ c_.T) @ Pm return xhat_new, P_new, g_, Pm
数値シミュレーションの実施
N_ = 100 np.random.seed(0) v_ = np.random.randn() * np.sqrt(sigma_nu2) # システム雑音 w_ = np.random.randn() * np.sqrt(sigma_omega2) # 観測雑音 x_arr = [np.array([[0.], [0.]])] y_arr = [(c_.T @ x_arr[0])[0][0] + w_] xhat_arr = [np.array([[0.], [0.]])] P_arr = [np.array([[1., 0.], [0., 1.]])] for i in range(1, N_): v_ = np.random.randn() * np.sqrt(sigma_nu2) # システム雑音 w_ = np.random.randn() * np.sqrt(sigma_omega2) # 観測雑音 x_ = A_ @ x_arr[-1] + b_ * v_ y_ = (c_.T @ x_)[0][0] + w_ x_arr += [x_] y_arr += [y_] xhat, P_, g_, Pm = kf_siso( A_, b_, c_, sigma_nu2, sigma_omega2, y_, xhat_arr[-1], P_arr[-1]) xhat_arr += [xhat] P_arr += [P_] x_arr = np.array(x_arr) y_arr = np.array(y_arr) xhat_arr = np.array(xhat_arr) P_arr = np.array(P_arr)
シミュレーション結果の確認
fig = plt.figure(figsize=(6, 5)) ax1 = fig.add_subplot(2, 1, 1) ax1.plot(xhat_arr[:, 0, 0], ls='-', label='$\hat{x_1}$') ax1.plot(x_arr[:, 0, 0], ls='--', label='$x_1$') ax1.set_ylabel('$x_1$') ax1.grid(ls=':'); ax1.legend() ax2 = fig.add_subplot(2, 1, 2) ax2.plot(xhat_arr[:, 1, 0], ls='-', label='$\hat{x_2}$') ax2.plot(x_arr[:, 1, 0], ls='--', label='$x_2$') ax2.plot(y_arr, ls='-', alpha=0.6, label='$y$') ax2.set_xlabel('$k$') ax2.set_ylabel('$x_2$') ax2.grid(ls=':'); ax2.legend() fig.tight_layout()
観測できない を推測できていることが分かる
共分散行列の確認
最後に、事後共分散行列 を確認しておく
fig = plt.figure(figsize=(6, 5)) ax1 = fig.add_subplot(111) ax1.plot(P_arr[:, 0, 0], '.-', label='$P_{0,0}$') ax1.plot(P_arr[:, 0, 1], '.-', label='$P_{0,1}$') ax1.plot(P_arr[:, 1, 0], '.-', label='$P_{1,0}$') ax1.plot(P_arr[:, 1, 1], '.-', label='$P_{1,1}$') ax1.text(x=40, y=0.90, s='$P_{0,0}$ = %7f' % P_arr[:, 0, 0][-1]) ax1.text(x=40, y=0.84, s='$P_{0,1}$ = %7f' % P_arr[:, 0, 1][-1]) ax1.text(x=40, y=0.78, s='$P_{1,0}$ = %7f' % P_arr[:, 1, 0][-1]) ax1.text(x=40, y=0.72, s='$P_{1,1}$ = %7f' % P_arr[:, 1, 1][-1]) ax1.set_xlabel('$k$') ax1.set_ylabel('$P(k)$') ax1.grid(ls=':'); ax1.legend() fig.tight_layout()
カルマンフィルタ(MIMO)
, も行列として扱う
とりあえず、ベクトル/スカラとして扱う
また、入力と出力もベクトルにし、MIMOにも対応できるようにする
カルマンフィルタのアルゴリズム(MIMO)
状態空間モデルがMIMOの場合のカルマンフィルタのアルゴリズムを整理する
はシステム雑音の共分散行列
は観測雑音の共分散行列
カルマンフィルタの実装(行列)
A_ = np.array([[0, -0.7], [1, -1.5]]) B_ = np.array([[0.5], [1.]]) C_ = np.array([[0.], [1.]]) Q_ = np.array([[1.]]) R_ = np.array([[0.1]])
def kf(A_, B_, C_, Q_, R_, y_, xhat, P_): xhatm = A_ @ xhat Pm = A_ @ P_ @ A_.T + B_ @ Q_ @ B_.T G_ = Pm @ C_ @ (C_.T @ Pm @ C_ + R_)**-1 xhat_new = xhatm + G_ @ (y_ - C_.T @ xhatm) P_new = (np.eye(A_.shape[0]) - G_ @ C_.T) @ Pm return xhat_new, P_new, G_, Pm
数値シミュレーションの実施2
N_ = 100 np.random.seed(0) v_ = np.array([[np.random.randn() * np.sqrt(Q_[0][0])]]) # システム雑音 w_ = np.array([[np.random.randn() * np.sqrt(R_[0][0])]]) # 観測雑音 x_arr = [np.array([[0.], [0.]])] y_arr = [C_.T @ x_arr[0] + w_] xhat_arr = [np.array([[0.], [0.]])] P_arr = [np.array([[1., 0.], [0., 1.]])] for i in range(1, N_): v_ = np.array([[np.random.randn() * np.sqrt(Q_[0][0])]]) # システム雑音 w_ = np.array([[np.random.randn() * np.sqrt(R_[0][0])]]) # 観測雑音 x_ = A_ @ x_arr[-1] + B_ @ v_ y_ = C_.T @ x_ + w_ x_arr += [x_] y_arr += [y_] xhat, P_, G_, Pm = kf( A_, B_, C_, Q_, R_, y_, xhat_arr[-1], P_arr[-1]) xhat_arr += [xhat] P_arr += [P_] x_arr = np.array(x_arr) y_arr = np.array(y_arr) xhat_arr = np.array(xhat_arr) P_arr = np.array(P_arr)
シミュレーション結果の確認2
fig = plt.figure(figsize=(6, 5)) ax1 = fig.add_subplot(2, 1, 1) ax1.plot(xhat_arr[:, 0, 0], ls='-', label='$\hat{x_1}$') ax1.plot(x_arr[:, 0, 0], ls='--', label='$x_1$') ax1.set_ylabel('$x_1$') ax1.grid(ls=':'); ax1.legend() ax2 = fig.add_subplot(2, 1, 2) ax2.plot(xhat_arr[:, 1, 0], ls='-', label='$\hat{x_2}$') ax2.plot(x_arr[:, 1, 0], ls='--', label='$x_2$') ax2.plot(y_arr[:, 0, 0], ls='-', alpha=0.6, label='$y$') ax2.set_xlabel('$k$') ax2.set_ylabel('$x_2$') ax2.grid(ls=':'); ax2.legend() fig.tight_layout()
こちらでも同様に、観測できない を推測できていることが分かる
共分散行列の確認2
最後に、事後共分散行列 を確認しておく
fig = plt.figure(figsize=(6, 5)) ax1 = fig.add_subplot(111) ax1.plot(P_arr[:, 0, 0], '.-', label='$P_{0,0}$') ax1.plot(P_arr[:, 0, 1], '.-', label='$P_{0,1}$') ax1.plot(P_arr[:, 1, 0], '.-', label='$P_{1,0}$') ax1.plot(P_arr[:, 1, 1], '.-', label='$P_{1,1}$') ax1.text(x=40, y=0.90, s='$P_{0,0}$ = %7f' % P_arr[:, 0, 0][-1]) ax1.text(x=40, y=0.84, s='$P_{0,1}$ = %7f' % P_arr[:, 0, 1][-1]) ax1.text(x=40, y=0.78, s='$P_{1,0}$ = %7f' % P_arr[:, 1, 0][-1]) ax1.text(x=40, y=0.72, s='$P_{1,1}$ = %7f' % P_arr[:, 1, 1][-1]) ax1.set_xlabel('$k$') ax1.set_ylabel('$P(k)$') ax1.grid(ls=':'); ax1.legend() fig.tight_layout()
参考文献
この記事は以下の書籍を参考にしましたが、
私の拙い知識で書いておりますので、誤り等ありましたらご指摘ください