サイトマップ

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

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

今回から、非線形カルマンフィルタとしてUKF(Unscented Kalman Filter)を取り扱う
今回は、UKFで使用するシグマポイントを求めるための、U変換について説明する

U変換

UKFでは、前回推定値  \hat{x}(k-1) と 誤差共分散行列  P(k-1) から、
「シグマポイント」と呼ばれる少数の点を生成して、
その点を状態モデルで遷移させ、その遷移前後の点の分布から、事後推定を行う

関数の定義

  • 平均値と共分散行列からシグマポイントを生成する関数
  • シグマポイントの平均値を求める関数
  • シグマポイントの共分散行列を求める関数
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd

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)

1次モデルに対するU変換例

下記のモデルに対して、U変換を実施する
また、EKFの線形近似と比較もする

 \displaystyle
y = x + 3 \cos (\frac{x}{10})
def f_(x_):
    return x_ + 3 * np.cos(x_ / 10)

乱数の生成

mu    = 10
sigma = 25
N_    = 500
np.random.seed(0)
x_arr = np.random.normal(mu, np.sqrt(sigma), N_)
fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(1, 1, 1)

ax1.hist(x_arr, color='b', alpha=0.1)
ax1.annotate(
    '', xy=[mu - np.sqrt(sigma), 50], xytext=[mu + np.sqrt(sigma), 50],
    arrowprops=dict(arrowstyle='<|-|>', color='b')
)
ax1.axvline(mu, color='b')

ax1.set_xlabel('$x$')
ax1.set_ylabel('$count$')
ax1.grid(ls=':')

fig.tight_layout()

出力  y の乱数分布 と 線形近似の比較

def Af(x_):
    return 1.0 - 3 / 10 * np.sin(x_ / 10)
y_arr    = []
ylin_arr = []
for i in range(N_):
    y_arr    += [f_(x_arr[i])]
    ylin_arr += [f_(mu) + Af(mu) * (x_arr[i] - mu)]
y_arr    = np.array(y_arr)
ylin_arr = np.array(ylin_arr)
fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(1, 1, 1)

mean1  = y_arr.mean()
sigma1 = y_arr.std()**2
mean2  = ylin_arr.mean()
sigma2 = ylin_arr.std()**2

ax1.hist(y_arr, color='b', alpha=0.1)
ax1.annotate(
    '', xy=[mean1 - np.sqrt(sigma1), 60], xytext=[mean1 + np.sqrt(sigma1), 60],
    arrowprops=dict(arrowstyle='<|-|>', color='b')
)
ax1.axvline(mean1, color='b')

ax1.hist(ylin_arr, color='r', alpha=0.1)
ax1.annotate(
    '', xy=[mean2 - np.sqrt(sigma2), 50], xytext=[mean2 + np.sqrt(sigma2), 50],
    arrowprops=dict(arrowstyle='<|-|>', color='r')
)
ax1.axvline(mean2, color='r')

ax1.set_xlabel('$y$')
ax1.set_ylabel('$count$')
ax1.grid(ls=':')

fig.tight_layout()

この例の場合、線形近似でもほぼ理想的な分布と一致する

U変換との比較

まずは、事前分布(この場合、元の設定値)の平均と分散からシグマポイントを算出する

Xm = calc_Xm(np.array([mu]), np.array([[sigma]]), kappa=0)
Xm
array([[[10.]],
       [[15.]],
       [[ 5.]]])
fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(1, 1, 1)

ax1.hist(x_arr, color='b', alpha=0.1)
ax1.annotate(
    '', xy=[mu - np.sqrt(sigma), 50], xytext=[mu + np.sqrt(sigma), 50],
    arrowprops=dict(arrowstyle='<|-|>', color='b')
)
ax1.axvline(mu, color='b')

ax1.axvline(Xm[0][0][0], color='g')
ax1.axvline(Xm[1][0][0], color='g')
ax1.axvline(Xm[2][0][0], color='g')

ax1.set_xlabel('$x$')
ax1.set_ylabel('$count$')
ax1.grid(ls=':')

fig.tight_layout()

平均と±標準偏差の3点がシグマポイントになる

次に、今回の遷移関数を通す

Ym = np.array([[[f_(x_[0][0])]] for x_ in Xm])
Ym
array([[[11.62090692]],
       [[15.21221161]],
       [[ 7.63274769]]])
fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(1, 1, 1)

mean1  = y_arr.mean()
sigma1 = y_arr.std()**2

ax1.hist(y_arr, color='b', alpha=0.1)
ax1.annotate(
    '', xy=[mean1 - np.sqrt(sigma1), 60], xytext=[mean1 + np.sqrt(sigma1), 60],
    arrowprops=dict(arrowstyle='<|-|>', color='b')
)
ax1.axvline(mean1, color='b')

ax1.axvline(Ym[0][0][0], color='g')
ax1.axvline(Ym[1][0][0], color='g')
ax1.axvline(Ym[2][0][0], color='g')

ax1.set_xlabel('$y$')
ax1.set_ylabel('$count$')
ax1.grid(ls=':')

fig.tight_layout()

この3点から、重みづけで平均と分散を求める

yhatm = calc_mean(Ym, kappa=0)
yhatm
array([[11.42247965]])
Pm = calc_Pm(Ym, Ym, kappa=0)
Pm
array([[14.36206833]])
fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(1, 1, 1)

mean1  = y_arr.mean()
sigma1 = y_arr.std()**2

ax1.hist(y_arr, color='b', alpha=0.1)
ax1.annotate(
    '', xy=[mean1 - np.sqrt(sigma1), 60], xytext=[mean1 + np.sqrt(sigma1), 60],
    arrowprops=dict(arrowstyle='<|-|>', color='b')
)
ax1.axvline(mean1, color='b')

ax1.annotate(
    '', xy=[yhatm[0][0] - np.sqrt(Pm[0][0]), 50], xytext=[yhatm[0][0] + np.sqrt(Pm[0][0]), 50],
    arrowprops=dict(arrowstyle='<|-|>', color='r')
)
ax1.axvline(yhatm[0][0], color='r')

ax1.set_xlabel('$y$')
ax1.set_ylabel('$count$')
ax1.grid(ls=':')

fig.tight_layout()

ほぼ理想分布の平均と分散を再現できている

2次モデルに対するU変換例

 \displaystyle
\begin{bmatrix}
  y_1 \\
  y_2 \\
\end{bmatrix}
=
\begin{bmatrix}
  x_1 \cos (x_2) \\
  x_1 \sin (x_2) \\
\end{bmatrix}
def f_(x_):
    return np.array([x_[0] * np.cos(x_[1]), x_[0] * np.sin(x_[1])])

乱数の生成

mu = np.array([10, np.pi/2])
sigma = np.array([[50, 1], [1, 0.025]])

N_ = 500
np.random.seed(0)
x_arr = np.random.multivariate_normal(mu, sigma, N_)
from scipy.linalg import sqrtm

def plot_dist(ax, mean_, cov_, color):
    uni_x = np.cos(np.linspace(0, 2 * np.pi, 100))
    uni_y = np.sin(np.linspace(0, 2 * np.pi, 100))
    sqrt_m = sqrtm(cov_)
    x_ = mean_[0] + uni_x * sqrt_m[0][0] * 2 + uni_y * sqrt_m[1][0] * 2
    y_ = mean_[1] + uni_x * sqrt_m[0][1] * 2 + uni_y * sqrt_m[1][1] * 2
    ax.plot(x_, y_, '-', color=color)

def plot_mean(ax, mean_, color):
    ax.scatter(mean_[0], mean_[1], marker='o', color=color)
fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(1, 1, 1)

ax1.scatter(x_arr[:, 0], x_arr[:, 1], c='b', alpha=0.2)
plot_dist(ax1, mu, sigma, 'b')
plot_mean(ax1, mu, 'b')

ax1.set_xlabel('$x_1$')
ax1.set_ylabel('$x_2$')
ax1.grid(ls=':')

fig.tight_layout()

出力  y の乱数分布 と 線形近似の比較

def Af(x_):
    return np.array([[np.cos(x_[1]), -x_[0] * np.sin(x_[1])], [np.sin(x_[1]), x_[0] * np.cos(x_[1])]])
y_arr    = []
ylin_arr = []
for i in range(N_):
    y_arr    += [f_(x_arr[i])]
    ylin_arr += [f_(mu) + Af(mu) @ (x_arr[i] - mu)]
y_arr    = np.array(y_arr)
ylin_arr = np.array(ylin_arr)
fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(1, 1, 1)

mean1 = np.array([y_arr[:, 0].mean(), y_arr[:, 1].mean()])
cov1  = np.cov(y_arr[:, 0], y_arr[:, 1])
mean2 = np.array([ylin_arr[:, 0].mean(), ylin_arr[:, 1].mean()])
cov2  = np.cov(ylin_arr[:, 0], ylin_arr[:, 1])

ax1.scatter(y_arr[:, 0], y_arr[:, 1], marker='.', color='b', alpha=0.2)
plot_dist(ax1, mean1, cov1, 'b')
plot_mean(ax1, mean1, 'b')

ax1.scatter(ylin_arr[:, 0], ylin_arr[:, 1], marker='.', color='r', alpha=0.2)
plot_dist(ax1, mean2, cov2, 'r')
plot_mean(ax1, mean2, 'r')

ax1.set_xlabel('$y_1$')
ax1.set_ylabel('$y_2$')
ax1.grid(ls=':')

fig.tight_layout()

線形近似だと、理想分布と差が出てしまう

U変換との比較

Xm = calc_Xm(mu, sigma, kappa=2)
Xm
array([[[10.        ],
        [ 1.57079633]],
       [[24.14213562],
        [ 1.85363904]],
       [[10.        ],
        [ 1.71221768]],
       [[-4.14213562],
        [ 1.28795361]],
       [[10.        ],
        [ 1.42937497]]])
fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(1, 1, 1)

ax1.scatter(x_arr[:, 0], x_arr[:, 1], c='b', alpha=0.2)
plot_dist(ax1, mu, sigma, 'b')
plot_mean(ax1, mu, 'b')

plot_mean(ax1, Xm[0], 'r')
plot_mean(ax1, Xm[1], 'g')
plot_mean(ax1, Xm[2], 'y')
plot_mean(ax1, Xm[3], 'g')
plot_mean(ax1, Xm[4], 'y')

ax1.set_xlabel('$x_1$')
ax1.set_ylabel('$x_2$')
ax1.grid(ls=':')

fig.tight_layout()

平均と±標準偏差の5点がシグマポイントになる

これらを今回の遷移関数に通す

Ym = np.array([f_(x_) for x_ in Xm])
Ym
array([[[ 6.12323400e-16],
        [ 1.00000000e+01]],
       [[-6.73774492e+00],
        [ 2.31828710e+01]],
       [[-1.40950423e+00],
        [ 9.90016656e+00]],
       [[-1.15601427e+00],
        [-3.97755183e+00]],
       [[ 1.40950423e+00],
        [ 9.90016656e+00]]])
fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(1, 1, 1)

mean1 = np.array([y_arr[:, 0].mean(), y_arr[:, 1].mean()])
cov1  = np.cov(y_arr[:, 0], y_arr[:, 1])

ax1.scatter(y_arr[:, 0], y_arr[:, 1], marker='.', color='b', alpha=0.2)
plot_dist(ax1, mean1, cov1, 'b')
plot_mean(ax1, mean1, 'b')

plot_mean(ax1, Ym[0], 'r')
plot_mean(ax1, Ym[1], 'g')
plot_mean(ax1, Ym[2], 'y')
plot_mean(ax1, Ym[3], 'g')
plot_mean(ax1, Ym[4], 'y')

ax1.set_xlabel('$y_1$')
ax1.set_ylabel('$y_2$')
ax1.grid(ls=':')

fig.tight_layout()

歪んだ位置にも遷移していて、モデルの特性を保持できていそう

この遷移後のシグマポイントから平均と共分散行列を求める

yhatm = calc_mean(Ym, kappa=2)
yhatm
array([[-0.9867199 ],
       [ 9.87570653]])
Pm = calc_Pm(Ym, Ym, kappa=2)
Pm
array([[ 5.36475633, -9.2057144 ],
       [-9.2057144 , 46.13204804]])
fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(1, 1, 1)

mean1 = np.array([y_arr[:, 0].mean(), y_arr[:, 1].mean()])
cov1  = np.cov(y_arr[:, 0], y_arr[:, 1])

ax1.scatter(y_arr[:, 0], y_arr[:, 1], marker='.', color='b', alpha=0.2)
plot_dist(ax1, mean1, cov1, 'b')
plot_mean(ax1, mean1, 'b')

plot_mean(ax1, Ym[0], 'r')
plot_mean(ax1, Ym[1], 'g')
plot_mean(ax1, Ym[2], 'y')
plot_mean(ax1, Ym[3], 'g')
plot_mean(ax1, Ym[4], 'y')

plot_dist(ax1, yhatm, Pm, 'k')
plot_mean(ax1, yhatm, 'k')

ax1.set_xlabel('$y_1$')
ax1.set_ylabel('$y_2$')
ax1.grid(ls=':')

fig.tight_layout()

かなり理想分布のに近い値になっている

最後に、U変換をまとめると下記になる

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
ut(f_, mu, sigma, kappa=2)
(array([[[10.        ],
         [ 1.57079633]],
        [[24.14213562],
         [ 1.85363904]],
        [[10.        ],
         [ 1.71221768]],
        [[-4.14213562],
         [ 1.28795361]],
        [[10.        ],
         [ 1.42937497]]]),

 array([[[ 6.12323400e-16],
         [ 1.00000000e+01]],
        [[-6.73774492e+00],
         [ 2.31828710e+01]],
        [[-1.40950423e+00],
         [ 9.90016656e+00]],
        [[-1.15601427e+00],
         [-3.97755183e+00]],
        [[ 1.40950423e+00],
         [ 9.90016656e+00]]]),

 array([[-0.9867199 ],
        [ 9.87570653]]),

 array([[ 5.36475633, -9.2057144 ],
        [-9.2057144 , 46.13204804]]))

おまけ コレスキー分解の実装

コレスキー分解をnumpyの関数で行っているが、念のため、実装も行ってみる

コレスキー分解のアルゴリズム

下記のように求める

 \displaystyle
\begin{aligned}
A &= S S^T \\
S_{ii} &= \sqrt{A_{ii} - \sum_{j=0}^{i-1} S_{ij}^2}, \quad i = 0, 1, \ldots, n - 1 \\
S_{ji} &= \frac{1}{S_{ii}} \left( A_{ji} - \sum_{k=0}^{i-1} S_{jk} S_{ik} \right), \quad j > i \\
S_{ji} &= 0, \quad j < i \\
\end{aligned}

3x3行列での計算例

下記のようになる

 \displaystyle
\begin{aligned}
S_{00} &= \sqrt{A_{00} - \sum_{j=0}^{-1} S_{0j}^2} \\ &= \sqrt{A_{00}} \\
S_{11} &= \sqrt{A_{11} - \sum_{j=0}^{0} S_{1j}^2}  \\ &= \sqrt{A_{11} - S_{10}^2} \\
S_{22} &= \sqrt{A_{22} - \sum_{j=0}^{1} S_{2j}^2}  \\ &= \sqrt{A_{11} - S_{20}^2 - S_{21}^2} \\
\end{aligned}
 \displaystyle
\begin{aligned}
S_{10} &= \frac{1}{S_{00}} \left( A_{10} - \sum_{k=0}^{-1} S_{1k} S_{0k} \right) \\
&= \frac{1}{S_{00}} \left( A_{10} \right) \\
S_{20} &= \frac{1}{S_{00}} \left( A_{20} - \sum_{k=0}^{-1} S_{2k} S_{0k} \right) \\
&= \frac{1}{S_{00}} \left( A_{20} \right) \\
S_{21} &= \frac{1}{S_{11}} \left( A_{21} - \sum_{k=0}^{0} S_{2k} S_{1k} \right) \\
&= \frac{1}{S_{11}} \left( A_{21} - S_{20} S_{10} \right) \\
\end{aligned}
 \displaystyle
S_{01} = S_{02} = S_{12} = 0

実際に求める場合は、行列の左上から1行ずつ求めていく
 S_{00} \rightarrow S_{01} (=0) \rightarrow S_{02} (=0)
 S_{10} \rightarrow S_{11} \rightarrow S_{12} (=0)
 S_{20} \rightarrow S_{21} \rightarrow S_{22}
で求めていく

実計算

3x3行列で実際に計算してみる

 A が正定値対称行列である必要があるため、
逆に、答え  S_1 から  A を生成して、そこから求めていく

 \displaystyle
A = S S^T =
\begin{bmatrix}
  1 & 0 & 0 \\
  2 & 3 & 0 \\
  4 & 5 & 6 \\
\end{bmatrix}
\begin{bmatrix}
  1 & 2 & 4 \\
  0 & 3 & 5 \\
  0 & 0 & 6 \\
\end{bmatrix}
=
\begin{bmatrix}
  1 &  2 &  4 \\
  2 & 13 & 23 \\
  4 & 23 & 77 \\
\end{bmatrix}
S1 = np.array([[1, 0, 0], [2, 3, 0], [4, 5, 6]])
A = S1 @ S1.T
A
array([[ 1,  2,  4],
       [ 2, 13, 23],
       [ 4, 23, 77]])
S00 = np.sqrt( A[0][0] )
S01 = S02 = 0

S10 = ( A[1][0] ) / S00
S11 = np.sqrt( A[1][1] - S10**2 )
S12 = 0

S20 = ( A[2][0] ) / S00
S21 = ( A[2][1] - S20 * S10 ) / S11
S22 = np.sqrt( A[2][2] - S20**2 - S21**2 )

S2 = np.array([[S00, S01, S02], [S10, S11, S12], [S20, S21, S22]])
S2
array([[1., 0., 0.],
       [2., 3., 0.],
       [4., 5., 6.]])

関数化

def calc_cholesky(A):
    n_ = A.shape[0]
    S_ = np.zeros(A.shape)
    for i in range(n_):
        for j in range(n_):
            if j == i:
                tmp = A[i][j]
                for k in range(i):
                    tmp -= S_[i][k] * S_[j][k]
                S_[i][j] = np.sqrt(tmp)
            elif j < i:
                tmp = A[i][j]
                for k in range(i):
                    tmp -= S_[i][k] * S_[j][k]
                S_[i][j] = tmp / S_[j][j]
            else:
                S_[i][j] = 0
    return np.array(S_)
calc_cholesky(A)
array([[1., 0., 0.],
       [2., 3., 0.],
       [4., 5., 6.]])

4x4行列でも試す

S3 = np.array([[1, 0, 0, 0 ], [2, 3, 0, 0], [4, 5, 6, 0], [7, 8, 9, 10]])
A2 = S3 @ S3.T
A2
array([[  1,   2,   4,   7],
       [  2,  13,  23,  38],
       [  4,  23,  77, 122],
       [  7,  38, 122, 294]])
calc_cholesky(A2)
array([[ 1.,  0.,  0.,  0.],
       [ 2.,  3.,  0.,  0.],
       [ 4.,  5.,  6.,  0.],
       [ 7.,  8.,  9., 10.]])

参考文献

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