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

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

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

即：  (1)

而此時(shí)，(2)

?如果我們加入額外的控制量，比如加速度，此時(shí)

?則此時(shí)：

(3)

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

(4)

(5)

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

(6)

(7)

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

?圖2 估計(jì)值與觀測(cè)值概率分布示意圖

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

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

(8)

(9)

(10)

?式中：

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

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

(11)

(12)

?(13)

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

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

?(14)

(15)

?(16)

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

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

2. 算法實(shí)現(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)典卡爾曼濾波對(duì)虛擬信號(hào)及真實(shí)信號(hào)進(jìn)行濾波，結(jié)果如下圖所示：

?圖3 經(jīng)典卡爾曼濾波對(duì)虛擬信號(hào)濾波結(jié)果

?圖4 經(jīng)典卡爾曼濾波對(duì)真實(shí)信號(hào)濾波結(jié)果

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

建議應(yīng)用場(chǎng)合：輸入信號(hào)相對(duì)平穩(wěn)或已知被測(cè)系統(tǒng)運(yùn)動(dòng)學(xué)模型，同時(shí)要求運(yùn)算量極小的場(chǎng)合。

作者：小石學(xué)CS