サイトマップ

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

制御工学 現代制御 カルマンフィルタ Python

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

今回は、状態と未知パラメータの同時推定をEKF/UKFで行う

状態と未知パラメータの同時推定

状態モデルと拡大系

下記のモデルにおいて、粘性係数 C が未知の場合を考える

\displaystyle \begin{aligned} \frac{d}{dt} \begin{bmatrix} x_1(t) \\ x_2(t) \\ \end{bmatrix} &= \begin{bmatrix} 0 & 1 \\ -\frac{K}{M} & -\frac{C}{M} \\ \end{bmatrix} \begin{bmatrix} x_1(t) \\ x_2(t) \\ \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{1}{M} \\ \end{bmatrix} u(t) \\ y(t) &= \begin{bmatrix} 1 & 0 \\ \end{bmatrix} \begin{bmatrix} x_1(t) \\ x_2(t) \\ \end{bmatrix} \\ M &= 2 \\ (C &= 1) \\ K &= 0.7 \\ \sigma_{\omega}^2 &= 0.1 \\ T &= 0.01 \\ \end{aligned}

C を状態変数に加えて、状態ベクトルを拡張する

\displaystyle \begin{aligned} \frac{d}{dt} \begin{bmatrix} x_1(t) \\ x_2(t) \\ C(t) \\ \end{bmatrix} &= \begin{bmatrix} x_2(t) \\ -\frac{K}{M} x_1(t) - \frac{C(t)}{M} x_2(t) \\ 0 \\ \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{1}{M} \\ 0 \\ \end{bmatrix} u(t) \\ y(t) &= \begin{bmatrix} 1 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} x_1(t) \\ x_2(t) \\ C(t) \\ \end{bmatrix} \\ \end{aligned}

入力 u(t) の設定

入力 u(t) として、下記のノコギリ波と正弦波の重ね合わせを設定する

import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
from scipy import signal

def u_(t_):
    return 4 * signal.sawtooth(t_ * np.sqrt(2)) + 10 * np.sin(t_)

times = np.linspace(0, 20, 2000+1)

fig = plt.figure(figsize=(6, 3))
ax1 = fig.add_subplot(1, 1, 1)

ax1.plot(times, u_(times))
ax1.set_ylabel('$u$')
ax1.grid(ls=':')
ax1.set_xlabel('$t$')
fig.tight_layout()

各パラメータと離散時間化の方法

状態モデルもパラメータは下記になる
（システムノイズは極小にしている）

K_ = 0.7
M_ = 2.
C_ = 1.
dT = 0.01

def dxdt(x_, t_):
    dx1 = x_[1][0]
    dx2 = - K_ / M_ * x_[0][0] - x_[2][0] / M_ * x_[1][0] + u_(t_) / M_
    dx3 = 0.
    return np.array([[dx1], [dx2], [dx3]])

B_ = np.array([[0.], [1. / M_], [0.]])

def h_(x_, t_=0.):
    return np.array([[x_[0][0]]])

Q_ = np.array([[1e-5]])
R_ = np.array([[0.1]])

このままだと、連続時間の微分方程式なので、
離散時間に変換することを考える
今回は、ルンゲ・クッタ法（4次）で変換を行う

def c2d_rk4(fc, dt, x_, t_):
    k1 = fc(x_, t_)
    k2 = fc(x_ + k1 * dt / 2., t_ + dt / 2.)
    k3 = fc(x_ + k2 * dt / 2., t_ + dt / 2.)
    k4 = fc(x_ + k3 * dt, t_ + dt)
    x_new = x_ + (k1 + 2.*k2 + 2.*k3 + k4) * dt / 6.
    return x_new

def f_rk4(x_, t_):
    return c2d_rk4(dxdt, dT, x_, t_)

カルマンフィルタの実装（EKF）

def Af(x_):
    A_ = np.zeros([3, 3])
    A_[0][0] = 1.
    A_[0][1] = dT
    A_[0][2] = 0.
    A_[1][0] = - K_ / M_ * dT
    A_[1][1] = 1. - x_[2] / M_ * dT
    A_[1][2] = - x_[1] / M_ * dT
    A_[2][0] = 0.
    A_[2][1] = 0.
    A_[2][2] = 1.
    return A_

def Ch(x_):
    return np.array([[1.], [0.], [0.]])

def ekf(f_, Af, B_, h_, Ch, Q_, R_, t_, y_, xhat, P_):
    xhatm = f_(xhat, t_)
    Am    = Af(xhat)
    Pm    = Am @ P_ @ Am.T + B_ @ Q_ @ B_.T

    C_       = Ch(xhatm)
    G_       = Pm @ C_ @ (C_.T @ Pm @ C_ + R_)**-1
    xhat_new = xhatm + G_ @ (y_ - h_(xhatm, t_))
    P_new    = (np.eye(Am.shape[0]) - G_ @ C_.T) @ Pm

    return xhat_new, P_new, G_, Pm

カルマンフィルタの実装（UKF）

def calc_Xm(x_, P_, kappa):
    n_ = x_.shape[0]
    x_ = x_.reshape(n_)
    L = np.linalg.cholesky(P_)

    Xm = [x_]
    for i in range(n_):
        mu = []
        for j in range(n_):
            mu += [x_[j] + np.sqrt(n_ + kappa) * L[j][i]]
        Xm += [mu]
    for i in range(n_):
        mu = []
        for j in range(n_):
            mu += [x_[j] - np.sqrt(n_ + kappa) * L[j][i]]
        Xm += [mu]
    Xm = np.array(Xm)
    Xm = Xm.reshape(2 * n_ + 1, n_, 1)
    return Xm

def calc_mean(Xm, kappa):
    n2 = Xm.shape[0]
    nl = Xm.shape[1]
    n_ = int((n2 - 1) / 2)
    Xm = Xm.reshape(n2, nl)
    w0 = kappa / (n_ + kappa)
    wi = 1. / (2 * (n_ + kappa))

    xhatm = []
    for i in range(nl):
        xhatm += [w0 * Xm[0][i]]
        for j in range(n_):
            xhatm[i] += wi * Xm[j + 1][i]
            xhatm[i] += wi * Xm[j + n_ + 1][i]
    xhatm = np.array(xhatm)
    xhatm = xhatm.reshape(nl, 1)
    return xhatm

def calc_Pm(Xm, Ym, kappa):
    xhatm = calc_mean(Xm, kappa)
    yhatm = calc_mean(Ym, kappa)
    Xm = Xm.reshape(Xm.shape[0], Xm.shape[1])
    Ym = Ym.reshape(Ym.shape[0], Ym.shape[1])
    xhatm = xhatm.reshape(xhatm.shape[0])
    yhatm = yhatm.reshape(yhatm.shape[0])
    l1 = Xm.shape[1]
    l2 = Ym.shape[1]
    n2 = Xm.shape[0]
    n_ = int((n2 - 1) / 2)
    w0 = kappa / (n_ + kappa)
    wi = 1. / (2 * (n_ + kappa))

    Pm = []
    for j in range(l1):
        Pm += [[]]
        for k in range(l2):
            Pm[j] += [w0 * (Xm[0][j] - xhatm[j]) * (Ym[0][k] - yhatm[k])]
            for i in range(2 * n_):
                Pm[j][k] += wi * (Xm[i + 1][j] - xhatm[j]) * (Ym[i + 1][k] - yhatm[k])
    return np.array(Pm)

def ut(f_, t_, x_, P_, kappa):
    X_    = calc_Xm(x_, P_, kappa)
    Xm    = np.array([f_(x_, t_) for x_ in X_])
    xhatm = calc_mean(Xm, kappa)
    Pm    = calc_Pm(Xm ,Xm, kappa)
    return X_, Xm, xhatm, Pm

def ukf(f_, h_, B_, Q_, R_, t_, y_, xhat, P_, kappa):
    X_, Xm, xhatm, Pxx = ut(f_, t_, xhat, P_, kappa)
    Pxx = Pxx + B_ @ Q_ @ B_.T

    Xp, Ym, yhatm, Pyy = ut(h_, t_, xhatm, Pxx, kappa)
    Pxy = calc_Pm(Xp, Ym, kappa)

    kG = Pxy @ (Pyy + R_)**-1
    xhat_new = xhatm + kG @ (y_ - yhatm)
    P_new = Pxx - kG @ Pxy.T

    return xhat_new, P_new, kG, Pxx

シミュレーションの実施

N_ = 2000
np.random.seed(200)
t_range = np.linspace(0, N_ * dT, N_ + 1)

w_ = np.array([[np.random.randn() * np.sqrt(R_[0][0])]])  # 観測雑音

x_arr = [np.array([[0.], [0.], [C_]])]
y_arr = [h_(x_arr[0]) + w_]

xhat_ekf_arr = [np.array([[0.], [0.], [0.1 * C_]])]
P_ekf_arr    = [np.diag([10., 10., 10.])]
G_ekf_arr    = []

xhat_ukf_arr = [np.array([[0.], [0.], [0.1 * C_]])]
P_ukf_arr    = [np.diag([10., 10., 10.])]
G_ukf_arr    = []

for t_ in t_range[1:]:
    w_ = np.array([[np.random.randn() * np.sqrt(R_[0][0])]])  # 観測雑音

    x_ = f_rk4(x_arr[-1], t_)
    y_ = h_(x_) + w_
    x_arr += [x_]
    y_arr += [y_]

    xhat, P_, G_, Pm = ekf(
        f_rk4, Af, B_, h_, Ch, Q_, R_, t_, y_, xhat_ekf_arr[-1], P_ekf_arr[-1])
    xhat_ekf_arr += [xhat]
    P_ekf_arr    += [P_]
    G_ekf_arr    += [G_]

    xhat, P_, G_, Pm = ukf(
        f_rk4, h_, B_, Q_, R_, t_, y_, xhat_ukf_arr[-1], P_ukf_arr[-1], kappa=0)
    xhat_ukf_arr += [xhat]
    P_ukf_arr    += [P_]
    G_ukf_arr    += [G_]

x_arr    = np.array(x_arr)
y_arr    = np.array(y_arr)

xhat_ekf_arr = np.array(xhat_ekf_arr)
P_ekf_arr    = np.array(P_ekf_arr)
G_ekf_arr    = np.array(G_ekf_arr)

xhat_ukf_arr = np.array(xhat_ukf_arr)
P_ukf_arr    = np.array(P_ukf_arr)
G_ukf_arr    = np.array(G_ukf_arr)

シミュレーションの確認

fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(3, 1, 1)
ax2 = fig.add_subplot(3, 1, 2, sharex=ax1)
ax3 = fig.add_subplot(3, 1, 3, sharex=ax1)

ax1.plot(t_range, xhat_ekf_arr[:, 0, 0], '--', label='$\hat{x_1}_{ekf}$')
ax1.plot(t_range, xhat_ukf_arr[:, 0, 0], '--', label='$\hat{x_1}_{ukf}$')
ax1.plot(t_range, x_arr[:, 0, 0], '-', label='$x_1$')
ax1.plot(t_range, y_arr[:, 0, 0], '-', label='$y$', alpha=0.3)
ax1.set_ylabel('$x_1$')
ax1.grid(ls=':'); ax1.legend()

ax2.plot(t_range, xhat_ekf_arr[:, 1, 0], '--', label='$\hat{x_2}_{ekf}$')
ax2.plot(t_range, xhat_ukf_arr[:, 1, 0], '--', label='$\hat{x_2}_{ukf}$')
ax2.plot(t_range, x_arr[:, 1, 0], '-', label='$x_2$')
ax2.set_ylabel('$x_2$')
ax2.grid(ls=':'); ax2.legend()

ax3.plot(t_range, xhat_ekf_arr[:, 2, 0], '--', label='$\hat{x_3}_{ekf}$')
ax3.plot(t_range, xhat_ukf_arr[:, 2, 0], '--', label='$\hat{x_3}_{ukf}$')
ax3.plot(t_range, x_arr[:, 2, 0], '-', label='$x_3$')
ax3.set_ylabel('$C$')
ax3.grid(ls=':'); ax3.legend()

ax2.set_xlabel('$k$')
fig.tight_layout()


C_ekf = xhat_ekf_arr[:, 2, 0][-100:].mean()
C_ukf = xhat_ukf_arr[:, 2, 0][-100:].mean()
print('C_ekf = %7.4f / C_ukf = %7.4f' % (C_ekf, C_ukf))

C_ekf =  0.9925 / C_ukf =  0.9949

EKFでも推定できているが、UKFの方が精度が良さそう

おまけ ルンゲ・クッタ法

今回使用した、連続時間の状態方程式を離散時間に変換するルンゲ・クッタ法の効果を確認する
比較対象として、オイラー法も確認する
さらに、単純なオイラー法だけでなく、分割数を増やした場合も比較する

下記の微分方程式を用いる
解析的にも解けるので、理想値とも比較する

\displaystyle \begin{aligned} \frac{d}{dt} x &= x \\ \frac{1}{x} dx &= dt \\ \ln(x) &= t \\ x &= \exp(t) \\ \end{aligned} 
dT = 0.1

def dxdt_test(x_, t_=0.):
    return x_

def c2d_rk4(fc, dt, x_, t_):
    k1 = fc(x_, t_)
    k2 = fc(x_ + k1 * dt / 2., t_ + dt / 2.)
    k3 = fc(x_ + k2 * dt / 2., t_ + dt / 2.)
    k4 = fc(x_ + k3 * dt, t_ + dt)
    x_new = x_ + (k1 + 2.*k2 + 2.*k3 + k4) * dt / 6.
    return x_new

def f_rk4(x_, t_):
    return c2d_rk4(dxdt_test, dT, x_, t_)

def f_div1(x_):
    return x_ + dxdt_test(x_) * dT

def f_div10(x_):
    ddT = dT / 10.
    for i in range(10):
        x_ = x_ + dxdt_test(x_) * ddT
    return x_

def f_div100(x_):
    ddT = dT / 100.
    for i in range(100):
        x_ = x_ + dxdt_test(x_) * ddT
    return x_

def f_div10000(x_):
    ddT = dT / 10000.
    for i in range(10000):
        x_ = x_ + dxdt_test(x_) * ddT
    return x_

N_ = 50

x_arr  = [1.]
x_arr1 = [1.]
x_arr2 = [1.]
x_arr3 = [1.]
x_arr4 = [1.]
x_arr5 = [1.]
t_arr  = [0.]
for i in range(N_):
    x_ = f_rk4(x_arr[-1], dT)
    x_arr += [x_]

    x_ = f_div1(x_arr2[-1])
    x_arr2 += [x_]

    x_ = f_div10(x_arr3[-1])
    x_arr3 += [x_]

    x_ = f_div100(x_arr4[-1])
    x_arr4 += [x_]

    x_ = f_div10000(x_arr5[-1])
    x_arr5 += [x_]

    t_arr += [t_arr[-1] + dT]

fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(1, 1, 1)

ax1.plot(t_arr, x_arr,  'o-', alpha=0.7, label='$rk4$')
ax1.plot(t_arr, x_arr2, 'o-', alpha=0.7, label='$div1$')
ax1.plot(t_arr, x_arr3, 'o-', alpha=0.7, label='$div10$')
ax1.plot(t_arr, x_arr4, 'o-', alpha=0.7, label='$div100$')
ax1.plot(t_arr, x_arr5, 'o-', alpha=0.7, label='$div10000$')
ax1.set_ylabel('$x$')
ax1.set_yscale('log')
ax1.grid(ls=':'); ax1.legend()

ax1.set_xlabel('$t$')
fig.tight_layout()

このグラフだと効果が分かりづらいので、
真値からの差分を真値で規格化して比較する

fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(1, 1, 1)

x_true = np.exp(np.array(t_arr))

ax1.plot(t_arr, (x_true - x_arr)  / x_true, 'o-', alpha=0.7, label='$rk4$')
ax1.plot(t_arr, (x_true - x_arr2) / x_true, 'o-', alpha=0.7, label='$div1$')
ax1.plot(t_arr, (x_true - x_arr3) / x_true, 'o-', alpha=0.7, label='$div10$')
ax1.plot(t_arr, (x_true - x_arr4) / x_true, 'o-', alpha=0.7, label='$div100$')
ax1.plot(t_arr, (x_true - x_arr5) / x_true, 'o-', alpha=0.7, label='$div10000$')
ax1.set_ylabel('$\~{x}/x$')
ax1.set_yscale('log')
ax1.grid(ls=':'); ax1.legend()

ax1.set_xlabel('$t$')
fig.tight_layout()

今回のモデルの場合、10,000分割よりもルンゲ・クッタの方が精度が良い
計算量としては、オイラー法の4倍程度
これまでは、数値計算する際に、適当に10分割ぐらいでやっていたが、
今後はルンゲ・クッタ法を活用していこうと思う

参考文献

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

カルマンフィルタの基礎

カルマンフィルタの基礎

Amazon