作為最早應(yīng)用在美國阿波羅登月項(xiàng)目上的kalman濾波绪氛，在很多領(lǐng)域都有不同的應(yīng)用懂傀，這篇文章主要對(duì)kalman的主要原理進(jìn)行介紹流强，并在最后的部分給出了一個(gè)簡單的實(shí)現(xiàn)慎玖。
狀態(tài)轉(zhuǎn)移
假設(shè)我們對(duì)于一輛汽車的狀態(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)行修正的秉沼，后面我們將會(huì)具體介紹如何進(jìn)行修正桶雀。
回到上面的狀態(tài)預(yù)測公式，我們利用這個(gè)公式推測出當(dāng)前的狀態(tài)唬复，對(duì)當(dāng)前的狀態(tài)進(jìn)行預(yù)測會(huì)不會(huì)存在什么問題呢矗积？答案是肯定的，在預(yù)測的過程中敞咧，是會(huì)收到噪聲的影響的棘捣，含有的噪聲越大，我們預(yù)測得出的狀態(tài)它的不確定性就會(huì)越大休建。
關(guān)于這種不確定性乍恐，需要通過協(xié)方差矩陣進(jìn)行衡量。
下面就對(duì)協(xié)方差矩陣進(jìn)行了解测砂，協(xié)方差矩陣除了表示了兩個(gè)維度變量的方差茵烈，還體現(xiàn)了兩個(gè)維度變量之間的相關(guān)性。其矩陣中的正對(duì)角線即為兩個(gè)變量的方差砌些，負(fù)對(duì)角線則為兩個(gè)變量之間的協(xié)方差呜投，如果兩個(gè)變量的協(xié)方差值為正，說明兩個(gè)變量是正相關(guān)的存璃，為負(fù)則表示負(fù)相關(guān)仑荐，不相關(guān)即為0.
在搞清楚了如何衡量狀態(tài)預(yù)測過程中的不確定性后，可以理解的是纵东，這種不確定性并不會(huì)永恒不變粘招，我們?cè)诓粩噙M(jìn)行狀態(tài)預(yù)測的同時(shí)，也需要了解這個(gè)時(shí)候狀態(tài)預(yù)測帶來的不確定性是如何變化的篮迎。
噪聲協(xié)方差矩陣的傳遞
這種不確定性的變化男图，我們將其定義為噪聲協(xié)方差矩陣的傳遞，也就是表明了不確定性在各個(gè)時(shí)刻時(shí)的傳遞關(guān)系甜橱，對(duì)于噪聲協(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í)際的值之間還是存在一定偏差的谍憔，既然存在偏差匪蝙，就需要對(duì)預(yù)測的狀態(tài)進(jìn)行修正，也就是所謂的狀態(tài)更新习贫。
狀態(tài)更新
在對(duì)預(yù)測狀態(tài)進(jìn)行更新的時(shí)候逛球，需要考量預(yù)測狀態(tài)與觀測值之間的偏差，若觀測值為Zt,其可以表示為
其中H為觀測矩陣苫昌，v表示觀測過程中引入的噪聲颤绕，其協(xié)方差矩陣為R。假設(shè)這里我們只對(duì)汽車運(yùn)動(dòng)狀態(tài)中的位置進(jìn)行觀測祟身，那么此時(shí)的觀測矩陣可以寫成
得到觀測矩陣后奥务，利用實(shí)際觀測值和預(yù)期觀測值，可以計(jì)算兩者之間的殘差為
根據(jù)這個(gè)殘差月而，我們可以對(duì)預(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)的更新世杀，除了對(duì)狀態(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í)候會(huì)引入噪聲，所以我們給觀測值加了方差為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
