カルマンフィルタによる状態推定

2020年5月2日

はじめに

バネ-ダンパー-質点系について、カルマンフィルタによる状態推定を行う。

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

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

雑音がある場合の状態量の推定

オブザーバにより観測量からシステムの状態量を推定することができるが、一般的には観測量には雑音が乗る。ここでは 質点 2 の位置 (\(x_2\)) のみ観測できるとして、\(C = (0\ 1\ 0\ 0)^T\) としてオブザーバーを構成する。

C = np.array(C0).reshape(1, len(C0))
poles = [-3.+0.5j, -3.-0.5j, -1., -0.5]

bunch = signal.place_poles(A0.T, C.T, poles)
K0 = bunch.gain_matrix.T

Ah0 = A0 - K0.dot(C)

from scipy import linalg
print(linalg.eigvals(Ah0))

シミュレーション結果は以下のようになる。

from numpy.linalg import inv

n = len(x0)
A = expm(A0*dt)
Ah = expm(Ah0*dt)
K = (Ah - np.eye(n)).dot(inv(Ah0)).dot(K0)

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

x[0, :] = x0
xh[0, :] = xh0

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

    y = C.dot(x1)

    xh1[:] = Ah.dot(xh1) + K.dot(y)
    xh[step, :] = xh1

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, xh[:, 0], ".", label="$\hat{x}_1$")
plt.plot(t, xh[:, 1], ".", label="$\hat{x}_2$")
plt.plot(t, xh[:, 2], ".", label="$\hat{x}_3$")
plt.plot(t, xh[:, 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()

観測量に標準偏差 0.1 の正規性白色雑音が加法的に乗るとした場合は、以下のようになる。

np.random.seed(0)
sigma = 0.1

n = len(x0)
A = expm(A0*dt)
Ah = expm(Ah0*dt)
K = (Ah - np.eye(n)).dot(inv(Ah0)).dot(K0)

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

x[0, :] = x0
xh[0, :] = xh0
y[0] = C.dot(x0) + np.random.normal(0., sigma)

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

    y0 = C.dot(x1) + np.random.normal(0., sigma)
    y[step-1] = y0

    xh1[:] = Ah.dot(xh1) + K.dot(y0)
    xh[step, :] = xh1

y[step] = C.dot(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.plot(t, xh[:, 0], ".", label="$\hat{x}_1$")
plt.plot(t, xh[:, 1], ".", label="$\hat{x}_2$")
plt.plot(t, xh[:, 2], ".", label="$\hat{x}_3$")
plt.plot(t, xh[:, 3], ".", label="$\hat{x}_4$")
plt.plot(t, y, label="$y$")
plt.xlim(0., end_time)
plt.ylim(-0.5, 1.5)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend(ncol=2)
plt.show()

この場合、もろに雑音の影響を受けている。さらに、システム自体に雑音が乗る場合もある。標準偏差 0.02 の正規性白色雑音が加法的に乗る場合は以下のようになる。

np.random.seed(0)
sigma = 0.1
sigma2 = 0.02

n = len(x0)
A = expm(A0*dt)
Ah = expm(Ah0*dt)
K = (Ah - np.eye(n)).dot(inv(Ah0)).dot(K0)

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

x[0, :] = x0
xh[0, :] = xh0
y[0] = C.dot(x0) + np.random.normal(0., sigma)

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

    y0 = C.dot(x1) + np.random.normal(0., sigma)
    y[step] = y0

    xh1[:] = Ah.dot(xh1) + K.dot(y0)
    xh[step, :] = xh1

y[step] = C.dot(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.plot(t, xh[:, 0], ".", label="$\hat{x}_1$")
plt.plot(t, xh[:, 1], ".", label="$\hat{x}_2$")
plt.plot(t, xh[:, 2], ".", label="$\hat{x}_3$")
plt.plot(t, xh[:, 3], ".", label="$\hat{x}_4$")
plt.plot(t, y, label="$y$")
plt.xlim(0., end_time)
plt.ylim(-0.5, 1.5)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend(ncol=2)
plt.show()

このような場合にもなんとか状態量を推定しようとする仕組みがカルマンフィルタである。

カルマンフィルタ

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

$$x(k+1) = A(k) x(k) + w(k)$$ $$y(k+1) = C(k) x(k) + v(k)$$

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

細かい話は置いといて、離散時間カルマンフィルタは以下のように表される。

$$\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)$$

ここで、\(\hat{x}(k|k)\) は状態量の推定値、\(P(k|k)\) は推定誤差共分散行列である。\(K(k)\) はカルマンゲインと呼ばれる。\(Q\)、\(R\) はそれぞれシステム、観測の雑音の共分散行列である。

まず、雑音がない場合のシミュレーションを以下に示す。

n = len(x0)
A = expm(A0*dt)
C = np.array(C0).reshape(1, len(C0))

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

x[0, :] = x0
xh[0, :] = xh0

x1 = np.array(x0)
xh1 = np.array(xh0)
K = np.zeros((n, 1))
P = np.eye(n)
eps = 1e-15

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

    y = C.dot(x1)

    K[:, :] = P.dot(C.T.dot(inv(C.dot(P.dot(C.T)))))
    xh1[:] += K.dot(y - C.dot(xh1))
    xh[step, :] = xh1
    xh1[:] = A.dot(xh1)

    P[:, :] -= K.dot(C.dot(P))
    P[:, :] = A.dot(P.dot(A.T)) + 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, xh[:, 0], ".", label="$\hat{x}_1$")
plt.plot(t, xh[:, 1], ".", label="$\hat{x}_2$")
plt.plot(t, xh[:, 2], ".", label="$\hat{x}_3$")
plt.plot(t, xh[:, 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()

ばっちり推定できている。\(P(k+1|k)\) が 0 に近づいていくため、ゼロ割りを避けるために小さい値 (eps) を足している。

観測値に雑音がある場合を以下に示す。

np.random.seed(0)
sigma = 0.1
R = np.array([[sigma**2]])

n = len(x0)
A = expm(A0*dt)
C = np.array(C0).reshape(1, len(C0))

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

x[0, :] = x0
xh[0, :] = xh0
y[0] = C.dot(x0) + np.random.normal(0., sigma)

x1 = np.array(x0)
xh1 = np.array(xh0)
K = np.zeros((n, 1))
P = np.eye(n)
eps = 1e-15

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

    y0 = C.dot(x1) + np.random.normal(0., sigma)
    y[step] = y0

    K[:, :] = P.dot(C.T.dot(inv(C.dot(P.dot(C.T)) + R)))
    xh1[:] += K.dot(y0 - C.dot(xh1))
    xh[step, :] = xh1
    xh1[:] = A.dot(xh1)

    P[:, :] -= K.dot(C.dot(P))
    P[:, :] = A.dot(P.dot(A.T)) + 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, xh[:, 0], ".", label="$\hat{x}_1$")
plt.plot(t, xh[:, 1], ".", label="$\hat{x}_2$")
plt.plot(t, xh[:, 2], ".", label="$\hat{x}_3$")
plt.plot(t, xh[:, 3], ".", label="$\hat{x}_4$")
plt.plot(t, y, label="$y$")
plt.xlim(0., end_time)
plt.ylim(-0.5, 1.5)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend(ncol=2)
plt.show()

なんとか推定できている。推定値 \(\hat{x}_2\) は観測値 \(y\) を滑らかにした感じになっているので、これを平滑化 (smoothing) と見ることもできる。

さらにシステムに雑音がある場合は以下の通りである。

n = len(x0)

np.random.seed(0)
sigma = 0.1
sigma2 = 0.02
R = np.array([[sigma**2]])
Q = np.eye(n)*sigma2**2

A = expm(A0*dt)
C = np.array(C0).reshape(1, len(C0))

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

x[0, :] = x0
xh[0, :] = xh0
y[0] = C.dot(x0) + np.random.normal(0., sigma)

x1 = np.array(x0)
xh1 = np.array(xh0)
K = np.zeros((n, 1))
P = np.eye(n)
eps = 1e-15

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

    y0 = C.dot(x1) + np.random.normal(0., sigma)
    y[step] = y0

    K[:, :] = P.dot(C.T.dot(inv(C.dot(P.dot(C.T)) + R)))
    xh1[:] += K.dot(y0 - C.dot(xh1))
    xh[step, :] = xh1
    xh1[:] = A.dot(xh1)

    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, xh[:, 0], ".", label="$\hat{x}_1$")
plt.plot(t, xh[:, 1], ".", label="$\hat{x}_2$")
plt.plot(t, xh[:, 2], ".", label="$\hat{x}_3$")
plt.plot(t, xh[:, 3], ".", label="$\hat{x}_4$")
plt.plot(t, y, label="$y$")
plt.xlim(0., end_time)
plt.ylim(-0.5, 1.5)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend(ncol=2)
plt.show()

定常カルマンフィルタ

上の問題の場合、係数行列が定数である時不変システムであるため、定常カルマンフィルタが使える。

定常カルマンフィルタにおいて、推定誤差共分散行列の定常解 \(\hat{P}\) は以下の連続時間の代数リッカチ方程式 (ARE) の解として得られる。

$$A_0 \hat{P} + \hat{P} A_0^T - \hat{P} C^T R^{-1} C \hat{P} + Q = 0$$ SciPy にはこれを解く scipy.linalg.solve_continuous_are がある。ただし、以下の式が仮定されている。 $$A^T X + X A - X B R^{-1} B^T X + Q = 0$$ 
from scipy.linalg import solve_continuous_are

n = len(x0)

np.random.seed(0)
sigma = 0.1
sigma2 = 0.02
R = np.array([[sigma**2]])
Q = np.eye(n)*sigma2**2

A = expm(A0*dt)
C = np.array(C0).reshape(1, len(C0))

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

x[0, :] = x0
xh[0, :] = xh0
y[0] = C.dot(x0) + np.random.normal(0., sigma)

x1 = np.array(x0)
xh1 = np.array(xh0)

P = solve_continuous_are(A0.T, C.T, Q, R)
K = P.dot(C.T.dot(inv(C.dot(P.dot(C.T)) + R)))

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

    y0 = C.dot(x1) + np.random.normal(0., sigma)
    y[step] = y0

    xh1[:] += K.dot(y0 - C.dot(xh1))
    xh[step, :] = xh1
    xh1[:] = A.dot(xh1)

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, xh[:, 0], ".", label="$\hat{x}_1$")
plt.plot(t, xh[:, 1], ".", label="$\hat{x}_2$")
plt.plot(t, xh[:, 2], ".", label="$\hat{x}_3$")
plt.plot(t, xh[:, 3], ".", label="$\hat{x}_4$")
plt.plot(t, y, label="$y$")
plt.xlim(0., end_time)
plt.ylim(-0.5, 1.5)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend(ncol=2)
plt.show()

離散時間代数リッカチ方程式のソルバーもある。

from scipy.linalg import solve_discrete_are
P = solve_discrete_are(A.T, C.T, Q, R)

参考

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