濾波演算法：經典卡爾曼濾波

這兩天學習了一些卡爾曼濾波演算法的相關知識。相比其它的濾波演算法，卡爾曼濾波在對計算量需求非常之低，同時又能達到相當不錯的濾波結果。

1. 演算法原理

網上看到一篇文章http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/對卡爾曼濾波講解的十分形象透徹，國內也有這篇文章的中文翻譯版，連結：63253509，這裡還是先簡單的介紹一下。

卡爾曼濾波實質上就是基於觀測值以及估計值二者的資料對真實值進行估計的過程。預測步驟1所示：

 

圖1 卡爾曼濾波原理流程圖

 

假設我們能夠得到被測物體的位置和速度的測量值，在已知上一時刻的最優估計值以及它的共變數矩陣的條件下（初始值可以隨意取，但共變數矩陣應為非0矩陣），則有，，即：

                                             (1)

而此時，

                   (2)

如果我們加入額外的控制量，比如加速度，此時，，則此時：

                                (3)

 

同時，我們認為我們對系統的估計值並非完全準確，比如運動物體會突然打滑之類的，即存在一個共變數為的雜訊幹擾。因此，我們需要對加上系統雜訊來保證描述的完備性。綜上，預測步驟的表達如下所示：

                                                        (4)

                                                       (5)

由於誤差累積的作用，單純對系統進行估計會導致估計值越來越離譜，因此我們以感應器的觀測資料對我們的估計進行修正。我們可以用與預測步驟類似的方法將估計值空間映射至觀測值空間，如下式所示：

                                                            (6)

                    (7)

我們假設觀測值為。同時由於觀測資料同樣會存在雜訊幹擾問題，比如感應器雜訊等，我們將這種雜訊的分布用共變數表示。此時，觀測值與估計值處於相同的狀態空間，但具有不同的機率分布，2所示：

 

圖2 估計值與觀測值機率分布

 

我們可以認為，這兩個機率分布的重疊部分，會更加趨近系統的真實資料，即有更高的信賴度，比如我們估計汽車速度是5~10km/h，感應器反饋的速度是8~12km/h，那我們有理由認為汽車的實際速度更趨近於8~10km/h這個區間。

這裡將觀測值與估計值兩個分布的高斯分布相乘，其結果的高斯分布描述如下：

                                                            (8)

                                                        (9)

                                                                  (10)

式中：描述高斯分布的共變數，表示高斯分布的均值，矩陣稱為卡爾曼增益矩陣。

那麼，將估計值以及觀測值代入式(8)至式(10)，可以得到：

               (11)

                                              (12)

               (13)

式中，稱為卡爾曼增益。

將式(11)至式(13)中約去，並化簡可得：

                                                     (14)

                                                         (15)

                                                                  (16)

即為我們所得到的最優估計值，同時為其對應的共變數矩陣。在實際應用中，只需要使用式(4)、式(5)以及式(14)至式(16)這5個方程即可實現完整的卡爾曼濾波過程。

在對單一訊號源濾波的場合，由於測量值與估計值具備幾乎完全相同的機率分布，為了更好的實現去噪效果，在假定被測對象變化不顯著的情況下，可以將之前（1~N）個時間節點的測量值隨機作為目前時間節點的測量值，以實現更好的去噪效果。原則上，N取值越大濾波效果越好，但也會導致濾波結果滯後越嚴重。

2. 演算法實現

function output = kalmanFilter(data, Q, R, N) if ~exist('Q', 'var')    Q = 0.01;endif ~exist('R', 'var')    R = 1;endif ~exist('N', 'var')    N = 0;end X = 0;P = 1;A = 1;H = 1; output = zeros(size(data)); for ii = N + 1 : length(data)   X_k = A * X;   P_k = A * P * A' + Q;   Kg = P_k * H' / (H * P_k * H' + R);   z_k = data(ii - round(rand() * N));   X = X_k + Kg * (z_k - H * X_k);   P = (1 - Kg*H) * P_k;   output(ii) = X;end end

 

3. 演算法分析

採用經典卡爾曼濾波對虛擬訊號及真實訊號進行濾波，結果如所示：

 

圖3 經典卡爾曼濾波對虛擬訊號濾波結果

 

圖4 經典卡爾曼濾波對真實訊號濾波結果

 

從濾波結果中可以看出，經典卡爾曼對訊號的濾波效果較為優秀，即時性相對較好，計算量需求極小，能夠有效去除高斯雜訊以及非高斯雜訊，基本不受脈衝訊號影響。在對被測系統的建模較為精確的條件下，其效能還能夠進一步提升。其缺點主要在於需人為給定系統模型，當系統模型不精確時濾波效果會有所下降，但可以通過增加採樣頻率解決此問題。

建議應用場合：輸入訊號相對平穩或已知被測系統運動學模型，同時要求運算量極小的場合。