課程4： Kalman Filters

2.練習：Tracking Intro

卡爾曼濾波，是一個非常流行的系統狀態的估計方法，和概率定位方法相當類似，

與蒙特卡洛定位方法最主要的區別是：卡爾曼濾波對一個連續的狀態進行估計，而蒙特卡洛定位方法中，得把世界分割成離散的小塊。

作爲結果，卡爾曼濾波給我們一個單峯分佈。而蒙特卡洛定位方法是多峯分佈。這兩種方法都適用於機器人定位和對其他車輛的跟蹤。

粒子濾波也是一種解決這類問題的方法。粒子濾波是連續且多峯分佈的。

3. 練習：Gaussian Intro

直方圖分佈中，僅僅是對連續分佈的近似表達。

卡爾曼濾波中，分佈取決於所謂的高斯函數 Gaussian,高斯函數下面的面積加起來等於1. 一維高斯函數中，有2個變量，均值和方差。那麼卡爾曼濾波中任務就是對一個未知的物體位置求出最佳的均值和方差。

4. 練習：Variance Comparison

方差越大，高斯函數波形越平緩，方差越小，高斯函數波形越窄

5. 練習：Preferred Gaussian

方差小，波形窄的高斯函數更好的。

6. 練習：Evaluate Gaussian

7. 練習：Maximize Gaussian

高斯函數的python簡單實現代碼：

from math import *

 

def f(mu, sigma2, x):

    return1/sqrt(2.*pi*sigma2) * exp(-.5*(x-mu)**2 / sigma2)

 

print f(10.,4.,10.) #Change the 8. to something else!

X=均值時，高斯函數獲得最大的值。

8. 練習：Measurements and Motion 1

測量時，我們是使用了乘法。移動時，我們是使用了卷積。

9. 練習： Measurements and Motion 2

測量時，使用的是貝葉斯定理，移動時，使用的是全概率定理

10. 練習：Shifting the Mean

先驗概率分佈假如是一個高斯函數的話，

如果測量得到了另一個高斯分佈，則後驗概率分佈的高斯函數的均值位於前2個高斯函數均值之間。

11. 練習: Predicting the Peak

對於10中的情況，後驗概率分佈的峯值應該比前兩個高斯分佈的峯值更高。

12. 練習：Parameter Update

提到了2個高斯函數的波形，分別代表一個測量前，一個測量後。如果依據貝葉斯定理，對這2個高斯函數進行相乘，會得到一個新的高斯函數。新的高斯函數的均值和方差，可以由前面的2個高斯函數的均值和方差計算得來。計算公式的實現見16中的代碼。

13. 練習：Parameter Update 2

14. 練習：Separated Gaussians

15. 練習： Separated Gaussians 2

16. 練習：New Mean and Variance

12中提到的計算方法的代碼化，如下：

def update(mean1, var1, mean2, var2):

    new_mean = (mean1*var2 +mean2*var1)/(var1+var2)

    new_var = 1/(1/var1 +1/var2)

    return [new_mean, new_var]

 

print update(10.,8.,13., 2.)

update的四個參數是2個高斯函數的均值和方差

17. 練習：Gaussian Motion

運動時，本身也存在誤差，這個誤差也可以表示爲一個高斯函數。那麼，運動後的高斯函數計算方式，是原高斯函數 + 運動誤差的高斯函數。

具體爲 U2 = U1 + v, Var2 = Var1 + var

18. 練習：Predict Function

19. 練習：Kalman Filter Code

簡單的一維卡爾曼過濾器，代碼如下：

# -*- coding: utf-8 -*-

# Write a program that will iteratively update and

# predict based on the location measurements

# and inferred motions shown below.

 

def update(mean1, var1, mean2, var2):

    new_mean = float(var2 *mean1 + var1 * mean2) / (var1 + var2)

    new_var = 1./(1./var1 +1./var2)

    return [new_mean, new_var]

 

def predict(mean1, var1, mean2, var2):

    new_mean = mean1 + mean2

    new_var = var1 + var2

    return [new_mean, new_var]

 

measurements = [5., 6., 7., 9., 10.]

motion = [1., 1., 2., 1., 1.]

measurement_sig = 4.

motion_sig = 2.

mu = 0.

sig = 10000.

 

#Please print out ONLY the final values of the mean

#and the variance in a list [mu, sig].

 

# Insert code here

for i in range(len(motion)):

    [mu, sig] =update(measurements[i], measurement_sig, mu, sig)

    #print('after update  [{},{}]'.format(mu, sig))

    [mu, sig] = predict(mu,sig, motion[i], motion_sig)

    #print('after predict[{},{}]'.format(mu, sig))

 

print('====================')

print([mu, sig])

輸出結果如下：

====================

[10.999906177177365, 4.005861580844194]

20. 練習：Kalman Prediction

卡爾曼過濾器，即使沒有測量物體的速度，也可以通過物體的位置變化，得到物體的速度，進而預測追蹤的物體以該速度出現的下一個位置。

21. Kalman Filter Land

多維高斯函數或多元高斯函數

對於n維空間來說，此時，均值是個nx1的向量，每個元素對應一個變量。

方差是協方差，是一個nxn的矩陣

多維高斯函數有專門的公式描述其函數值，跟均值和協方差還有當前的向量x有關。

22. 練習：Kalman Filter Prediction

在一維空間，已知當前位置，和速度的高斯函數，假設一個當前速度，可預測到後一時刻的位置和速度。

23. 練習：Another Prediction

24. More Kalman Filters

卡爾曼過濾器能通過已有的變量，觀察到隱藏的變量。比如無人駕駛車能通過被追蹤的其他汽車的不同時刻的位置，從而獲得它們的速度。

25. Kalman Filter Design

1.狀態轉換函數

表示從當前狀態到後一個狀態的過程

Nx1 的新狀態向量 = nxn 狀態轉換矩陣 * nx1 的當前狀態向量

nxn 狀態轉換矩陣被稱爲 F （在後續代碼中）

2. 測量函數

Z = 1xn 矩陣 * nx1狀態向量

這裏的 1xn矩陣被稱爲 H（在後續代碼中）

3. 卡爾曼濾波器的實際更新方程

下面講一些代碼中會出現的相關變量名的含義：

x 估計值

P 不確定性協方差矩陣

F 狀態轉換矩陣

u 移動向量

Z 測量值

H 測量方程

R 測量噪聲

K 卡爾曼增益

I 單位矩陣

預測(prediction)過程：

x’ = F*x + u 預測新的x值

p’ = F*P*F^T 預測新的P值，其中F^T表示F的轉置矩陣

測量更新過程（measurementupdate）：

y = Z – H*x y表示誤差（error）

S = H*P*H^T + R y被映射到S中

K = P*H^T*S^-1 K通常被稱爲卡爾曼增益，S^-1 是S的反轉矩陣

x’ = x + (K * y)

p’ = (I – K*H)*P

26. 練習：Kalman Matrices

具體代碼實現例子，一個一維卡爾曼過濾器的代碼：

# -*- coding:utf-8 -*-

# Write afunction 'kalman_filter' that implements a multi-

# dimensionalKalman Filter for the example given

 

from math import*

 

 

class matrix:

   

    # implements basic operations of a matrixclass

   

    def __init__(self, value):

        self.value = value

        self.dimx = len(value)

        self.dimy = len(value[0])

        if value == [[]]:

            self.dimx = 0

   

    def zero(self, dimx, dimy):

        # check if valid dimensions

        if dimx < 1 or dimy < 1:

            raise ValueError("Invalid sizeof matrix")

        else:

            self.dimx = dimx

            self.dimy = dimy

            self.value = [[0 for row in range(dimy)]for col in range(dimx)]

   

    def identity(self, dim):

        # check if valid dimension

        if dim < 1:

            raise ValueError("Invalid sizeof matrix")

        else:

            self.dimx = dim

            self.dimy = dim

            self.value = [[0 for row inrange(dim)] for col in range(dim)]

            for i in range(dim):

                self.value[i][i] = 1

   

    def show(self):

        for i in range(self.dimx):

            print(self.value[i])

        print(' ')

   

    def __add__(self, other):

        # check if correct dimensions

        if self.dimx != other.dimx or self.dimy!= other.dimy:

            raise ValueError("Matricesmust be of equal dimensions to add")

        else:

            # add if correct dimensions

           res = matrix([[]])

            res.zero(self.dimx, self.dimy)

            for i in range(self.dimx):

                for j in range(self.dimy):

                    res.value[i][j] =self.value[i][j] + other.value[i][j]

            return res

    

    def __sub__(self, other):

        # check if correct dimensions

        if self.dimx != other.dimx or self.dimy!= other.dimy:

            raise ValueError("Matricesmust be of equal dimensions to subtract")

        else:

            # subtract if correct dimensions

            res = matrix([[]])

            res.zero(self.dimx, self.dimy)

            for i in range(self.dimx):

                for j in range(self.dimy):

                    res.value[i][j] =self.value[i][j] - other.value[i][j]

            return res

   

    def __mul__(self, other):

        # check if correct dimensions

        if self.dimy != other.dimx:

            raise ValueError("Matricesmust be m*n and n*p to multiply")

        else:

            # multiply if correct dimensions

            res = matrix([[]])

            res.zero(self.dimx, other.dimy)

            for i in range(self.dimx):

                for j in range(other.dimy):

                    for k in range(self.dimy):

                        res.value[i][j] += self.value[i][k] *other.value[k][j]

            return res

   

    def transpose(self):

        # compute transpose

        res = matrix([[]])

        res.zero(self.dimy, self.dimx)

        for i in range(self.dimx):

            for j in range(self.dimy):

                res.value[j][i] =self.value[i][j]

        return res

   

    # Thanks to Ernesto P. Adorio for use ofCholesky and CholeskyInverse functions

   

    def Cholesky(self, ztol=1.0e-5):

        # Computes the upper triangularCholesky factorization of

        # a positive definite matrix.

        res = matrix([[]])

        res.zero(self.dimx, self.dimx)

       

        for i in range(self.dimx):

            S = sum([(res.value[k][i])**2 for kin range(i)])

            d = self.value[i][i] - S

            if abs(d) < ztol:

                res.value[i][i] = 0.0

            else:

                if d < 0.0:

                    raiseValueError("Matrix not positive-definite")

                res.value[i][i] = sqrt(d)

            for j in range(i+1, self.dimx):

                S = sum([res.value[k][i] *res.value[k][j] for k in range(self.dimx)])

                if abs(S) < ztol:

                    S = 0.0

                try:

                   res.value[i][j] =(self.value[i][j] - S)/res.value[i][i]

                except:

                   raise ValueError("Zerodiagonal")

        return res

   

    def CholeskyInverse(self):

        # Computes inverse of matrix given itsCholesky upper Triangular

        #decomposition of matrix.

        res = matrix([[]])

        res.zero(self.dimx, self.dimx)

       

        # Backward step for inverse.

        for j in reversed(range(self.dimx)):

            tjj = self.value[j][j]

            S = sum([self.value[j][k]*res.value[j][k]for k in range(j+1, self.dimx)])

            res.value[j][j] = 1.0/tjj**2 -S/tjj

            for i in reversed(range(j)):

                res.value[j][i] =res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]

        return res

   

    def inverse(self):

        aux = self.Cholesky()

        res = aux.CholeskyInverse()

        return res

   

    def __repr__(self):

        return repr(self.value)

 

 

########################################

 

# Implement thefilter function below

 

def kalman_filter(x, P):

    for n in range(len(measurements)):

       

        # measurement update

        Z = matrix([[measurements[n]]])

        y = Z - (H * x)

        S = H * P * H.transpose() + R

        K = P * H.transpose() * S.inverse()

        x = x + (K * y)

       

        P = (I - (K * H)) * P

 

        # prediction

        x = (F * x) + u

        P = F * P * F.transpose()

 

        # Test code

        print('x :')

        x.show()

        print('----------------------')

        print('P :')

        P.show()

    return x,P

 

############################################

### use the codebelow to test your filter!

############################################

 

measurements =[1, 2, 3]

 

x =matrix([[0.], [0.]]) # initial state (location and velocity)

P =matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty

u =matrix([[0.], [0.]]) # external motion

F = matrix([[1.,1.], [0, 1.]]) # next state function

H = matrix([[1.,0.]]) # measurement function

R =matrix([[1.]]) # measurement uncertainty

I = matrix([[1.,0.], [0., 1.]]) # identity matrix

 

print(kalman_filter(x,P))

# output shouldbe:

# x:[[3.9996664447958645], [0.9999998335552873]]

# P:[[2.3318904241194827, 0.9991676099921091], [0.9991676099921067,0.49950058263974184]]

當卡爾曼過濾從一維擴展到多維時，相關變量的維數相應會增加，

比如擴展到二維時，25中的例子裏面，每個測量值 Z 爲 [x,y] 的表示二維空間中的位置的值，而狀態值 x 會從 2x1兩個量變化爲 4個量，分別表示x維度的位置和速度，y維度的位置和速度。。。

另一個關於卡爾曼過濾的在線課程地址這個課程的視頻講到了多維卡爾曼過濾，後面的一部分講到了追蹤飛機的例子。。應該比 Udacity的這個要詳細一些。這個鏈接的網頁可以打開，但是播放視頻不了，因爲它是youtube的鏈接，被大陸屏蔽了。。