サイトマップ

カルマンフィルタの基礎 第6回

カルマンフィルタの基礎 第6回

前回は状態変数が1次元の状態モデルでカルマンフィルタの実践を行った
今回は、状態変数は複数で、入力と出力は1つのSISOのカルマンフィルタを実装する

カルマンフィルタ(SISO)

今回は、下記の状態空間モデルを考える

 \displaystyle
\begin{aligned}
x_{k+1} &= A x_k + b \nu_k \\
y_k &= c^T x_k + \omega_k \\
\end{aligned}
 \displaystyle
\begin{aligned}
A &= \begin{bmatrix}
  0  & -0.7 \\
  1  & -1.5 \\
\end{bmatrix} \\
b &= \begin{bmatrix}
  0.5 \\
  1   \\
\end{bmatrix} \\
c^T &= \begin{bmatrix}
  0  & 1
\end{bmatrix} \\
Q & = \sigma_\nu^2 = 1 \\
R & = \sigma_\omega^2 = 0.1 \\
\end{aligned}

カルマンフィルタの実装(ベクトル)

 Q ,  R をベクトル/スカラとして扱うか、すべて行列として扱うか、で実装が変わる
とりあえず、ベクトル/スカラとして扱う

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()

観測できない  x_1 を推測できていることが分かる

共分散行列の確認

最後に、事後共分散行列  P(k) を確認しておく

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)

 Q ,  R も行列として扱う
とりあえず、ベクトル/スカラとして扱う
また、入力と出力もベクトルにし、MIMOにも対応できるようにする

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

状態空間モデルがMIMOの場合のカルマンフィルタのアルゴリズムを整理する
 Q はシステム雑音の共分散行列  E[\nu \nu^T]
 R は観測雑音の共分散行列  E[\omega \omega^T]

 \displaystyle
\begin{aligned}
x(k+1) &= A x(k) + B \nu (k) \\
y(k) &= C^T x(k) + \omega (k)  
\end{aligned}
 \displaystyle
\begin{aligned}
\hat{x}^-(k) &= A \hat{x}(k-1) \\
P^-(k) &= A P(k-1) A^T + B Q B^T \\
\end{aligned}
 \displaystyle
\begin{aligned}
g(k) &= P^-(k) C (C^T P^-(k) C + R)^{^-1} \\
\hat{x}(k) &= \hat{x}^-(k) + g(k) (y(k) - C^T \hat{x}^-(k)) \\
P(k) &= (I - g(k) C^T) P^-(k) \\
\end{aligned}

カルマンフィルタの実装(行列)

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()

こちらでも同様に、観測できない  x_1 を推測できていることが分かる

共分散行列の確認2

最後に、事後共分散行列  P(k) を確認しておく

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()

参考文献

この記事は以下の書籍を参考にしましたが、
私の拙い知識で書いておりますので、誤り等ありましたらご指摘ください