仔細(xì)看完你就懂卡爾曼濾波（Kalman Filter）

一、引言

以下我們引用文獻(xiàn)【1】中的一段話作為本文的開始：

想象你在黃昏時分看著一僅僅小鳥飛行穿過濃密的叢林。你僅僅能隱隱約約、斷斷續(xù)續(xù)地瞥見小鳥運(yùn)動的閃現(xiàn)。你試圖努力地猜測小鳥在哪里以及下一時刻它會出如今哪里，才不至于失去它的行蹤。或者再想象你是二戰(zhàn)中的一名雷達(dá)操作員，正在跟蹤一個微弱的游移目標(biāo)。這個目標(biāo)每隔10秒鐘在屏幕上閃爍一次。

或者回到更遠(yuǎn)的從前。想象你是開普勒，正試圖依據(jù)一組通過不規(guī)則和不準(zhǔn)確的測量間隔得到的非常不精確的角度觀測值來又一次構(gòu)造行星的運(yùn)動軌跡。在全部這些情況下。你都試圖依據(jù)隨對問變化并且?guī)в性肼暤挠^察數(shù)據(jù)去預(yù)計物理系統(tǒng)的狀態(tài)（比如位置、速度等等）。這個問題能夠被形式化表示為時序概率模型上的推理，模型中的轉(zhuǎn)移模型描寫敘述了運(yùn)動的物理本質(zhì)，而傳感器模型則描寫敘述了測量過程。

為解決這類問題。人們發(fā)展出來了一種特殊的表示方法和推理算法——卡爾曼濾波。

二、基本概念

回憶一下HMM的基本模型（例如以下圖所看到的）。當(dāng)中涂有陰影的圓圈（yt-2,yt-1,yt）相當(dāng)于是觀測變量，空白圓圈（xt-2,xt-1,xt）相當(dāng)于是隱變量。

這事實上揭示了卡爾曼濾波與HMM之間擁有非常深的淵源。

回到剛剛提及的那幾個樣例，你所觀測到的物體狀態(tài)（比如雷達(dá)中目標(biāo)的位置或者速度）相當(dāng)于是對其真實狀態(tài)的一種預(yù)計（由于觀測的過程中必定存在噪聲），用數(shù)學(xué)語言來表述就是P(yt|xt)，這就是模型中的測量模型或測量概率（Measurement Probability）。另外一方面，物體當(dāng)前的（真實）狀態(tài)應(yīng)該與其上一個觀測狀態(tài)相關(guān)，即存在這樣的一個分布P(xt|xt-1)，這就是模型中的轉(zhuǎn)移模型或轉(zhuǎn)移概率（Transition Probability）。當(dāng)然，HMM中隱變量必須都是離散的，觀測變量并無特殊要求。

而從信號處理的角度來講，濾波是從混合在一起的諸多信號中提取出所需信號的過程[2]。比如，我們有一組含有噪聲的行星執(zhí)行軌跡。我們希望濾除當(dāng)中的噪聲，預(yù)計行星的真實運(yùn)動軌跡。這一過程就是濾波。

假設(shè)從機(jī)器學(xué)習(xí)和數(shù)據(jù)挖掘的角度來說。濾波是一個理性智能體為了把握當(dāng)前狀態(tài)以便進(jìn)行理性決策所採取的行動[1]。比方，前兩天我們沒出門，可是我們能夠從房間里觀察路上的行人有沒有打傘（觀測狀態(tài)）來預(yù)計前兩天有沒有下雨（真實狀態(tài)）。

基于這些情況，如今我們要來決策今天（是否會有雨以及）外出是否須要打傘。這個過程就是濾波。

讀者應(yīng)該注意把握上面兩個定義的統(tǒng)一性。

所謂預(yù)計就是依據(jù)測量得出的與狀態(tài)X(t) 有關(guān)的數(shù)據(jù)Y(t) =h[X(t)] + V(t) 解算出X(t)的計算值，當(dāng)中隨機(jī)向量V(t) 為測量誤差，稱為X的預(yù)計，Y 稱為 X 的測量。

由于是依據(jù)Y(t) 確定的．所以?是Y(t) 的函數(shù)。

若?是Y 的線性函數(shù)。則 X?稱作 X 的線性預(yù)計。設(shè)在 [t0,?t1] 時間段內(nèi)的測量為Y。對應(yīng)的預(yù)計為。則

當(dāng)t=t1時。?稱為X(t)的預(yù)計。

當(dāng)t>t1肘，稱為X(t)的預(yù)測；

當(dāng)t稱為X(t)的平滑。

最優(yōu)預(yù)計是指某一指標(biāo)函數(shù)達(dá)到最值時的預(yù)計。

卡爾曼濾波就是一種線性最優(yōu)濾波器。

由于后面會用到。這里我們補(bǔ)充一下關(guān)于協(xié)方差矩陣的概念。

設(shè)n維隨機(jī)變量（X1,X2,…,Xn）的2階混合中心距

σij= cov(Xi,Xj) = E[(Xi-E(Xi))(Xj-E(Xj))], (i,j = 1, 2,…,n)

都存在，則稱矩陣

為n維隨機(jī)變量（X1,X2,…,Xn）的協(xié)方差矩陣，協(xié)方差矩陣是一個對稱矩陣，并且對角線是各個維度的方差。

維基百科中還給出了協(xié)方差矩陣的一些重要性質(zhì)，比如以下這兩條（此處不做具體證明）。

興許的內(nèi)容會用到當(dāng)中的第一條。

三、卡爾曼濾波的方程推導(dǎo)

直接從數(shù)學(xué)公式和概念入手來考慮卡爾曼濾波無疑是一件非?？菰锏氖虑?。為了便于理解，我們?nèi)匀粡囊粋€現(xiàn)實中的實例開始以下的介紹。這一過程中你所需的預(yù)備知識僅僅是高中程度的物理學(xué)內(nèi)容。

假如如今有一輛在路上做直線運(yùn)動的小車（例如以下所看到的），該小車在t時刻的狀態(tài)能夠用一個向量來表示，當(dāng)中pt表示他當(dāng)前的位置，vt表示該車當(dāng)前的速度。當(dāng)然，司機(jī)還能夠踩油門或者剎車來給車一個加速度ut。ut相當(dāng)于是一個對車的控制量。顯然，假設(shè)司機(jī)既沒有踩油門也沒有踩剎車，那么ut就等于0。此時車就會做勻速直線運(yùn)動。

假設(shè)我們已知上一時刻t-1時小車的狀態(tài)。如今來考慮當(dāng)前時刻t小車的狀態(tài)。顯然有

易知。上述兩個公式中，輸出變量都是輸入變量的線性組合，這也就是稱卡爾曼濾波器為線性濾波器的原因所在。既然上述公式表征了一種線性關(guān)系。那么我們就能夠用一個矩陣來表示它，則有

假設(shè)另當(dāng)中的

則得到卡爾曼濾波方程組中的第一條公式——狀態(tài)預(yù)測公式，而F就是狀態(tài)轉(zhuǎn)移矩陣。它表示我們怎樣從上一狀態(tài)來猜測當(dāng)前狀態(tài)。而B則是控制矩陣，它表示控制量u怎樣作用于當(dāng)前狀態(tài)。

???（1）

上式中x頂上的hat表示為預(yù)計值（而非真實值）。等式左端部分的右上標(biāo)“-”表示該狀態(tài)是依據(jù)上一狀態(tài)猜測而來的，稍后我們還將對其進(jìn)行修正以得到最優(yōu)預(yù)計。彼時才干夠?qū)ⅰ?”去掉。

既然我們是在對真實值進(jìn)行預(yù)計，那么就理應(yīng)考慮到噪聲的影響。

實踐中，我們通常都是假設(shè)噪聲服從一個0均值的高斯分布。即Noise~Guassian(0,σ)。

比如對于一個一維的數(shù)據(jù)進(jìn)行預(yù)計時，若要引入噪聲的影響。事實上僅僅要考慮當(dāng)中的方差σ就可以。當(dāng)我們將維度提高之后。為了綜合考慮各個維度偏離其均值的程度，就須要引入?yún)f(xié)方差矩陣。

回到我們的樣例。系統(tǒng)中每一個時刻的不確定性都是通過協(xié)方差矩陣Σ來給出的。

并且這樣的不確定性在每一個時刻間還會進(jìn)行傳遞。也就是說不僅當(dāng)前物體的狀態(tài)（比如位置或者速度）是會（在每一個時刻間）進(jìn)行傳遞的，并且物體狀態(tài)的不確定性也是會（在每一個時刻間）進(jìn)行傳遞的。這樣的不確定性的傳遞就能夠用狀態(tài)轉(zhuǎn)移矩陣來表示，即（注意。這里用到了前面給出的關(guān)于協(xié)方差矩陣的性質(zhì)）

可是我們還應(yīng)該考慮到。預(yù)測模型本身也并不絕對準(zhǔn)確的，所以我們要引入一個協(xié)方差矩陣Q來表示預(yù)測模型本身的噪聲（也即是噪聲在傳遞過程中的不確定性），則有

? （2）

這就是卡爾曼濾波方程組中的第二條公式，它表示不確定性在各個時刻間的傳遞關(guān)系。

繼續(xù)我們的小汽車樣例。你應(yīng)該注意到，前面我們所討論的內(nèi)容都是環(huán)繞小汽車的真實狀態(tài)展開的。

而真實狀態(tài)我們事實上是無法得知的，我們僅僅能通過觀測值來對真實值進(jìn)行預(yù)計。

所以如今我們在路上布設(shè)了一個裝置來測定小汽車的位置。觀測到的值記為Y(t)。

并且從小汽車的真實狀態(tài)到其觀測狀態(tài)另一個變換關(guān)系。這個變換關(guān)系我們記為h(?)。并且這個h(?)還是一個線性函數(shù)。此時便有（該式前面以前給出過）

Y(t) =h[X(t)] + V(t)

當(dāng)中V(t)表示觀測的誤差。既然h(?)還是一個線性函數(shù)，所以我們相同能夠把上式改寫成矩陣的形式。則有

Yt=Hxt+ v

就本例而言，觀測矩陣H= [1 0]，這事實上告訴我們x和Z的維度不一定非得相同。

在我們的樣例中，x是一個二維的列向量，而Z僅僅是一個標(biāo)量。此時當(dāng)把x與上面給出的H相乘就會得出一個標(biāo)量，此時得到的Y就是x中的首個元素，也就是小車的位置。

相同，我們還須要用一個協(xié)方差矩陣R來代替上述式子中的v來表示觀測中的不確定性。

當(dāng)然，由于Z是一個一維的值，所以此時協(xié)方差矩陣R也僅僅有一維，也就是僅僅有一個值，即觀測噪聲之高斯分布的參數(shù)σ。假設(shè)我們有非常多裝置來測量小汽車的不同狀態(tài)，那么Z就會是一個包括全部觀測值的向量。

接下來要做的事情就是對前面得出的狀態(tài)預(yù)計進(jìn)行修正，具體而言就是利用以下這個式子

??? （4）

直觀上來說，上式并不難理解。前面我們提到。是依據(jù)上一狀態(tài)猜測而來的。那么它與“最優(yōu)”預(yù)計值之間的差距如今就是等式右端加號右側(cè)的部分。表示實際觀察值與預(yù)估的觀測值之間的殘差。這個殘差再乘以一個系數(shù)K就能夠用來對預(yù)計值進(jìn)行修正。

K稱為卡爾曼系數(shù)，它也是一個矩陣，它是對殘差的加權(quán)矩陣。有的資料上稱其為濾波增益陣。

?? （3）

上式的推導(dǎo)比較復(fù)雜，有興趣深入研究的讀者能夠參閱文獻(xiàn)【2】（P35~P37）。

假設(shè)有時間我會在后面再做具體推導(dǎo)?？墒侨缃裎覀?nèi)匀荒軌蚨ㄐ缘貙@個系數(shù)進(jìn)行解讀：濾波增益陣首先權(quán)衡預(yù)測狀態(tài)協(xié)方差矩陣Σ和觀測值矩陣R的大小。并以此來認(rèn)為我們是更傾向于相信預(yù)測模型還是具體觀測模型。

假設(shè)相信預(yù)測模型多一點(diǎn)。那么這個殘差的權(quán)重就會小一點(diǎn)。反之亦然。假設(shè)相信觀察模型多一點(diǎn)，這個殘差的權(quán)重就會大一點(diǎn)。不僅如此，濾波增益陣還負(fù)責(zé)把殘差的表現(xiàn)形式從觀測域轉(zhuǎn)換到了狀態(tài)域。

比如本題中觀測值Z僅僅是一個一維的向量，狀態(tài)x是一個二維的向量。

所以在實際應(yīng)用中，觀測值與狀態(tài)值所採用的描寫敘述特征或者單位都有可能不同，顯然直接用觀測值的殘差去更新狀態(tài)值是不合理的。

而利用卡爾曼系數(shù)，我們就能夠完畢這樣的轉(zhuǎn)換。比如。在小車運(yùn)動這個樣例中，我們僅僅觀察到了汽車的位置，但K里面已經(jīng)包括了協(xié)方差矩陣P的信息（P里面就給出了速度和位置的相關(guān)性）。所以它利用速度和位置這兩個維度的相關(guān)性，從位置的殘差中推算出了速度的殘差。

從而讓我們能夠?qū)顟B(tài)值x的兩個維度同一時候進(jìn)行修正。

最后，還需對最優(yōu)預(yù)計值的噪聲分布進(jìn)行更新。所使用的公式為

?（5）

至此。我們便獲得了實現(xiàn)卡爾曼濾波所需的全部五個公式，我在前面分別用（1）~（5）的標(biāo)記進(jìn)行了編號。我如今把它們再次羅列出來：

我們將這五個公式分成預(yù)測組和更新組。

預(yù)測組總是依據(jù)前一個狀態(tài)來預(yù)計當(dāng)前狀態(tài)。更新組則依據(jù)觀測信息來對預(yù)測信息進(jìn)行修正。以期達(dá)到最優(yōu)預(yù)計之目的。

四、一個簡單的實例

當(dāng)然，你可能困惑于卡爾曼濾波是否真的有效。以下利用文獻(xiàn)[4]中給出的樣例（為提升顯示效果。筆者略有改動）來演示卡爾曼濾波的威力。

這個樣例模擬質(zhì)點(diǎn)進(jìn)行勻速直線運(yùn)動（速度為1），然后引入一個非常大的噪聲。再用卡爾曼濾波來對質(zhì)點(diǎn)的運(yùn)動狀態(tài)進(jìn)行軌跡。注意是勻速直線運(yùn)動。所以當(dāng)中不含有控制變量。

Z=(1:100); %觀測值
noise=randn(1,100); %方差為1的高斯噪聲
Z=Z+noise;
  
X=[0; 0]; %狀態(tài)
Sigma = [1 0; 0 1]; %狀態(tài)協(xié)方差矩陣
F=[1 1; 0 1]; %狀態(tài)轉(zhuǎn)移矩陣
Q=[0.0001, 0; 0 0.0001]; %狀態(tài)轉(zhuǎn)移協(xié)方差矩陣
H=[1 0]; %觀測矩陣
R=1; %觀測噪聲方差
  
figure;
hold on;
  
for i=1:100
  
  X_ = F*X;
  Sigma_ = F*Sigma*F'+Q;
  K = Sigma_*H'/(H*Sigma_*H'+R);
  X = X_+K*(Z(i)-H*X_);
  Sigma = (eye(2)-K*H)*Sigma_;
    
  plot(X(1), X(2), '.','MarkerSize',10); %畫點(diǎn)，橫軸表示位置。縱軸表示速度
end

plot([0,100],[1,1],'r-');

下圖給出了上述代碼的執(zhí)行結(jié)果。

可見經(jīng)過最開始的幾次迭代后。質(zhì)點(diǎn)運(yùn)動的狀態(tài)預(yù)計就回到了正確軌跡上，并且預(yù)計的結(jié)果基本環(huán)繞在真實值附近，效果還是非常理想的。

審核編輯：湯梓紅