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

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

今回は、非線形カルマンフィルタを取り扱う
これまでは、線形カルマンフィルタで、状態モデルが $A$ や $C$ で表せられ、
$x(k+1)$ や $y$ が $x(k)$ に比例していた
今回は、単純な比例関係で表せない状態モデルでカルマンフィルタを適用する場合を考える

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

非線形カルマンフィルタ（EKF）

非線形モデルでのカルマンフィルタ（今回は、拡張カルマンフィルタ：EKF）をPythonで実装する

状態モデルが下記のように表せられる場合を考える

$\displaystyle \begin{aligned} x(k+1) &= f(x(k)) + B \nu(k) \\ y(k) &= h(x(k)) + \omega(k) \\ \end{aligned}$

$A$ や $C$ が存在しないため、これまでの線形カルマンフィルタは適用できない
そこで、関数 $f(x)$ と $h(x)$ を偏微分して、ヤコビアン行列を求めることで、
線形近似し、これまでの線形カルマンフィルタを適用することを考える

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

具体的には、下記のように $A(k-1)$ と $C^T(k)$ を逐次求めることで、処理を行う

$\displaystyle \begin{aligned} \hat{x}^-(k) &= f(\hat{x}(k-1)) \\ A(k-1) &= \left. \frac{\partial f}{\partial x} \right |_{x=\hat{x}(k-1)}, \quad C^T(k) = \left. \frac{\partial h}{\partial x} \right |_{x=\hat{x}^-(k)} \\ \\ P^-(k) &= A(k-1) P(k-1) A(k-1)^T + B Q B^T \\ \end{aligned}$

$\displaystyle \begin{aligned} G(k) &= P^-(k) C(k) (C^T(k) P^-(k) C(k) + R)^{^-1} \\ \hat{x}(k) &= \hat{x}^-(k) + G(k) (y(k) - h(\hat{x}^-(k))) \\ P(k) &= (I - G(k) C^T(k)) P^-(k) \\ \end{aligned}$

ヤコビアン

ここで、行列の偏微分の際に必要になるヤコビアン行列は下記のようになる

$\displaystyle \begin{aligned} x &= \begin{bmatrix} x_1 \\ x_2 \\ \end{bmatrix} \\ f(x) &= \begin{bmatrix} f_1 (x) \\ f_2 (x) \\ \end{bmatrix} \\ \\ \frac{\partial f(x)}{\partial x} &= \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2} \\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} \\ \end{bmatrix} \\ \end{aligned}$

非線形カルマンフィルタの実践（EKF）

実際に、下記の状態モデルで、EKFを実装してみる

$\displaystyle \begin{aligned} x(k+1) &= x(k) + 3 \cos(\frac{x}{10}) + \nu(k) \\ y(k) &= x^3(k) + \omega(k) \\ \end{aligned}$

$A(k-1)$ と $C^T(k)$ は下記のようになる

$\displaystyle \begin{aligned} A(k-1) &= \left. \frac{\partial f}{\partial x} \right |_{x=\hat{x}(k-1)} = 1 - \frac{3}{10} \sin(\frac{\hat{x}(k-1)}{10}) \\ C^T(k) &= \left. \frac{\partial h}{\partial x} \right |_{x=\hat{x}^-(k)} = 3 \{\hat{x}^-(k)\}^2 \\ \end{aligned}$

数値シミュレーション

初期値は $x(0) = 10$ で初めて、カルマンフィルタの初期値は $\hat{x}(0) = 11$ 、 $P(0)=1$ で動かす
システム雑音は $Q=1$ 、観測雑音は $R=100$ にする

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

def ekf(f_, Af, B_, h_, Ch, Q_, R_, y_, xhat, P_):
    xhatm = f_(xhat)
    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))
    P_new    = (np.eye(Am.shape[0]) - G_ @ C_.T) @ Pm

    return xhat_new, P_new, G_, Pm

def f_(xhat):
    return xhat + 3 * np.cos(xhat / 10)

def Af(xhat):
    return 1.0 - 3 / 10 * np.sin(xhat / 10)

B_ = np.array([[1.]])

def h_(xhatm):
    return xhatm**3

def Ch(xhatm):
    return 3 * xhatm**2

Q_ = np.array([[1.]])
R_ = np.array([[100.]])

N_ = 50
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([[10.]])]
y_arr = [h_(x_arr[0]) + w_]

xhat_arr = [np.array([[11.]])]
P_arr    = [np.array([[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_ = f_(x_arr[-1]) + B_ @ v_
    y_ = h_(x_) + w_
    x_arr += [x_]
    y_arr += [y_]

    xhat, P_, G_, Pm = ekf(
        f_, Af, B_, h_, Ch, 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)

fig = plt.figure(figsize=(6, 5))

ax1 = fig.add_subplot(2, 1, 1)
ax1.plot(y_arr[:, 0, 0]**(1/3), ls='-', alpha=0.6, label='$y^{1/3}$')
ax1.plot(xhat_arr[:, 0, 0], ls='-', label='$\hat{x}$')
ax1.plot(x_arr[:, 0, 0], ls='--', label='$x$')
ax1.set_ylabel('$x$')
ax1.grid(ls=':'); ax1.legend()

ax2 = fig.add_subplot(2, 1, 2)
ax2.plot(y_arr[:, 0, 0]**(1/3) - x_arr[:, 0, 0], ls='-', alpha=0.6, label='$y^{1/3}$')
ax2.plot(xhat_arr[:, 0, 0] - x_arr[:, 0, 0], ls='-', label='$\hat{x}$')
ax2.set_xlabel('$k$')
ax2.set_ylabel('$\tilde{x}$')
ax2.grid(ls=':'); ax2.legend()

fig.tight_layout()

パッと見だと、良く推定できているように見えるが、
観測値を変換した $y^{1/3}$ と比較すると、観測値の方が精度が良いことがわかる
観測雑音が $100$ となっているが、これが $x^3$ に対するノイズになるので、意外と小さい

数値シミュレーション2

観測雑音を $R=1,000,000$ と大きくする

def f_(xhat):
    return xhat + 3 * np.cos(xhat / 10)

def Af(xhat):
    return 1.0 - 3 / 10 * np.sin(xhat / 10)

B_ = np.array([[1.]])

def h_(xhatm):
    return xhatm**3

def Ch(xhatm):
    return 3 * xhatm**2

Q_ = np.array([[1.]])
R_ = np.array([[1000000.]])

N_ = 50
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([[10.]])]
y_arr = [h_(x_arr[0]) + w_]

xhat_arr = [np.array([[11.]])]
P_arr    = [np.array([[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_ = f_(x_arr[-1]) + B_ @ v_
    y_ = h_(x_) + w_
    x_arr += [x_]
    y_arr += [y_]

    xhat, P_, G_, Pm = ekf(
        f_, Af, B_, h_, Ch, 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)

fig = plt.figure(figsize=(6, 5))

ax1 = fig.add_subplot(2, 1, 1)
ax1.plot(y_arr[:, 0, 0]**(1/3), ls='-', alpha=0.6, label='$y^{1/3}$')
ax1.plot(xhat_arr[:, 0, 0], ls='-', label='$\hat{x}$')
ax1.plot(x_arr[:, 0, 0], ls='--', label='$x$')
ax1.set_ylabel('$x$')
ax1.grid(ls=':'); ax1.legend()

ax2 = fig.add_subplot(2, 1, 2)
ax2.plot(y_arr[:, 0, 0]**(1/3) - x_arr[:, 0, 0], ls='-', alpha=0.6, label='$y^{1/3}$')
ax2.plot(xhat_arr[:, 0, 0] - x_arr[:, 0, 0], ls='-', label='$\hat{x}$')
ax2.set_xlabel('$k$')
ax2.set_ylabel('$\tilde{x}$')
ax2.grid(ls=':'); ax2.legend()

fig.tight_layout()