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

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

今回は、カルマンフィルタによる「相補フィルタ」を紹介する
特性の異なる2つのセンサを用いてセンサフュージョンを行い、高精度の測定を実現する

カルマンフィルタの基礎第15回
- 相補フィルタ
- 参考文献

相補フィルタ

状態モデル

観測対象

下記のモデルを測定する
ただし、今回は、対象のモデルは信号生成にのみ使用して、フィルタリングには使用しない

$\displaystyle \begin{aligned} z(k+1) &= z(k) + 2 \cos(0.05k) + d(k), \quad z(0) = 10 \\ d(k) &: 平均値 0 \quad 分散 5 \\ \end{aligned}$

センサ1

$\displaystyle \begin{aligned} e_1(k) &= \mu(k) + \beta(k) \\ \mu(k+1) &= a \mu(k) + (1-a) \nu_1(k), &\quad& \mu(0) = \mu_0 \\ \beta(k+1) &= \beta(k), &\quad& \beta(0) = \beta_0 \\ nu_1(k) &: 平均値 0 \quad 分散 \sigma_{\nu1}^2 \\ \end{aligned}$

センサ2

$\displaystyle \begin{aligned} e_2(k) &= \nu_2(k) \\ \nu_2(k) &: 平均値 0 \quad 分散 \sigma_{\nu2}^2 \\ \end{aligned}$

センサの計測値

$\displaystyle \begin{aligned} y_1(k) &= z(k) + e_1(k) \\ y_2(k) &= z(k) + e_2(k) \\ \end{aligned}$

数値シミュレーションのパラメータ

$\displaystyle \begin{aligned} \sigma_{\nu1}^2 &= \sigma_{\nu2}^2 = 60 \\ a &= 0.75 \\ \mu_0 &= 0 \\ \beta_0 &= 20 \\ e_2(0) &= 0 \\ \end{aligned}$

相補フィルタに用いる状態モデル

状態モデルを拡張し、下記のようにする
観測量は $z(k)$ が計測できないため、 $y_1(k)$ 、 $y_2(k)$ の差分を用いる

$\displaystyle \begin{aligned} x(k) &= \begin{bmatrix} \mu(k) \\ \beta(k) \\ e_2(k) \\ \end{bmatrix} \\ x(k+1) &= \begin{bmatrix} a & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} x(k) + \begin{bmatrix} 1-a & 0 \\ 0 & 0 \\ 0 & 1 \\ \end{bmatrix} \begin{bmatrix} \nu_1(k) \\ \nu_2(k) \\ \end{bmatrix} \\ y(k) &= y_1(k) - y_2(k) \\ &= y_1(k) - y_2(k) \\ &= e_1(k) - e_2(k) \\ &= \mu(k) + \beta(k) - e_2(k) \\ &= \begin{bmatrix} 1 & 1 & -1 \\ \end{bmatrix} x(k) \\ \end{aligned}$

カルマンフィルタのパラメータは下記にする

$\displaystyle \begin{aligned} \hat{x}(0) &= \begin{bmatrix} 10 \\ 30 \\ 20 \\ \end{bmatrix} \\ P(0) &= \begin{bmatrix} 1000 & 0 & 0 \\ 0 & 1000 & 0 \\ 0 & 0 & 1000 \\ \end{bmatrix} \\ Q &= \begin{bmatrix} 60 & 0 \\ 0 & 60 \\ \end{bmatrix} \\ R &= 10^{-4} \\ \end{aligned}$

ちなみに、それぞれのセンサの測定誤差は下記のように求めることができる

$\displaystyle \begin{aligned} \hat{e}_1(k) &= \hat{x}_1(k) + \hat{x}_2(k) \\ \hat{e}_2(k) &= \hat{x}_3(k) \\ \end{aligned}$

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

A_ = np.array([[0.75, 0, 0], [0, 1, 0], [0, 0, 0]])
B_ = np.array([[0.25, 0], [0, 0], [0, 1]])
C_ = np.array([[1], [1], [-1]])
Q_ = np.array([[60, 0], [0, 60]])
R_ = np.array([[1e-4]])

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

今回は線形カルマンフィルタを用いる

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

シミュレーションの実施

N_ = 200
np.random.seed(0)

v_ = np.array([
    [np.random.randn() * np.sqrt(Q_[0][0])],
    [np.random.randn() * np.sqrt(Q_[1][1])],
])  # システム雑音
w_ = np.array([[np.random.randn() * np.sqrt(R_[0][0])]])  # 観測雑音

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

xhat_arr = [np.array([[10], [30], [20]])]
P_arr    = [np.diag([1000, 1000, 1000])]
G_arr    = []

for i in range(1, N_):
    d_ = np.random.randn() * np.sqrt(5)
    z_ = z_arr[-1] + 2 * np.cos(0.05 * i) + d_

    v_ = np.array([
        [np.random.randn() * np.sqrt(Q_[0][0])],
        [np.random.randn() * np.sqrt(Q_[1][1])],
    ])  # システム雑音
    w_ = np.array([[np.random.randn() * np.sqrt(R_[0][0])]])  # 観測雑音

    x_ = A_ @ x_arr[-1] + B_ @ v_
    y_ = C_.T @ x_ + w_

    z_arr += [z_]
    x_arr += [x_]
    y_arr += [y_]

    xhat, P_, G_, Pm = kf(
        A_, B_, C_, Q_, R_, y_, xhat_arr[-1], P_arr[-1])
    xhat_arr += [xhat]
    P_arr    += [P_]
    G_arr    += [G_]

x_arr    = np.array(x_arr)
y_arr    = np.array(y_arr)

xhat_arr = np.array(xhat_arr)
P_arr    = np.array(P_arr)
G_arr    = np.array(G_arr)

シミュレーション結果の確認

真値 $z$ とセンサ計測値 $y_1$ 、 $y_2$

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

ax1.plot(z_arr, '-', label='$z$')
ax1.plot(z_arr + x_arr[:, 0, 0] + x_arr[:, 1, 0], '--', label='$y_1$')
ax1.set_ylabel('sensor1')
ax1.grid(ls=':'); ax1.legend()

ax2.plot(z_arr, '-', label='$z$')
ax2.plot(z_arr + x_arr[:, 2, 0], '--', label='$y_2$')
ax2.set_ylabel('sensor2')
ax2.grid(ls=':'); ax2.legend()

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

センサ1は、低ノイズだが、バイアスがあるセンサ
センサ2は、高ノイズだが、バイアルがないセンサ

推定値の確認

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

ax1.plot(z_arr, '-', label='$z$')
ax1.plot(z_arr + x_arr[:, 0, 0] + x_arr[:, 1, 0], '--', label='$y_1$')
ax1.plot(z_arr + x_arr[:, 0, 0] + x_arr[:, 1, 0] - xhat_arr[:, 0, 0] - xhat_arr[:, 1, 0], '--', label='$\hat{y}_1$')
ax1.set_ylabel('sensor1')
ax1.grid(ls=':'); ax1.legend()

ax2.plot(z_arr, '-', label='$z$')
ax2.plot(z_arr + x_arr[:, 2, 0], '--', label='$y_2$')
ax2.plot(z_arr + x_arr[:, 2, 0] - xhat_arr[:, 2, 0], '--', label='$\hat{y}_2$')
ax2.set_ylabel('sensor2')
ax2.grid(ls=':'); ax2.legend()

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

推定値が真値をよく表していることがわかる

$|mean| + 3 \sigma$ を計測生値と推定値で比較する

mean_  = (x_arr[:, 0, 0] + x_arr[:, 1, 0])[100:].mean()
std_   = (x_arr[:, 0, 0] + x_arr[:, 1, 0])[100:].std()
m3std1 = np.abs(mean_) + 3 * std_
mean_  = (x_arr[:, 2, 0])[100:].mean()
std_   = (x_arr[:, 2, 0])[100:].std()
m3std2 = np.abs(mean_) + 3 * std_

mean_  = (x_arr[:, 0, 0] + x_arr[:, 1, 0] - xhat_arr[:, 0, 0] - xhat_arr[:, 1, 0])[100:].mean()
std_   = (x_arr[:, 0, 0] + x_arr[:, 1, 0] - xhat_arr[:, 0, 0] - xhat_arr[:, 1, 0])[100:].std()
m3std3 = np.abs(mean_) + 3 * std_
mean_  = (x_arr[:, 2, 0] - xhat_arr[:, 2, 0])[100:].mean()
std_   = (x_arr[:, 2, 0] - xhat_arr[:, 2, 0])[100:].std()
m3std4 = np.abs(mean_) + 3 * std_

print('raw y1    : %7.3f  /  y2    : %7.3f' % (m3std1, m3std2))
print('est z(y1) : %7.3f  /  z(y2) : %7.3f' % (m3std3, m3std4))

raw y1    :  26.519  /  y2    :  24.820
est z(y1) :   9.529  /  z(y2) :   9.530

相補フィルタで、より高精度の計測ができることが分かる

参考文献

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

カルマンフィルタの基礎

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

Amazon

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