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

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

前回のカルマンフィルタの設計をPythonで実践する
また、数値シミュレーションで動作も確認する

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

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

以下の状態空間モデルに対するカルマンフィルタのアルゴリズムを整理する

$\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} \hat{x}^-(k) &= A \hat{x}(k-1) \\ P^-(k) &= A P(k-1) A^T + \sigma_\nu^2 b b^T \\ \end{aligned}$

フィルタリングステップ

$\displaystyle \begin{aligned} g(k) &= \frac{P^-(k) c}{c^T P^-(k) c + \sigma_\omega^2} \\ \hat{x}(k) &= \hat{x}^-(k) + g(k) (y(k) - c^T \hat{x}^-(k)) \\ P(k) &= (I - g(k) c^T) P^-(k) \\ \end{aligned}$

カルマンフィルタの数値シミュレーション

今回は、下記の状態空間モデルを考える
１入力 / 1出力かつ 1次元状態変数

$\displaystyle \begin{aligned} x_{k+1} &= x_k + \nu_k \\ y_k &= x_k + \omega_k \\ \end{aligned}$

$\displaystyle \begin{aligned} A & = 1 \\ b & = 1 \\ c & = 1 \\ Q & = \sigma_\nu^2 = 1 \\ R & = \sigma_\omega^2 = 10 \\ \end{aligned}$

カルマンフィルタの実装(スカラ)

カルマンフィルタの関数を定める
スカラなので、行列演算が簡略化されている

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

def kf_scalar(A_, b_, c_, sigma_nu, sigma_omega, y_, xhat, P_):
    xhatm = A_ * xhat
    Pm    = A_ * P_ * A_ + sigma_nu * b_ * b_

    g_       = Pm * c_ / (c_ * Pm * c_ + sigma_omega)
    xhat_new = xhatm + g_ * (y_ - c_ * xhatm)
    P_new    = (1. - g_ * c_) * Pm

    return xhat_new, P_new, g_

数値シミュレーションの実施

A_ = 1.
b_ = 1.
c_ = 1.
sigma_nu2 = 1.
sigma_omega2 = 10.

N_ = 300
np.random.seed(0)

v_ = np.random.randn() * np.sqrt(sigma_nu2)     # システム雑音
w_ = np.random.randn() * np.sqrt(sigma_omega2)  # 観測雑音

x_arr = [0.]
y_arr = [c_ * x_arr[0] + w_]

xhat_arr = [0.]
P_arr    = [0.]

for i in range(1, N_):
    v_ = np.random.randn() * np.sqrt(sigma_nu2)     # システム雑音
    w_ = np.random.randn() * np.sqrt(sigma_omega2)  # 観測雑音

    x_ = A_ * x_arr[-1] + b_ * v_
    y_ = c_ * x_ + w_
    x_arr += [x_]
    y_arr += [y_]

    xhat, P_, g_ = kf_scalar(A_, b_, c_, sigma_nu2, sigma_omega2, 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(111)

ax1.plot(y_arr, ls='-', label='$y$')
ax1.plot(xhat_arr, ls='--', label='$\hat{x}$')
ax1.plot(x_arr, ls='-', alpha=0.6, label='$x$')

ax1.set_xlabel('$k$')
ax1.set_ylabel('$x$')
ax1.grid(ls=':'); ax1.legend()
fig.tight_layout()

観測値 $y$ より、カルマンフィルタによる推定値 $\hat{x}$ が真値 $x$ を良く再現していることが分かる

共分散行列の確認

最後に、事後共分散行列 $P(k)$ を確認しておく

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

ax1.plot(P_arr, '.-', label='$P$')
ax1.text(x=200, y=1.5, s='P[-1] = %7f' % P_arr[-1])

ax1.set_xlabel('$k$')
ax1.set_ylabel('$P(k)$')
ax1.grid(ls=':'); ax1.legend()
fig.tight_layout()