無跡卡爾曼濾波-2

對於上一篇中的問題：X ∼ N(µ, σ^2 ) , Y = sin(X)要求隨機變量Y的期望和方差。還有一種思路是對X進行采樣，比如取500個采樣點(這些采樣點可以稱為sigma點)，然后求取這些采樣點的期望和方差。當采樣值足夠大時,結果與理論值接近。這種思路的問題顯而易見，當隨機變量維數增大時采樣點的數量會急劇增加，比如一維需要500個采樣點，二維就需要500^=250,000個采樣點，三維情況下需要500^3=125,000,000個采樣點，顯然這樣會造成嚴重的計算負擔。無跡卡爾曼濾法中采用一種方法來選取2n+1個sigma點，n為隨機變量維數。

如上圖所示，考慮正態分布曲線的對稱性，選取3個sigma點來計算(sigma點的選取方法不唯一)，其中一個是均值，另外兩個sigma點關於均值對稱分布。比如三個點可選為：

χ₀ = μ;

χ₁ = μ + σ;

χ₂ = μ - σ;

然后選取合適的權值W_i滿足如下關系_：

使用同樣的公式近似計算Y=sin(X)分布的數字特征：

類比推廣到多維隨機變量X ∼ N(μ ,Σ)，Σ為協方差矩陣，采用Cholesky分解計算出矩陣L(Σ = LL^T)  ，矩陣L可以類比一維情況下的標准差σ。則sigma點可以寫成下面的形式：

χ₀ = μ;

χ_i = μ + cL;

χ_n+i = μ - cL; c為一個正的常數

則對於非線性變換Y=g(X)，可以計算出變換后的期望和方差：

關鍵問題是如何選取sigma點和權值。常見的一種方法是第一個sigma點選為χ₀=μ，n是隨機變量X的維數，α，κ，λ均為常數，並有如下關系

 

其余2n個sigma點的計算公式如下，其中下標i表示矩陣L的第i列

接下來要計算權值。對求期望來說，第一個sigma點的權值為：

對求方差來說，第一個sigma點的權值為如下，式子中的β也為一個常數

對於剩下的2n個sigma點來說求期望的權值和求方差的權值都相同：

因此，一共有三個常數(α,β,κ)需要我們來設定。根據其它參考資料，選擇參數的經驗為：β=2 is a good choice for Gaussian problems, κ=3−n is a good choice for κ , and 0≤α≤1 is an appropriate choice for α. 

α越大，第一個sigma點(平均值處)占的權重越大，並且其它sigma點越遠離均值。如下圖所示，對2維隨機變量選取5個sigma點，點的大小代表權重，由感性認識可知sigma點離均值越遠其權重應該越小。

 

根據上述計算步驟和公式我們可以編程實現sigma點的選擇，權值的計算。在Python中我們可以采用現成的工具FilterPy安裝pip工具后可以直接輸入命令:pip install filterpy進行安裝。

# -*- coding: utf-8 -*-
from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints

mean = 0    # 均值
cov =  1    # 方差

points = SigmaPoints(n=1, alpha=0.1, beta=2.0, kappa=1.0)  
Wm, Wc = points.weights()
sigmas = points.sigma_points(mean, cov)

print Wm, Wc    # 計算均值和方差的權值
print sigmas    # sigma點的坐標

對於標准正態分，輸出結果如下:

 

下面舉一個2維正態分布的例子。從一個服從二維正態分布的隨機變量中隨機選取1000個點，該二維隨機變量的期望和協方差矩陣為：

接下來將這1000個點進行如下非線性變換：

下面使用unscented transform方法近似計算非線性變換后隨機變量的期望，並與直接從1000個點計算出的期望值對比

# -*- coding: utf-8 -*-
import numpy as np
from scipy import stats
import matplotlib.pyplot as plt
from numpy.random import multivariate_normal
from filterpy.kalman import unscented_transform
from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints

# 非線性變換函數
def f_nonlinear_xy(x, y):
    return np.array([x + y, 0.1*x**2 + y**2])

def plot1(xs, ys):
    xs = np.asarray(xs) 
    ys = np.asarray(ys)
    xmin = xs.min()
    xmax = xs.max()
    ymin = ys.min()
    ymax = ys.max()
    values = np.vstack([xs, ys])  
    kernel = stats.gaussian_kde(values)   
    X, Y = np.mgrid[xmin:xmax:100j, ymin:ymax:100j]
    positions = np.vstack([X.ravel(), Y.ravel()])
    Z = np.reshape(kernel.evaluate(positions).T, X.shape)  
    plt.imshow(np.rot90(Z),cmap=plt.cm.Greys,extent=[xmin, xmax, ymin, ymax])
    plt.plot(xs, ys, 'k.', markersize=2)
    plt.xlim(-20, 20)
    plt.ylim(-20, 20)
   
def plot2(xs, ys, f, mean_fx):
    fxs, fys = f(xs, ys)    # 將采樣點進行非線性變換
    computed_mean_x = np.average(fxs)   
    computed_mean_y = np.average(fys)
    plt.subplot(121)
    plt.grid(False)
    plot1(xs, ys)
    plt.subplot(122)
    plt.grid(False)
    plot1(fxs, fys)
    plt.scatter(fxs, fys, marker='.', alpha=0.01, color='k')
    plt.scatter(mean_fx[0], mean_fx[1], marker='o', s=100, c='r', label='UT_mean')
    plt.scatter(computed_mean_x, computed_mean_y, marker='*',s=120, c='b', label='mean')
    plt.ylim([-10, 200])
    plt.xlim([-100, 100])
    plt.legend(loc='best', scatterpoints=1)
    print ('Difference in mean x={:.3f}, y={:.3f}'.format(
           computed_mean_x-mean_fx[0], computed_mean_y-mean_fx[1]))
    
# -------------------------------------------------------------------------------------------
mean = [0, 0]               # Mean of the N-dimensional distribution.
cov = [[32, 15], [15, 40]]  # Covariance matrix of the distribution.

# create sigma points(2n+1個sigma點)
# uses 3 parameters to control how the sigma points are distributed and weighted
points = SigmaPoints(n=2, alpha=.1, beta=2., kappa=1.)  
Wm, Wc = points.weights()
sigmas = points.sigma_points(mean, cov)

# pass through nonlinear function
sigmas_f = np.empty((5, 2))
for i in range(5):  
    sigmas_f[i] = f_nonlinear_xy(sigmas[i, 0], sigmas[i ,1]) 

# use unscented transform to get new mean and covariance
ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc)

# generate random points
xs, ys = multivariate_normal(mean, cov, size=1000).T  # 從二維隨機變量的正態分布中產生1000個數據點
plot2(xs, ys, f_nonlinear_xy, ukf_mean)

# 畫sigma點
plt.xlim(-30, 30); plt.ylim(0, 90)
plt.subplot(121)
plt.scatter(sigmas[:,0], sigmas[:,1], c='r', s=30) 
plt.show()

結果如下圖所示，左圖中黑點為1000個采樣點，5個紅色的點為sigma點，圖中陰影表示概率密度的大小，顏色越深的地方概率密度越大。右圖中的紅點為用5個sigma點經UT變換計算出的近似期望，藍色星號標記點為將1000個采樣點非線性變換后直接計算出的期望。可以看出和直接產生1000個點經非線性變換計算期望相比,使用unscented transform的計算量要小的多，並且誤差也不大。