卡爾曼濾波算法介紹

2，可信度預(yù)測(cè)

講解卡爾曼濾波之前，先講一個(gè)故事。兩個(gè)釀酒師傅判斷當(dāng)前酒的度，一個(gè)老師傅，一個(gè)學(xué)徒。酒廠里酒的度數(shù)，老師傅的判斷大多時(shí)候是更加準(zhǔn)確，而學(xué)徒大多時(shí)候的判斷誤差會(huì)大一些。一壇經(jīng)過10天發(fā)酵的酒，老師傅說是30度，學(xué)徒說是26度。但是這壇酒的實(shí)際度數(shù)是不知道的，那么我們?cè)鯓又酪粋€(gè)相對(duì)準(zhǔn)確的結(jié)果呢？我們可以直接認(rèn)為老師傅的回答正確，但是 老師傅也會(huì)馬失前蹄，學(xué)徒也會(huì)有準(zhǔn)的時(shí)候 ，只是大多數(shù)時(shí)候誤差大一些。怎么得到相對(duì)可信度更高的結(jié)果呢。可以通過如下公式：

預(yù)測(cè)度數(shù)=K* 師傅預(yù)測(cè)+(1-K) * 學(xué)徒預(yù)測(cè)

來(lái)作為酒的度數(shù)， K可以理解為對(duì)老師傅判斷的可信度，1-K可以認(rèn)為是對(duì)學(xué)徒判斷的可信度 。顯然讓K大一些，比如K=0.8,說明我們更相信師傅的判斷。隨著學(xué)徒技藝的熟練K變?yōu)榱?.6，那么那么對(duì)學(xué)徒判斷的權(quán)重增加了，對(duì)師傅判斷的可信度權(quán)重減小了。所以這個(gè)K很重要。

同樣的，從工程試驗(yàn)中來(lái)說明，假設(shè)有兩個(gè)廠商的加速度傳感器A和B，汽車在跑，不斷的讀取A和B，每次會(huì)得到兩個(gè)值，那么該相信哪個(gè)傳感器的結(jié)果呢？傳感器測(cè)量出來(lái)的結(jié)果一定是有誤差的，不同廠商的傳感器誤差范圍不一樣，加上傳感器受到的環(huán)境的干擾等因素這樣也會(huì)干擾傳感器的測(cè)量結(jié)果。那么怎么得到更加可靠的結(jié)果呢？是不是和剛才的師傅判斷酒的度數(shù)很像？

假設(shè)A廠商的傳感器質(zhì)量好精度高，B廠商的誤差大，精度小，那么我們肯定更加相信A的測(cè)量結(jié)果。我們同樣 ，傳感器A ，傳感器B ，則：

綜合考慮傳感器A和傳感器B這就是兼聽則明。K越大說明越相信傳感器A測(cè)量的數(shù)據(jù) 。

如何選擇最優(yōu)的K，使上面得到的 可信度最高呢？這就需要卡爾曼濾波 。

3，卡爾曼濾波

下面這段是百度出來(lái)的卡爾曼濾波：

卡爾曼濾波（Kalman filtering）是一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測(cè)數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法 。由于觀測(cè)數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計(jì)也可看作是濾波過程。

用土話來(lái)解釋就是測(cè)量系統(tǒng)的狀態(tài)，由于有各種噪聲，會(huì)導(dǎo)致測(cè)量結(jié)果存在一定的誤差，那么如何降低這些誤差得到更加可信的數(shù)據(jù)呢，那么就是使用卡爾曼濾波做最優(yōu)估計(jì)。

卡爾曼濾波適用于線性系統(tǒng)，并且系統(tǒng)的噪聲滿足高斯分布 (通常的工程過程噪聲都滿足)。

卡爾曼濾波是 以最小均方誤差為估計(jì)的最佳準(zhǔn)則 。也就是卡爾曼濾波定義的系統(tǒng)的最優(yōu)是讓估計(jì)結(jié)果的分布的方差最小的估計(jì)。

接下來(lái)引入卡爾曼濾波，首先引入 高斯分布或者叫正態(tài)分布 。

通常工業(yè)場(chǎng)景中許多測(cè)量和過程中的誤差都都滿足正態(tài)分布，所以傳感器的測(cè)量結(jié)果X=T+a，噪聲a滿足高斯分布，所以X也滿足高斯分布。

傳感器的測(cè)量結(jié)果X滿足高斯分布：

其中期望值是u，方差是****sigma(這個(gè)符號(hào)打不出來(lái)，就是上面分布中的值，后面都用sigma)

數(shù)學(xué)表達(dá)為：

坐標(biāo)圖為：

高斯分布

然后解釋卡爾曼濾波，還是以剛才的傳感器來(lái)說明，日常中各種噪聲，干擾都符合高斯分布。所以傳感器的測(cè)量結(jié)果也是符合高斯分布

傳感器A的測(cè)量 符合：

傳感器B的測(cè)量 符合：

其中a和b就是測(cè)量噪聲

預(yù)測(cè)加速度是：。 概率論可以證明兩個(gè)高斯分布的線性組合也符合高斯分布 ：

所以預(yù)測(cè) 符合：。當(dāng)K取不同值的時(shí)候得到的 u‘和sigma 也不同。那么 最優(yōu)估計(jì)是什么呢？剛才說了是讓方差最小。也就是當(dāng)取某個(gè)K的時(shí)候 sigma'最小的估計(jì)是最優(yōu)估計(jì) 。

這是一個(gè)拋物線，可以證明當(dāng) 的時(shí)候，

sigma'取最小值。將K帶入可以得到:

的時(shí)候 預(yù)測(cè)加速度X’也符合高斯分布，并且sigma' 取最小值。這個(gè)時(shí)候是最優(yōu)估計(jì)，可信度最高。

這就是卡爾曼濾波，我們畫圖如下：

卡爾曼濾波分布

可以看到 預(yù)測(cè)加速度X’ 更加的瘦高，所以這樣得到的估計(jì)更加的可信 。