カルマンフィルタの基礎 第10回
今回から、非線形カルマンフィルタとしてUKF(Unscented Kalman Filter)を取り扱う
今回は、UKFで使用するシグマポイントを求めるための、U変換について説明する
U変換
UKFでは、前回推定値 と 誤差共分散行列
から、
「シグマポイント」と呼ばれる少数の点を生成して、
その点を状態モデルで遷移させ、その遷移前後の点の分布から、事後推定を行う
関数の定義
- 平均値と共分散行列からシグマポイントを生成する関数
- シグマポイントの平均値を求める関数
- シグマポイントの共分散行列を求める関数
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の線形近似と比較もする
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()
出力 の乱数分布 と 線形近似の比較
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変換例
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()
出力 の乱数分布 と 線形近似の比較
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の関数で行っているが、念のため、実装も行ってみる
コレスキー分解のアルゴリズム
下記のように求める
3x3行列での計算例
下記のようになる
実際に求める場合は、行列の左上から1行ずつ求めていく
で求めていく
実計算
3x3行列で実際に計算してみる
が正定値対称行列である必要があるため、
逆に、答え から を生成して、そこから求めていく
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.]])
参考文献
この記事は以下の書籍を参考にしましたが、
私の拙い知識で書いておりますので、誤り等ありましたらご指摘ください