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

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

前回のU変換を用いて、UKF（Unscented Kalman Filter）のアルゴリズムを実装する
また、線形の状態モデルで動作の確認も行う

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

UKFのアルゴリズム

まずは、UFKのアルゴリズムを整理する

推定値と共分散行列の初期値は下記のように定める

$\displaystyle \begin{aligned} \hat{x}(0) &= E[x(0)] = x_0 \\ P(0) &= E[(x(0) - x_0)(x(0) - x_0)^T] = \Sigma_0 \\ \end{aligned}$

シグマポイントの計算

最初に、前回の推定値 $\hat{x}(k-1)$ と共分散行列 $P(k-1)$ から、
2n + 1個のシグマポイント $X_i$ を計算する

$\displaystyle \begin{aligned} X_0 (k-1) &= \hat{x}(k-1) \\ X_i (k-1) &= \hat{x}(k-1) + \sqrt{n + \kappa} \left( \sqrt{P(k-1)} \right)_i, \quad i = 1, 2, \ldots, n \\ X_{n+i} (k-1) &= \hat{x}(k-1) - \sqrt{n + \kappa} \left( \sqrt{P(k-1)} \right)_i, \quad i = 1, 2, \ldots, n \\ \end{aligned}$

各成分の重みは下記とする
（ $\kappa$ がパラメータになる）

$\displaystyle \omega_0 = \frac{\kappa}{n + \kappa}, \quad \omega_i = \frac{1}{2(n + \kappa)}, \quad i = 1, 2, \ldots, 2n$

予測ステップ

シグマポイントの遷移

生成したシグマポイントを状態モデルの $f(x)$ から遷移させる

$\displaystyle X^-_i(k) = f( X_i(k-1) ), \quad i = 0, 1, \ldots, 2n$

事前状態推定値

遷移後のシグマポイントから事前推定値 $\hat{x}^-(k)$ を求める

$\displaystyle \hat{x}^-(k) = \sum_{i=0}^{2n} \omega_i X^-_i(k)$

事前誤差共分散行列

同様に、事前（誤差）共分散行列 $P^-(k)$ を求める

$\displaystyle P^-(k) = \sum_{i=0}^{2n} \omega_i \{ X^-_i(k) - \hat{x}^-(k) \} \{ X^-_i(k) - \hat{x}^-(k) \}^T + \sigma_\nu^2 b b^T$

シグマポイントの再計算

求めた事前推定値 $\hat{x}^-(k)$ と事前（誤差）共分散行列 $P^-(k)$ から
シグマポイント $X^+_i(k)$ を計算する

$\displaystyle \begin{aligned} X^+_0 (k) &= \hat{x}^-(k) \\ X^+_i (k) &= \hat{x}^-(k) + \sqrt{n + \kappa} \left( \sqrt{P^-(k)} \right)_i, \quad i = 1, 2, \ldots, n \\ X^+_{n+i} (k) &= \hat{x}^-(k) - \sqrt{n + \kappa} \left( \sqrt{P^-(k)} \right)_i, \quad i = 1, 2, \ldots, n \\ \end{aligned}$

出力のシグマポイントの更新

次に、状態モデルの出力関数 $h(x)$ から、出力のシグマポイント $Y^-_i(k)$ を計算する

$\displaystyle Y^-_i (k) = h( X^+_i (k) ), \quad i = 0, 1, \ldots, 2n$

事前出力推定値

$Y^-_i(k)$ から事前出力推定値 $\hat{y}^-(k)$ を求める

$\displaystyle \hat{y}^-(k) = \sum_{i=0}^{2n} \omega_i Y_i^-(k)$

事前出力誤差共分散行列

同様に、出力 $y$ に関する事前誤差共分散行列 $P_{yy}^-(k)$ を求める

$\displaystyle P_{yy}^-(k) = \sum_{i=0}^{2n} \omega_i \{ Y^-_i(k) - \hat{y}^-(k) \}^2$

事前状態・出力誤差共分散行列

$X^+_i (k)$ と $Y^-_i(k)$ の共分散行列 $P_{xy}^-(k)$ を求める

$\displaystyle P_{xy}^-(k) = \sum_{i=0}^{2n} \omega_i \{ X^+_i(k) - \hat{x}^-(k) \} \{ Y^-_i(k) - \hat{y}^-(k) \}$

カルマンゲイン

$P_{xy}^-(k)$ 、 $P_{yy}^-(k)$ からカルマンゲインを求める

$\displaystyle g(k) = \frac{P_{xy}^-(k)}{P_{yy}^-(k) + \sigma_\omega^2}$

フィルタリングステップ

状態推定値

観測値 $y(k)$ を加えて、事後推定値 $\hat{x}(k)$ を求める

$\displaystyle \hat{x}(k) = \hat{x}^-(k) + g(k) \{ y(k) - \hat{y}^-(k) \}$

事後誤差共分散行列

次回のために、事後誤差共分散行列 $P(k)$ を求める

$\displaystyle P(k) = P^-(k) - g(k) ( P_{xy}^-(k) )^T$

UKFの実践（線形モデル）

実際にUKFの動作を確認する
状態モデルは、第6回と同じものを使用する

$\displaystyle \begin{aligned} x_{k+1} &= A x_k + B \nu_k \\ y_k &= C^T x_k + \omega_k \\ \end{aligned}$

$\displaystyle \begin{aligned} A &= \begin{bmatrix} 0 & -0.7 \\ 1 & -1.5 \\ \end{bmatrix} \\ B &= \begin{bmatrix} 0.5 \\ 1 \\ \end{bmatrix} \\ C^T &= \begin{bmatrix} 0 & 1 \end{bmatrix} \\ Q & = \sigma_\nu^2 = 1 \\ R & = \sigma_\omega^2 = 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]])
B_ = np.array([[0.5], [1.]])
C_ = np.array([[0.], [1.]])
Q_ = np.array([[1.]])
R_ = np.array([[0.1]])

def f_(x_):
    return A_ @ x_

def h_(x_):
    return C_.T @ x_

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

単純な線形カルマンフィルタと比較する

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

カルマンフィルタの実装（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_, x_, P_, kappa):
    X_    = calc_Xm(x_, P_, kappa)
    Xm    = np.array([f_(x_) 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_, y_, xhat, P_, kappa):
    X_, Xm, xhatm, Pxx = ut(f_, xhat, P_, kappa)
    Pxx = Pxx + B_ @ Q_ @ B_.T

    Xp, Ym, yhatm, Pyy = ut(h_, 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_ = 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])]])  # 観測雑音

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

xhat_kf_arr = [np.array([[0.], [0.]])]
P_kf_arr    = [np.array([[1., 0.], [0., 1.]])]
G_kf_arr    = []

xhat_ukf_arr = [np.array([[0.], [0.]])]
P_ukf_arr    = [np.array([[1., 0.], [0., 1.]])]
G_ukf_arr    = []

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_ = A_ @ x_arr[-1] + B_ @ v_
    y_ = C_.T @ x_ + w_
    x_arr += [x_]
    y_arr += [y_]

    xhat, P_, G_, Pm = kf(
        A_, B_, C_, Q_, R_, y_, xhat_kf_arr[-1], P_kf_arr[-1])
    xhat_kf_arr += [xhat]
    P_kf_arr    += [P_]
    G_kf_arr    += [G_]

    xhat, P_, G_, Pm = ukf(
        f_, h_, B_, Q_, R_, y_, xhat_ukf_arr[-1], P_ukf_arr[-1], kappa=2)
    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_kf_arr = np.array(xhat_kf_arr)
P_kf_arr    = np.array(P_kf_arr)
G_kf_arr    = np.array(G_kf_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(2, 1, 1)
ax2 = fig.add_subplot(2, 1, 2, sharex=ax1)

ax1.plot(xhat_kf_arr[:, 0, 0], ls='-', label='$kf$')
ax1.plot(xhat_ukf_arr[:, 0, 0], ls='--', label='$ukf$')
ax1.set_ylabel('$x_1$')
ax1.grid(ls=':'); ax1.legend()

ax2.plot(xhat_kf_arr[:, 1, 0], ls='-', label='$kf$')
ax2.plot(xhat_ukf_arr[:, 1, 0], ls='--', label='$ukf$')
ax2.set_ylabel('$x_2$')
ax2.grid(ls=':'); ax2.legend()

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

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(P_kf_arr[:, 0, 0], '-', label='kf')
ax1.plot(P_ukf_arr[:, 0, 0], '--', label='ukf')
ax1.set_ylabel('$P_{0,0}$')
ax1.grid(ls=':'); ax1.legend()

ax2.plot(P_kf_arr[:, 0, 1], '-', label='kf')
ax2.plot(P_ukf_arr[:, 0, 1], '--', label='ukf')
ax2.set_ylabel('$P_{0,1},P_{1,0}$')
ax2.grid(ls=':'); ax2.legend()

ax3.plot(P_kf_arr[:, 1, 1], '-', label='kf')
ax3.plot(P_ukf_arr[:, 1, 1], '--', label='ukf')
ax3.set_ylabel('$P_{1,1}$')
ax3.grid(ls=':'); ax3.legend()

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

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

ax1.plot(G_kf_arr[:, 0, 0], '-', label='kf')
ax1.plot(G_ukf_arr[:, 0, 0], '--', label='ukf')
ax1.set_ylabel('$G_{0,0}$')
ax1.grid(ls=':'); ax1.legend()

ax2.plot(G_kf_arr[:, 1, 0], '-', label='kf')
ax2.plot(G_ukf_arr[:, 1, 0], '--', label='ukf')
ax2.set_ylabel('$G_{1,0}$')
ax2.grid(ls=':'); ax2.legend()

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

推定値も共分散行列もカルマンゲインも線形カルマンフィルタとUKFで一致している

参考文献

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

カルマンフィルタの基礎

作者:足立修一,丸田一郎
東京電機大学出版局

Amazon

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