前回 : カルマンフィルタの基礎第6回 - ma-hi-ro Hobby

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

今回は、制御入力がある場合のカルマンフィルタを実装する

カルマンフィルタの基礎第7回
- カルマンフィルタ（制御入力あり）
- 参考文献

カルマンフィルタ（制御入力あり）

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

$\displaystyle \begin{aligned} x(k+1) &= A x(k) + B_u u(k) + B \nu(k) \\ y(k) &= C^T x(k) + \omega(k) \\ \end{aligned}$

カルマンフィルタのアルゴリズム（制御入力あり）

前回との違いは、事前状態推定値 $\hat{x}^-(k)$ を求める際に、制御入力の影響も加えるだけ

$\displaystyle \begin{aligned} \hat{x}^-(k) &= A \hat{x}(k-1) + B_u u(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}$

カルマンフィルタの実装

制御入力は、±1 をランダムに入力する

$\displaystyle \begin{aligned} A &= \begin{bmatrix} 0 & -0.7 \\ 1 & -1.5 \\ \end{bmatrix} \\ B_u &= B = \begin{bmatrix} 0.5 \\ 1 \\ \end{bmatrix} \\ C^T &= \begin{bmatrix} 0 & 1 \end{bmatrix} \\ Q & = 0.01 \\ R & = 0.1 \\ \end{aligned}$

import matplotlib.pyplot as plt
import numpy as np
import pandas as pd

A_ = np.array([[0, -0.7], [1, -1.5]])
Bu = np.array([[0.5], [1.]])
B_ = np.array([[0.5], [1.]])
C_ = np.array([[0.], [1.]])
Q_ = np.array([[0.01]])
R_ = np.array([[0.1]])

def kf(A_, Bu, B_, C_, Q_, R_, u_, y_, xhat, P_):
    xhatm = A_ @ xhat + Bu @ u_
    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

数値シミュレーションの実施

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])]])  # 観測雑音
u_arr = []

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])]])  # 観測雑音
    u_ = np.array([[np.sign(np.random.randn())]])             # 制御入力
    u_arr += [u_]

    x_ = A_ @ x_arr[-1] + Bu @ u_ + B_ @ v_
    y_ = C_.T @ x_ + w_
    x_arr += [x_]
    y_arr += [y_]

    xhat, P_, G_, Pm = kf(
        A_, Bu, B_, C_, Q_, R_, u_, y_, xhat_arr[-1], P_arr[-1])
    xhat_arr += [xhat]
    P_arr    += [P_]

u_arr    = np.array(u_arr)
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(3, 1, 1)
ax1.step(np.arange(1, N_), u_arr[:, 0, 0], where='pre', ls='-', label='$u$')
ax1.set_ylabel('$u$')
ax1.grid(ls=':'); ax1.legend()

ax2 = fig.add_subplot(3, 1, 2)
ax2.plot(xhat_arr[:, 0, 0], ls='-', label='$\hat{x_1}$')
ax2.plot(x_arr[:, 0, 0], ls='--', label='$x_1$')
ax2.set_ylabel('$x_1$')
ax2.grid(ls=':'); ax2.legend()

ax3 = fig.add_subplot(3, 1, 3)
ax3.plot(xhat_arr[:, 1, 0], ls='-', label='$\hat{x_2}$')
ax3.plot(x_arr[:, 1, 0], ls='--', label='$x_2$')
ax3.plot(y_arr[:, 0, 0], ls='-', alpha=0.6, label='$y$')
ax3.set_xlabel('$k$')
ax3.set_ylabel('$x_2$')
ax3.grid(ls=':'); ax3.legend()

fig.tight_layout()

入力が入っても、推定できていることが分かる

共分散行列の確認

最後に、事後共分散行列 $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()