卡爾曼濾波的原理（Python實現)

https://blog.csdn.net/weixin_43956732/article/details/107023254

 

我們假設有一輛運動的汽車，要跟蹤汽車的位置 p 和速度 v，這兩個變量稱為狀態變量，我們使用狀態變量矩陣  來表示小車在 t 時刻的狀態，那么在經過 Δt 的時間之后，當前時刻的位置和速度分別為：

                                                                                 （式1.1） 
                                                                                                              （式1.2）

    其中  表示 t 時刻的加速度大小，由於卡爾曼濾波只能描述狀態與狀態之間的線性關系，所以可將上述兩個表達式轉換為矩陣向量的形式為：

                                                                                           （式1.3）

    其中兩個狀態變化矩陣為： ， ，所以（式1.3）可以寫為：

                                                                                                                    （式1.4） 

     式l.4就是卡爾曼濾波器的第一個公式——狀態預測公式，其中F稱為狀態轉移矩陣，表示我們如何從上一狀態推測出當前狀態；B表示控制矩陣，表示控制量u對當前狀態的影響；x頭上之所以加一個^表示它是一個估計值，而不是真實值，而-上標表示這並非最佳估計。

 

    用協方差矩陣表示預測的不確定性，比如對二維的噪聲，x方向滿足高斯分布，y方向也滿足高斯分布，而這兩個方向的高斯分布是可能存在相關性的，所以我們需要引入協方差矩陣。

                                                                                                               （式1.5）

     正對角線上的值表示的是兩個維度噪聲各自的方差，反對角線上的兩個值是相等的，表示的是兩個維度噪聲的協方差。

 

    噪聲協方差矩陣對系統會產生影響，所以我們要傳遞到系統中去，這時候噪聲協方差矩陣要乘以狀態轉移矩陣F，於是我們得到式1.6。

                                                                                                                                    （式1.6）

     這里我們用到了協方差矩陣的性質 。而由於我們的模型肯定是存在誤差的，所以我們要加入表示誤差的矩陣Q。

                                                                                                                           （式1.7）

     式1.7就是卡爾曼濾波器的第二個公式——表示不確定性在各個時刻之間的傳遞關系。

 

     假設我們通過一個距離傳感器測量小車的位置記為 ，由於我們的狀態變量是一個二維矩陣，而表示距離的  是一個標量，所以狀態變量矩陣就要乘以一個觀測矩陣H，當然我們觀測到的位置也不一定是絕對准確的，所以也要加上一個觀測噪聲v，得到式1.8。

                                                                                                                                        （式1.8）

     同樣，觀測噪聲的協方差矩陣我們用R來表示。這里我們只采用了一種測量方式得到汽車的位置，而實際中我們可能通過多種方式來得到小車的觀測位置，這時候  就不是一個標量，而是多個觀測值組成的矩陣，而每一個觀測值都是真實狀態的一種不完全的表現，我們就可以從這些不完整觀測值推斷小車的真實位置。

 

     通過距離傳感器，我們得到了位置觀測值  和觀測噪聲的協方差矩陣R，現在我們需要將觀測得到的這些值融入到狀態向量中，如式1.9所示。

                                                                                                             （式1.9）

     在式1.4中我們得到了非最佳估計 ，然后我們將觀測得到的值加到它的后面就得到了最佳估計 。我們在（式1.9）增加的內容中，表示我們實際的觀測值和預測值之間的殘差，殘差乘上系數K就可以用來修正 的值。這個K就是卡爾曼系數，表示為：

                                                                                                       （式1.10）

 

    卡爾曼系數的作用主要有兩個方面：

    第一是權衡預測狀態協方差矩陣P和觀察量協方差矩陣R的大小，來決定我們是相信預測模型多一點還是觀察模型多一點。如果相信預測模型多一點，這個殘差的權重就會小一點，如果相信觀察模型多一點，權重就會大一點。

    第二就是把殘差的表現形式從觀察域轉換到狀態域，在我們這個例子中，觀察值z表示的是小車的位置，只是一個一維向量，而狀態向量是一個二維向量，它們所使用的單位和描述的特征都是不同的。而卡爾曼系數就是要實現這樣將一維的觀測向量轉換為二維的狀態向量的殘差，在本例中我們只觀測了小車的位置，而在K中已經包含了協方差矩陣P的信息，所以就利用了位置和速度這兩個維度的相關性，從位置的信息中推測出了速度的信息，從而讓我們可以對狀態量x的兩個維度同時進行修正。

    最后，我們還要對噪聲協方差進行更新，用於下一次迭代，可以表示為：

                                                                                                                              （式1.11）

     在這一輪里噪聲的不確定性是減小的，而在下一輪迭代中，由於傳遞噪聲的引入，不確定性又會增大，卡爾曼濾波器就是在這種不確定性的變化中尋求一種平衡。

 

    綜上所述，我們得到了卡爾曼濾波器的5個公式如下：

                                                                                                             ①

                                                                                                                     ②

 

                                                                                                ③

                                                                                                       ④

                                                                                                                       ⑤

     前兩個表示的是根據上一時刻的狀態來預測當前時刻的狀態，通過這兩個公式我們得到的是非最佳估計的x和P；后三個公式就是通過當前的觀測值來更新x和P，更新之后的就是最佳觀測值。

 

    下面是我用Python簡單模擬預測小車位置和速度的一段程序，觀測的值是一段從0到99，速度為1的勻速行駛路徑，加入的噪聲是均值為0，方差為1的高斯噪聲。

import numpy as np import matplotlib.pyplot as plt # 創建一個0-99的一維矩陣 z = [i for i in range(100)] z_watch = np.mat(z) #print(z_mat) # 創建一個方差為1的高斯噪聲，精確到小數點后兩位 noise = np.round(np.random.normal(0, 1, 100), 2) noise_mat = np.mat(noise) # 將z的觀測值和噪聲相加 z_mat = z_watch + noise_mat #print(z_watch) # 定義x的初始狀態 x_mat = np.mat([[0,], [0,]]) # 定義初始狀態協方差矩陣 p_mat = np.mat([[1, 0], [0, 1]]) # 定義狀態轉移矩陣，因為每秒鍾采一次樣，所以delta_t = 1 f_mat = np.mat([[1, 1], [0, 1]]) # 定義狀態轉移協方差矩陣，這里我們把協方差設置的很小，因為覺得狀態轉移矩陣准確度高 q_mat = np.mat([[0.0001, 0], [0, 0.0001]]) # 定義觀測矩陣 h_mat = np.mat([1, 0]) # 定義觀測噪聲協方差 r_mat = np.mat([1]) for i in range(100): x_predict = f_mat * x_mat p_predict = f_mat * p_mat * f_mat.T + q_mat kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat) x_mat = x_predict + kalman *(z_mat[0, i] - h_mat * x_predict) p_mat = (np.eye(2) - kalman * h_mat) * p_predict plt.plot(x_mat[0, 0], x_mat[1, 0], 'ro', markersize = 1) plt.show() 

         運行結果如下：

                                     

        橫坐標表示離初始位置的距離p，縱坐標表示在該位置的速度v。可以發現，開始時預測值和實際值有較大出入，在經過一段很短的時間后，速度預測值與實際值1基本就很接近了！