Unscented Transformation(アンセンテッド変換,U変換)のpythonプログラム

Unscented Transformation

U変換は，標準正規分布に従う確率変数$x$の平均$\bar{x}$と分散共分散行列$P_x$が既知であるとき，$x$の非線形変換$y=f(x)$で変換される確率変数$y$の$\bar{y}$と$P_y$を推定する方法である．

まず，この場合，モンテカロ的に

$$ \bar{y}\simeq\frac{1}{N}\sum_{i=1}^Nf(x_i) $$

$$ P_y \simeq \frac{1}{N}\sum_{i=1}^N(f(x_i)-\bar{y})(f(x_i)-\bar{y})^T $$

のように計算することが思いつくが，精度をよく計算するにはNを大きくする必要があり，実用上問題がある．

線形近似をすることなく，モンテカロ法のよいところを利用できるように，できるだけ少ないサンプル点を用いて，変換後の確率変数の統計的性質を推定する方法がU変換である．

まず，確率変数$x$からサンプルする値(シグマ点)を決め，シグマ点を非線形変換し，変換した値から$y$の$\bar{y}$と$P_y$を求める．

参考1：UKF （Unscented Kalman Filter）って何？

import matplotlib.pyplot as plt
import numpy as np
import random
import math
import scipy.linalg

入力には$X=(X_1,X_2)$を用いる．

平均ベクトルは，

$$ \mu=(E[X_1],E[X_2])=(\bar{x}_1,\bar{x}_2) $$ 分散共分散ベクトルは，

$$ P_x = [ \begin{array}{cc} var[X_1] & cov[X_1,X_2] \ cov[X_2,X_1] & var[X_2] \end{array} ] = [ \begin{array}{cc} \sigma_1^2 & \sigma_1\sigma_2 \ \sigma_1\sigma_2 & \sigma_2^2 \end{array} ] $$

で表すことができる．
よって， $X_1,X_2$にそれぞれ平均0分散1，平均1分散2の標準正規分布を用いると考えると，

$$ \bar{x}=[0, 1] $$

$$ P_x = [ \begin{array}{cc} 1 & 2 \ 2 & 4 \end{array} ] $$ となる．

これを入力として用いる．出力は1次元とし$f(x)=x[0]*x[1]$の非線形変換を考える．

# Number of dimensions of x
n = 2
# Number of dimensions of y
m = 1

# Mean and Variance of x
x_mean = np.array([0, 1])
x_P = np.array([[1,2],[2,4]])
print("Mean",x_mean)
print("Variance",x_P)

# Nonlinear transformation of x
def f(x):
  return [x[0]*x[1]]

シグマ点の計算

$$ \sigma_0 = \bar{x} \tag{1} $$

$$ \sigma_i = \bar{x}+(\sqrt{(n+\lambda)}P_x) \tag{2} $$

$$ \sigma_i = \bar{x}-(\sqrt{(n+\lambda)}P_x) \tag{3} $$

ここで，$\lambda$は以下のように計算される．

$$ \lambda = \alpha^2*(n+\kappa)-n \tag{4} $$

ここで，$\alpha$と$\kappa$はハイパーパラメータである．

$\alpha$:0-1のスカラー値として指定され，平均の状態値の周りのシグマポイントの広がりを決定する．通常、小さい正の数値をとる．シグマポイントの広がりは$\alpha$に比例し，値が小さいほどシグマポイントは平均の状態に近くなる．

$\kappa$：通常は0に設定される．値が小さいほどシグマポイントは平均の状態に近くなり，広がりは$\kappa$の平方根に比例する．

# Parameter
alpha = 0.5
kappa = 0
# 式4
lambd=alpha**2*(n+kappa)-n
print(lambd)
# Sampling sigma points
sigma = np.zeros((n, 2*n+1))

# 式1
sigma[:,0] = x_mean
gamma=math.sqrt(n+lamda)
# 式2
for i in range(n):
    sigma[:,i+1] = x_mean + gamma * x_P[:,i]
# 式3
for i in range(n):
    sigma[:,i+n+1] =x_mean - gamma * x_P[:,i]

print("selected sigma point")
for i in range(2*n+1):
  print(sigma[:,i])

変換

非線形変換により$y_{\sigma}$をもとめる．

$$ y_{\sigma}=f(\sigma) \tag{5} $$

重み関数$w_i$を求める．

$$ w_0=\frac{\lambda}{n+\lambda} \tag{6} $$

$$ w_i=\frac{1}{2(n+\lambda)} \tag{7} $$

$y$の平均と分散は以下のように計算できる．

$$ \bar{y} \simeq \sum_{i=0}^{2n}w_iy_{\sigma i} \tag{8} $$

$$ P_y \simeq \sum_{i=0}^{2n} w_i (y_{\sigma i}-\bar{y})*(y_{\sigma i}-\bar{y})^T \tag{9} $$

sigma_y = np.zeros((m, 2*n+1))
# Non-linear transformation　式5
for i in range(len(sigma)):
  sigma_y[:,i] = f(sigma[:,i])

w = np.zeros((1, 2*n+1))
# Weight function calculation　式6,7
w[:,0] = lambd/(n+lambd)
for i in range(2*n):
  w[:,i+1] = 1/(2*(n+lambd))
print("重み関数", w)
y_mean=0
# mean of y 式8
for i in range(2*n+1):
  y_mean += w[:,i]*sigma_y[:,i]

y_P = 0
# variance of y 式9
for i in range(2*n+1):
  y_P += w[:,i]*(sigma_y[:,i]-y_mean)*(sigma_y[:,i]-y_mean).T


print(y_mean)
print(y_P)

Tags:

機械学習