Kalman filtering--卡爾曼濾波

參考資料：

https://mp.weixin.qq.com/s/SuDysn3Y4li8fdxz3pqIKw

圖說(shuō)卡爾曼濾波，一份通俗易懂的教程 - 論智的文章 - 知乎 https://zhuanlan.zhihu.com/p/39912633

卡爾曼濾波(Kalman filter) 含詳細(xì)數(shù)學(xué)推導(dǎo) - Mockingjay的文章 - 知乎 https://zhuanlan.zhihu.com/p/134595781

http://t.csdn.cn/ZpWW1

開(kāi)啟煉丹之旅

以導(dǎo)航系統(tǒng)為例，物體在某一時(shí)刻的位置p以及速度v狀態(tài)可以用向量表示為：

由于傳感器的讀數(shù)是對(duì)當(dāng)前狀態(tài)的最佳估計(jì)，故存在不確定性，該不確定性可用協(xié)方差矩陣P表示，另一種理解為位置和當(dāng)前速度間是存在相關(guān)關(guān)系，不難想到當(dāng)期速度增加，下一期位置也就更遠(yuǎn)：

1、基礎(chǔ)預(yù)測(cè)

基于該信息可以預(yù)測(cè)其下一時(shí)期的狀態(tài)：

位置p=上期位置+移動(dòng)距離

速度v=上期狀態(tài)

矩陣形式為

??_??|?????表示基于t-1 期狀態(tài)對(duì)t 期進(jìn)行預(yù)測(cè)，其中????為狀態(tài)轉(zhuǎn)移矩陣，描述的是從上一狀態(tài)轉(zhuǎn)化到下一狀態(tài)的發(fā)生的變換。

協(xié)方差矩陣同樣會(huì)發(fā)生變化，????|?????表示表示基于t-1 期精度/不確定性/方差對(duì)t 期的進(jìn)行

預(yù)測(cè)，根據(jù)矩陣協(xié)方差性質(zhì)：

2、加入系統(tǒng)控制

當(dāng)向物體施加外力是會(huì)對(duì)其狀態(tài)產(chǎn)生影響，F(xiàn)=ma這里可以用加速度a表示外力影響：

矩陣形式：

其中Bt為狀態(tài)控制矩陣，該部分表示外力如何影響物體狀態(tài)/影響程度；ut為狀態(tài)控制向量，表示施加的外力大小。因?yàn)槭┘拥耐饬κ侨藶榭刂频?，不存在不確定性，所以協(xié)方差矩陣不作更新。

3、加入不確定因素

例如路面狀況導(dǎo)致輪胎打滑、逆風(fēng)阻力抵消施加的力等情況均為無(wú)法控制的外部的不確定因素。對(duì)于這一部分，假設(shè)該干擾????服從均值為0，標(biāo)準(zhǔn)差為????的正態(tài)分布。

此時(shí)狀態(tài)預(yù)測(cè)方程進(jìn)一步更新為：

到這里總結(jié)一下：

新的狀態(tài)估計(jì)=上期卡爾曼最佳估計(jì)???????+系統(tǒng)控制+不確定因素（此處假設(shè)均值為零）

新的不確定性=上期方差最優(yōu)估計(jì) ????|?????+外部不確定因素

4、狀態(tài)空間方程

以上物體運(yùn)動(dòng)可以用如下?tīng)顟B(tài)空間方程描述：

????|???1是基于上一狀態(tài)對(duì)當(dāng)前狀態(tài)的預(yù)測(cè)；狀態(tài)讀數(shù)????是傳感器對(duì)于真實(shí)狀態(tài)的感知加上傳感器自身精度誤差之和的結(jié)果。傳感器讀數(shù)永遠(yuǎn)不會(huì)等于真實(shí)狀態(tài)，但會(huì)是對(duì)物體真實(shí)狀態(tài)的映射，兩者存在對(duì)應(yīng)關(guān)系但絕非對(duì)等。這里引入關(guān)系矩陣????表示真實(shí)狀態(tài)與傳感器讀數(shù)的映射關(guān)系，同時(shí)用????表示噪聲，????后面進(jìn)一步解釋。

有了 ????|?????和????，怎么估計(jì)真實(shí)狀態(tài)真實(shí)狀態(tài) ????？？？

傳感器視角

預(yù)期觀測(cè)到什么？

以上“基礎(chǔ)預(yù)測(cè)+系統(tǒng)控制+不確定因素”共同預(yù)測(cè)物體下一狀態(tài)。此前對(duì)物體所做的預(yù)測(cè)狀態(tài)????|???1以及????|???1下，觀測(cè)到傳感器的觀測(cè)值應(yīng)該為：

此處是將預(yù)測(cè)狀態(tài)直接轉(zhuǎn)化為傳感器讀數(shù)，不存在額外的不確定性????，因此這時(shí)對(duì)應(yīng)的協(xié)方差矩陣：

此時(shí)完成了對(duì)觀測(cè)值的預(yù)測(cè)，預(yù)測(cè)其結(jié)果服從如下高斯分布：

實(shí)際觀測(cè)到什么？

再申：傳感器對(duì)當(dāng)前狀態(tài)讀數(shù)????是一種（最優(yōu)）估計(jì)，因此存在不確定性/噪聲，可認(rèn)為噪聲????服從均值為0，標(biāo)準(zhǔn)差為????的正態(tài)分布：

那么觀察結(jié)果服從以下分布：

Kalman煉丹大法

這里借用一張圖，出處為https://zhuanlan.zhihu.com/p/444977764

這幅圖展現(xiàn)是預(yù)測(cè)觀測(cè)結(jié)果預(yù)測(cè)觀結(jié)果 ??0（圖中用??????????????????表示 ）和實(shí)際觀測(cè)結(jié)果 ????的分布可能，卡爾曼濾波的核心就是要從這兩個(gè)分布中對(duì)真實(shí)結(jié)果進(jìn)行估計(jì)。黃色部分是兩個(gè)分布重疊部分，也就是概率更高的結(jié)果，如何獲取這部分？概率密度函數(shù)乘積

預(yù)期觀測(cè)結(jié)果與真實(shí)觀測(cè)結(jié)果的概率密度函數(shù)乘積

將預(yù)期觀測(cè)結(jié)果??(??0,??0)與真實(shí)觀測(cè)結(jié)果??(??1,??1)相乘可以得到以下新的正態(tài)分布（此處不涉及分布具體推導(dǎo)內(nèi)容，只對(duì)結(jié)果進(jìn)行概述）：

令

那么

矩陣形式

代入 ??0,??0,??1,??1

有

通過(guò)乘逆矩陣可以將上式化簡(jiǎn)為

其中??′就是卡爾曼增益

另一個(gè)角度

卡爾曼濾波的核心思想可以理解為加權(quán)，即通過(guò)對(duì)預(yù)測(cè)值????|???1（先驗(yàn)估計(jì)）和觀測(cè)值????賦予不同權(quán)重求和得到最優(yōu)估計(jì)（后驗(yàn)估計(jì)）。

那么該加權(quán)表達(dá)式可以寫(xiě)成

如何求得卡爾曼增益??′？最小化估計(jì)誤差

從最小化t期估計(jì)誤差????入手，估計(jì)誤差等于真實(shí)狀態(tài)????與最優(yōu)估計(jì)????′的差值

此時(shí)最優(yōu)估計(jì)協(xié)方差????

????的展開(kāi)式中包含卡爾曼增益??′，最小化估計(jì)誤差的目標(biāo)下，對(duì)??′求導(dǎo)并令其等于零，可以求解得到相同得結(jié)果

具體過(guò)程參考http://t.csdn.cn/ZpWW1

寫(xiě)在最后的一些思考：

1、為什么要將“預(yù)測(cè)狀態(tài)????|???1”再轉(zhuǎn)化為“預(yù)測(cè)的觀測(cè)結(jié)果????????|???1”？

我認(rèn)為是要兼顧傳感器的實(shí)際讀數(shù)這一分布，類(lèi)似統(tǒng)一量綱/統(tǒng)一單位，這樣得到的兩個(gè)分布都是基于傳感器視角的才能直接將其概率密度函數(shù)進(jìn)行相乘。否則????|???1是一個(gè)狀態(tài)分布，而????為傳感器讀數(shù)分布，兩者不能直接相乘。

2、為什么兩個(gè)分布密度函數(shù)相乘后左邊是????????′ 而不直接是????′?？

統(tǒng)一量綱，因?yàn)閮蓚€(gè)進(jìn)行相乘的兩個(gè)分布都是基于傳感器讀數(shù)視角，從真實(shí)狀態(tài)到傳感器讀數(shù)狀態(tài)間存在關(guān)系矩陣????，所以乘積結(jié)果就是傳感器讀數(shù)估計(jì)。