這兩天學(xué)習(xí)了一些卡爾曼濾波算法的相關(guān)知識,。相比其它的濾波算法,，卡爾曼濾波在對計算量需求非常之低，同時又能達(dá)到相當(dāng)不錯的濾波結(jié)果,。

1. 算法原理

網(wǎng)上看到一篇文章http://www./p/how-a-kalman-filter-works-in-pictures/對卡爾曼濾波講解的十分形象透徹,，國內(nèi)也有這篇文章的中文翻譯版，鏈接：https://blog.csdn.net/u010720661/article/details/63253509，這里還是先簡單的介紹一下,。

卡爾曼濾波實質(zhì)上就是基于觀測值以及估計值二者的數(shù)據(jù)對真實值進(jìn)行估計的過程,。預(yù)測步驟如圖1所示：

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

假設(shè)我們能夠得到被測物體的位置和速度的測量值，在已知上一時刻的最優(yōu)估計值以及它的協(xié)方差矩陣的條件下（初始值可以隨意取,，但協(xié)方差矩陣應(yīng)為非0矩陣）,，則有，,，即：

                                             (1)

而此時,，

                   (2)

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

                                (3)

同時,，我們認(rèn)為我們對系統(tǒng)的估計值并非完全準(zhǔn)確,，比如運(yùn)動物體會突然打滑之類的，即存在一個協(xié)方差為的噪聲干擾,。因此,，我們需要對加上系統(tǒng)噪聲來保證描述的完備性。綜上,，預(yù)測步驟的表達(dá)如下所示：

                                                        (4)

                                                       (5)

由于誤差累積的作用,，單純對系統(tǒng)進(jìn)行估計會導(dǎo)致估計值越來越離譜，因此我們以傳感器的觀測數(shù)據(jù)對我們的估計進(jìn)行修正,。我們可以用與預(yù)測步驟類似的方法將估計值空間映射至觀測值空間,，如下式所示：

                                                            (6)

                    (7)

我們假設(shè)觀測值為。同時由于觀測數(shù)據(jù)同樣會存在噪聲干擾問題,，比如傳感器噪聲等,，我們將這種噪聲的分布用協(xié)方差表示。此時,，觀測值與估計值處于相同的狀態(tài)空間,，但具有不同的概率分布，如圖2所示：

圖2 估計值與觀測值概率分布示意圖

我們可以認(rèn)為,，這兩個概率分布的重疊部分,，會更加趨近系統(tǒng)的真實數(shù)據(jù)，即有更高的置信度,，比如我們估計汽車速度是5~10km/h,，傳感器反饋的速度是8~12km/h，那我們有理由認(rèn)為汽車的實際速度更趨近于8~10km/h這個區(qū)間,。

這里將觀測值與估計值兩個分布的高斯分布相乘,，其結(jié)果的高斯分布描述如下：

                                                            (8)

                                                        (9)

                                                                  (10)

式中：描述高斯分布的協(xié)方差，表示高斯分布的均值,，矩陣稱為卡爾曼增益矩陣,。

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

               (11)

                                              (12)

               (13)

式中,，稱為卡爾曼增益。

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

                                                     (14)

                                                         (15)

                                                                  (16)

即為我們所得到的最優(yōu)估計值,，同時為其對應(yīng)的協(xié)方差矩陣。在實際應(yīng)用中,，只需要使用式(4),、式(5)以及式(14)至式(16)這5個方程即可實現(xiàn)完整的卡爾曼濾波過程。

在對單一信號源濾波的場合,，由于測量值與估計值具備幾乎完全相同的概率分布,，為了更好的實現(xiàn)去噪效果，在假定被測對象變化不顯著的情況下,，可以將之前（1~N）個時間節(jié)點的測量值隨機(jī)作為當(dāng)前時間節(jié)點的測量值,，以實現(xiàn)更好的去噪效果。原則上,，N取值越大濾波效果越好,，但也會導(dǎo)致濾波結(jié)果滯后越嚴(yán)重。

2. 算法實現(xiàn)

function output = kalmanFilter(data, Q, R, N)
 
if ~exist('Q', 'var')
    Q = 0.01;
end
if ~exist('R', 'var')
    R = 1;
end
if ~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. 算法分析

采用經(jīng)典卡爾曼濾波對虛擬信號及真實信號進(jìn)行濾波,，結(jié)果如下圖所示：

圖3 經(jīng)典卡爾曼濾波對虛擬信號濾波結(jié)果

圖4 經(jīng)典卡爾曼濾波對真實信號濾波結(jié)果

從濾波結(jié)果中可以看出,，經(jīng)典卡爾曼對信號的濾波效果較為優(yōu)秀，實時性相對較好,，計算量需求極小,，能夠有效去除高斯噪聲以及非高斯噪聲，基本不受脈沖信號影響,。在對被測系統(tǒng)的建模較為精確的條件下,，其性能還能夠進(jìn)一步提升。其缺點主要在于需人為給定系統(tǒng)模型,，當(dāng)系統(tǒng)模型不精確時濾波效果會有所下降,，但可以通過增加采樣頻率解決此問題。

建議應(yīng)用場合：輸入信號相對平穩(wěn)或已知被測系統(tǒng)運(yùn)動學(xué)模型,，同時要求運(yùn)算量極小的場合,。