カルマンフィルタによるシステム同定

2020年5月30日

はじめに

バネ-ダンパー-質点系について、カルマンフィルタによるシステム同定を行う。

バネ-ダンパー-質点系のシミュレーション

2 自由度のバネ-ダンパー-質点系を線形システム状態方程式として解く。SymPy を用いてラグランジュの手法で運動方程式を導出し、線形化する。

from sympy import symbols, diff, Matrix, lambdify
from sympy.physics.mechanics import dynamicsymbols
from sympy.physics.mechanics import ReferenceFrame, Point, Particle
from sympy.physics.mechanics import LagrangesMethod, Lagrangian

t = symbols("t")
z1, z2, f1, f2 = dynamicsymbols("z1 z2 f1 f2")
m1, m2, c1, c2, k1, k2 = symbols("m1 m2 c1 c2 k1 k2")
q = Matrix([z1, z2])

N = ReferenceFrame("N")

p1 = Point("p1")
v1 = z1.diff(t)
p1.set_vel(N, v1*N.x)
pa1 = Particle("pa1", p1, m1)
pa1.potential_energy = k1*z1**2/2 + k2*(z2 - z1)**2/2

p2 = Point("p2")
v2 = z2.diff(t)
p2.set_vel(N, v2*N.x)
pa2 = Particle("pa2", p2, m2)

F = c1*v1**2/2 + c2*(v2 - v1)**2/2
fc1 = -F.diff(v1)
fc2 = -F.diff(v2)

fl = [(p1, (f1 + fc1)*N.x), (p2, (f2 + fc2)*N.x)]

L = Lagrangian(N, pa1, pa2)
LM = LagrangesMethod(L, q, forcelist = fl, frame = N)

LM.form_lagranges_equations()
As, Bs, u = LM.linearize(q_ind=q, qd_ind=q.diff(t), A_and_B=True)

具体的な数値を設定する。

import numpy as np

m1n = 1.
m2n = 1.
c1n = 0.5
c2n = 0.5
k1n = 0.2
k2n = 0.2

As_func = lambdify((m1, m2, c1, c2, k1, k2), As, modules="numpy")
A0 = As_func(m1n, m2n, c1n, c2n, k1n, k2n)

Bs_func = lambdify((m1, m2), Bs, modules="numpy")
B0 = Bs_func(m1n, m2n)

C0 = [0., 1., 0., 0.]
x0 = [0.5, 1., 0., 0.]

dt = 0.1
end_time = 10.
t = np.linspace(0., end_time, int(end_time/dt) + 1)

線形システム状態方程式をシミュレートする。

from scipy.linalg import expm
from matplotlib import pyplot as plt
import seaborn as sns
sns.set()

n = len(x0)
A = expm(A0*dt)

steps = int(end_time/dt) + 1
x = np.zeros((steps, n))

x[0, :] = x0

x1 = np.array(x0)
for step in range(1, steps):
    x1[:] = A.dot(x1)
    x[step, :] = x1

plt.plot(t, x[:, 0], label="$x_1$")
plt.plot(t, x[:, 1], label="$x_2$")
plt.plot(t, x[:, 2], label="$x_3$")
plt.plot(t, x[:, 3], label="$x_4$")
plt.xlim(0., end_time)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend()
plt.show()

減衰係数の推定

線形システム状態方程式を次式で表す。

$$\dot{x}(t) = A_0 x(t) + B_0 u(t) + w(t)$$ $$y_0(t) = C_0 x(t) + v(t)$$

\(x(t)\) は状態量、\(y_0(t)\) は観測量、\(w(t)\) はシステムの雑音、\(v(t)\) は観測の雑音である。

値を設定する前のシステム行列は次のようになっている。

$$A_0 = \left[\begin{matrix}0 & 0 & 1 & 0\\0 & 0 & 0 & 1\\\frac{- k_{1} - k_{2}}{m_{1}} & \frac{k_{2}}{m_{1}} & \frac{- c_{1} - c_{2}}{m_{1}} & \frac{c_{2}}{m_{1}}\\\frac{k_{2}}{m_{2}} & - \frac{k_{2}}{m_{2}} & \frac{c_{2}}{m_{2}} & - \frac{c_{2}}{m_{2}}\end{matrix}\right]$$

これの要素を測定値から推定する。システムのパラメータを推定することをシステム同定という。ここでは、\(m_1 = m_2 = 1\) として、粘性係数 \(c_1\)、\(c_2\) を推定するために、要素 \(a_{33}\)、\(a_{34}\) の推定を目指す。\(a_{43}\)、\(a_{44}\) については、\(a_{34}\) から計算する。

システム行列を既知の部分と未知の部分に分ける。

$$A_0 = A_k + A_u$$

未知の要素をベクトル \(\theta\) で表す。たとえば \(\theta = [a_{33}\ a_{34}]^T\) などとする。次式を満たす \(X\) を考える。

$$A_u x = X \theta$$

\(X\) を次式で表す。

$$X = \sum_{i=1}^n x_i M_i$$

たとえば、\(A_u\) が次のようであるとする。

$$A_u = \displaystyle \left[\begin{matrix}0 & 0 & 0 & 0\\0 & 0 & 0 & 0\\0 & 0 & \theta_1 & \theta_2\\0 & 0 & 0 & 0\end{matrix}\right]$$

この場合、\(X\) は次のように表される。

$$X = x_3 \left[\begin{matrix}0 & 0\\0 & 0\\1 & 0\\0 & 0\end{matrix}\right] + x_4 \left[\begin{matrix}0 & 0\\0 & 0\\0 & 1\\0 & 0\end{matrix}\right]$$

右辺の 2 つのマトリックスがそれぞれ \(M_3\)、\(M_4\) に当たる。

\(A_k\)、\(M_i\) を作るコードは次のようになる。

index = [
    (2, 2),
    (2, 3),
]

n = len(x0)
p = len(index)

Au = np.zeros((n, n))
for i, j in index:
    Au[i, j] = A0[i, j]

Ak = A0 - Au

M0 = np.empty(shape=n, dtype=object)
for i in range(n):
    M = np.zeros((n, p))
    for j, idx in enumerate(index):
        if idx[1] == i:
            M[idx[0], j] = 1.
    M0[i] = M

システム方程式は次のように書ける。

$$\dot{x}(t) = A_k x(t) + X(t) \theta(t) + B_0 u(t) + w(t)$$

これを以下の拡大システム方程式として表す。

$$\dot{z}(t) = A_{e0}(t) z(t) + B_{e0} u_e(t) + w_e(t)$$ $$A_{e0}(t) = \left[\begin{matrix}A_k & X(t)\\0 & 0\end{matrix}\right]$$ $$B_{e0} = \left[\begin{matrix}B_0\\0\end{matrix}\right]$$

ここで \(z(t) = [x^T(t)\ \theta^T(t)]^T\)、\(u_e(t) = [u^T(t)\ 0]^T\)、\(w_e(t) = [w^T(t)\ w_\theta^T(t)]^T\) (\(w_\theta\) は \(\theta\) の変動) である。要はシステムのパラメータの一部を状態変数のように扱うのである。

未知パラメータ \(\theta\) についての擬似観測量を次式で表す。

$$y_p = C_p(t) \theta + v_p(t)$$ $$c(t) = C_0^{\dagger} \int_0^t y_0(\tau) d\tau$$ $$C_p(t) = \sum_i^n c_i(t) M_i$$

ここで \(C_0^{\dagger}\) は \(C_0\) の擬似逆行列である。これと本来の観測量で拡大した観測量を表す。

$$y(t) = C(t) z(t) + v_e(t)$$ $$C(t) = \left[\begin{matrix}C_0 & 0\\0 & C_p\end{matrix}\right]$$

ここで \(y(t) = [y_0^T(t)\ y_p^T(t)]^T\)、\(v_e(t) = [v^T(t)\ v_p^T(t)]^T\) である。

以上を離散時間時間方程式に変換して離散時間カルマンフィルタを適用し、\(\theta\) を推定する。

離散時間カルマンフィルタは以下のように表される。

$$\hat{x}(k|k) = \hat{x}(k|k-1) + K(k)\{y(k) - C(k) \hat{x}(k|k-1)\}$$ $$K(k) = P(k|k-1) C^T(k) \{C(k) P(k|k-1) C^T(k) + R(k)\}^{-1}$$ $$\hat{x}(k+1|k) = A(k) \hat{x}(k|k)$$ $$P(k|k) = P(k|k-1) - K(k) C(k) P(k|k-1)$$ $$P(k+1|k) = A(k) P(k|k) A^T(k) + Q(k)$$

シミュレーションは以下の通りである。状態変数はすべて観測できるものと仮定している。\(A_k\) の値を毎回更新している。

from scipy.linalg import expm
from numpy.linalg import pinv

m = 4
C0 = np.eye(m, n)

np.random.seed(0)
sigma = 0.1
sigma2 = 0.02
R = np.zeros((m+n, m+n))
R[:m, :m] = np.eye(m)*sigma**2
R[m:, m:] = np.eye(n)
Q = np.zeros((n+p, n+p))
Q[:n, :n] = np.eye(n)*sigma2**2
Q[n:, n:] = np.eye(p)*sigma2**2

An = expm(A0*dt)
Cn = C0

steps = int(end_time/dt) + 1
x = np.zeros((steps, n))
zh = np.zeros((steps, n+p))
zh0 = np.zeros(n+p)

x[0, :] = x0
zh[0, :] = zh0

x1 = np.array(x[0, :])
zh1 = np.array(zh[0, :])
K = np.zeros((n+p, m+n))
P = np.eye(n+p)
eps = 1e-15

yn = Cn.dot(x1) + np.random.normal(0., sigma)
yn0 = yn
int_yn = np.zeros(m)

for step in range(1, steps):
    x1[:] = An.dot(x1) + np.random.normal(0., sigma2, n)
    x[step, :] = x1

    yn = Cn.dot(x1) + np.random.normal(0., sigma)
    int_yn += 0.5*(yn + yn0)*dt
    yn0 = yn

    Cp = np.zeros((n, p))
    Ci = pinv(Cn)
    c = Ci.dot(int_yn)
    for i in range(n):
        Cp[:, :] = c[i]*M0[i]

    C = np.zeros((m+n, n+p))
    C[:m, :n] = Cn
    C[m:, n:] = Cp

    y = np.zeros(m+n)
    y[:m] = yn
    y[m:] = Cp.dot(zh1[n:])

    K[:, :] = P.dot(C.T.dot(pinv(C.dot(P.dot(C.T)) + R)))
    zh1[:] += K.dot(y - C.dot(zh1))
    zh[step, :] = zh1

    Ak[3, 2] = zh1[5]
    Ak[3, 3] = -zh1[5]

    X = np.zeros((n, p))
    for i in range(n):
        X[:, :] += zh1[i]*M0[i]

    Ae0 = np.zeros((n+p, n+p))
    Ae0[:n, :n] = Ak
    Ae0[:n, n:] = X

    A = expm(Ae0*dt)

    zh1[:] = A.dot(zh1)

    P[:, :] -= K.dot(C.dot(P))
    P[:, :] = A.dot(P.dot(A.T)) + Q + eps

plt.plot(t, x[:, 0], label="$x_1$")
plt.plot(t, x[:, 1], label="$x_2$")
plt.plot(t, x[:, 2], label="$x_3$")
plt.plot(t, x[:, 3], label="$x_4$")
plt.plot(t, zh[:, 0], ".", label="$\hat{x}_1$")
plt.plot(t, zh[:, 1], ".", label="$\hat{x}_2$")
plt.plot(t, zh[:, 2], ".", label="$\hat{x}_3$")
plt.plot(t, zh[:, 3], ".", label="$\hat{x}_4$")
plt.xlim(0., end_time)
plt.ylim(-0.5, 1.5)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend(ncol=2)
plt.show()

\(\theta\) の要素の推移を以下に示す。

plt.plot([t[0], t[-1]], [A0[2, 2], A0[2, 2]], "-", label="$a_{33}$")
plt.plot([t[0], t[-1]], [A0[2, 3], A0[2, 3]], "-", label="$a_{34}$")
plt.plot(t, zh[:, 4], ".", label="$\hat{a}_{33}$")
plt.plot(t, zh[:, 5], ".", label="$\hat{a}_{34}$")
plt.xlim(0., end_time)
plt.ylim(-1.5, 1.5)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend(ncol=2)
plt.show()

\(\theta\) の値から推定された粘性係数は以下の通りである。

theta1 = np.average(zh[-10:, 4])
theta2 = np.average(zh[-10:, 5])
c1e = -theta1 - theta2
c2e = theta2

print("c1 =", c1e, "( true =", c1n, "error =", (c1e - c1n)/c1n, ")")
print("c2 =", c2e, "( true =", c2n, "error =", (c2e - c2n)/c2n, ")")

結果

c1 = 0.3919556254596114 ( true = 0.5 error = -0.21608874908077724 )
c2 = 0.557163338737101 ( true = 0.5 error = 0.114326677474202 )

そこそこの値を推定できている。

メモ

推定する要素を変えたり、時間刻み幅を変えたりすると、結果が結構変わることがある。

バネ定数と減衰係数の推定

同様に、バネ定数と減衰係数を両方推定してみる。index を次のようにする。

index = [
     (2, 0),
     (2, 1),
     (2, 2),
     (2, 3),
]

\(A_k\) の更新部分は次のようになる。

    Ak[3, 0] = zh1[5]
    Ak[3, 1] = -zh1[5]
    Ak[3, 2] = zh1[7]
    Ak[3, 3] = -zh1[7]

\(\theta\) の要素の推移は以下の通りである。

plt.plot([t[0], t[-1]], [A0[2, 0], A0[2, 0]], "-", label="$a_{31}$")
plt.plot([t[0], t[-1]], [A0[2, 1], A0[2, 1]], "-", label="$a_{32}$")
plt.plot([t[0], t[-1]], [A0[2, 2], A0[2, 2]], "-", label="$a_{33}$")
plt.plot([t[0], t[-1]], [A0[2, 3], A0[2, 3]], "-", label="$a_{34}$")
plt.plot(t, zh[:, 4], ".", label="$\hat{a}_{31}$")
plt.plot(t, zh[:, 5], ".", label="$\hat{a}_{32}$")
plt.plot(t, zh[:, 6], ".", label="$\hat{a}_{33}$")
plt.plot(t, zh[:, 7], ".", label="$\hat{a}_{34}$")
plt.xlim(0., end_time)
plt.ylim(-1.5, 2.5)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend(ncol=2)
plt.show()

バネ定数と粘性係数の推定値は以下のようになる。

theta1 = np.average(zh[-10:, 4])
theta2 = np.average(zh[-10:, 5])
theta3 = np.average(zh[-10:, 6])
theta4 = np.average(zh[-10:, 7])
k1e = -theta1 - theta2
k2e = theta2
c1e = -theta3 - theta4
c2e = theta4

print("k1 =", k1e, "( true =", k1n, "error =", (k1e - k1n)/k1n, ")")
print("k2 =", k2e, "( true =", k2n, "error =", (k2e - k2n)/k2n, ")")
print("c1 =", c1e, "( true =", c1n, "error =", (c1e - c1n)/c1n, ")")
print("c2 =", c2e, "( true =", c2n, "error =", (c2e - c2n)/c2n, ")")

結果

k1 = 0.3276984026711226 ( true = 0.2 error = 0.6384920133556129 )
k2 = 0.13853014656558035 ( true = 0.2 error = -0.30734926717209826 )
c1 = 0.5980450821646159 ( true = 0.5 error = 0.19609016432923188 )
c2 = 0.4992934497844175 ( true = 0.5 error = -0.0014131004311650486 )

まあまあの値を推定している。

参考

  • 大住晃: 構造物のシステム制御, 森北出版 (2013).
  • 大住晃, 亀山健太郎, 松田吉隆: カルマンフィルタとシステムの同定, 森北出版 (2016).