作為最早應(yīng)用在美國阿波羅登月項(xiàng)目上的kalman濾波，在很多領(lǐng)域都有不同的應(yīng)用，這篇文章主要對kalman的主要原理進(jìn)行介紹，并在最后的部分給出了一個(gè)簡單的實(shí)現(xiàn)。
狀態(tài)轉(zhuǎn)移
假設(shè)我們對于一輛汽車的狀態(tài)進(jìn)行觀測，觀測的狀態(tài)包括了位置和速度信息，該汽車在時(shí)刻t的狀態(tài)可以表示為
其中位置 Pt表示汽車的位置狀態(tài)，vt為汽車的速度狀態(tài)，考慮加速度ut,以及相鄰時(shí)間間隔△t，具體可以寫成
可以發(fā)現(xiàn)，上式中的狀態(tài)輸出為輸入的線性組合，這也是為什么kalman濾波器又叫做最佳線性濾波器的原因。
將上述線性組合用矩陣表示，為
其中令Ft表示狀態(tài)轉(zhuǎn)移矩陣，Bt為控制矩陣，分別為
這樣就可以得到Kalman濾波的第一個(gè)公式，狀態(tài)預(yù)測公式
這里的變量上面戴了個(gè)尖帽子，這是由于這個(gè)變量是個(gè)估計(jì)量，另外，變量右上角的-則表示這個(gè)變量是由上一時(shí)刻的推測得到，既然是由推測得到，說明這個(gè)值是需要進(jìn)行修正的，后面我們將會具體介紹如何進(jìn)行修正。
回到上面的狀態(tài)預(yù)測公式，我們利用這個(gè)公式推測出當(dāng)前的狀態(tài)，對當(dāng)前的狀態(tài)進(jìn)行預(yù)測會不會存在什么問題呢？答案是肯定的，在預(yù)測的過程中，是會收到噪聲的影響的，含有的噪聲越大，我們預(yù)測得出的狀態(tài)它的不確定性就會越大。
關(guān)于這種不確定性，需要通過協(xié)方差矩陣進(jìn)行衡量。
下面就對協(xié)方差矩陣進(jìn)行了解，協(xié)方差矩陣除了表示了兩個(gè)維度變量的方差，還體現(xiàn)了兩個(gè)維度變量之間的相關(guān)性。其矩陣中的正對角線即為兩個(gè)變量的方差，負(fù)對角線則為兩個(gè)變量之間的協(xié)方差，如果兩個(gè)變量的協(xié)方差值為正，說明兩個(gè)變量是正相關(guān)的，為負(fù)則表示負(fù)相關(guān)，不相關(guān)即為0.
在搞清楚了如何衡量狀態(tài)預(yù)測過程中的不確定性后，可以理解的是，這種不確定性并不會永恒不變，我們在不斷進(jìn)行狀態(tài)預(yù)測的同時(shí)，也需要了解這個(gè)時(shí)候狀態(tài)預(yù)測帶來的不確定性是如何變化的。
噪聲協(xié)方差矩陣的傳遞
這種不確定性的變化，我們將其定義為噪聲協(xié)方差矩陣的傳遞，也就是表明了不確定性在各個(gè)時(shí)刻時(shí)的傳遞關(guān)系，對于噪聲協(xié)方差矩陣的傳遞，可以表示為
上面的式子中有用到協(xié)方差矩陣的一個(gè)性質(zhì)，
由上一時(shí)刻的噪聲協(xié)方差矩陣推測出當(dāng)前狀態(tài)下的噪聲協(xié)方差矩陣，在傳遞的過程中，還需要考慮由于預(yù)測模型本身帶來的噪聲影響，用Q表示預(yù)測模型本身帶來的噪聲的協(xié)方差矩陣。
這個(gè)時(shí)候，我們已經(jīng)得到當(dāng)前時(shí)刻的狀態(tài)預(yù)測以及此時(shí)的噪聲協(xié)方差矩陣，當(dāng)我們完成預(yù)測，出現(xiàn)的一個(gè)新的問題就是，這個(gè)預(yù)測的結(jié)果我們可以直接用嗎？答案顯然是不可以的，畢竟我們預(yù)測的值同實(shí)際的值之間還是存在一定偏差的，既然存在偏差，就需要對預(yù)測的狀態(tài)進(jìn)行修正，也就是所謂的狀態(tài)更新。
狀態(tài)更新
在對預(yù)測狀態(tài)進(jìn)行更新的時(shí)候，需要考量預(yù)測狀態(tài)與觀測值之間的偏差，若觀測值為Zt,其可以表示為
其中H為觀測矩陣，v表示觀測過程中引入的噪聲，其協(xié)方差矩陣為R。假設(shè)這里我們只對汽車運(yùn)動狀態(tài)中的位置進(jìn)行觀測，那么此時(shí)的觀測矩陣可以寫成
得到觀測矩陣后，利用實(shí)際觀測值和預(yù)期觀測值，可以計(jì)算兩者之間的殘差為
根據(jù)這個(gè)殘差，我們可以對預(yù)期的觀測值進(jìn)行修正，具體的修正表述為
其中Kt為kalman增益，它的作用主要是權(quán)衡預(yù)測狀態(tài)中的協(xié)方差矩陣P和觀測時(shí)候引入的噪聲協(xié)方差矩陣R的大小，根據(jù)兩者大小判斷最終的更新是相信預(yù)測模型還是觀測模型多一點(diǎn)。這個(gè)Kt的推導(dǎo)較為復(fù)雜，直接給出為
至此，我們可以得到當(dāng)前狀態(tài)的更新，除了對狀態(tài)進(jìn)行更新，同樣的噪聲協(xié)方差矩陣也需要進(jìn)行更新，更新公式為
綜上，我們將kalman濾波的幾個(gè)公式放在一起，分成兩個(gè)部分，一個(gè)部分進(jìn)行預(yù)測，另外一個(gè)則進(jìn)行更新。
在搞清楚了Kalman濾波后，下面考慮如何利用一個(gè)簡單的例子進(jìn)行建模，設(shè)小雷開著車，這輛車以1m/s的速度進(jìn)行行駛，在100m的位置處停止。
首先可以得到觀測值
Zt = 1:100;
前面有提到，在觀測的時(shí)候會引入噪聲，所以我們給觀測值加了方差為1的高斯白噪聲，這個(gè)地方我又在生成的噪聲后面乘了0.2，目的是為了使例子的效果更加直觀，減小了觀測時(shí)的噪聲影響。
Zt_noise = randn(1,100);
Zt_noise = Zt_noise * 0.2;
所以最終構(gòu)建的觀測值就為
Zt = Zt + Zt_noise;
然后構(gòu)造預(yù)測模型中的預(yù)測初始狀態(tài)，狀態(tài)協(xié)方差矩陣
Xt = [0; 0];
Pt = [1 0; 0 1];
設(shè)置狀態(tài)轉(zhuǎn)移矩陣Ft，以及狀態(tài)轉(zhuǎn)移所在的預(yù)測模型本身帶來的噪聲的協(xié)方差矩陣Q
Ft = [1 1; 0 1];
Q = [0.0001 0; 0 0.0001];
除了上述需要構(gòu)建的變量外，觀察kalman濾波的五個(gè)公式，我們還需要觀測矩陣和觀測噪聲的協(xié)方差矩陣R
H = [1 0];
R = 1;
最后直接利用kalman濾波的五個(gè)公式進(jìn)行不斷迭代計(jì)算，并將最后的結(jié)果plot了出來
for ii = 1:100
Xt_ = Ft * Xt;
Pt_ = Ft * Pt * Ft' + Q;
Kt = Pt_ * H'/(H * Pt_ * H' + R);
Xt = Xt_ + Kt * (Zt(ii) - H * Xt_);
Pt = (eye(2) - Kt * H) * Pt_;
end
得到的Kalman濾波結(jié)果如下
這個(gè)系列的內(nèi)容基本寫完，有空接著寫。
題圖：RyanMcGuire，from Pixabay
