卡爾曼濾波（一）
我們將從理論和實(shí)際案例兩個(gè)方面討論卡爾曼濾波器配阵，以下的討論可能需要一些線性代數(shù)和概率論知識(shí)。
首先是理論方面：
1. 模型建立
卡爾曼濾波建立在線性代數(shù)和隱馬爾可夫模型（hidden Markov model）上沐飘。我們?cè)谶@里只討論最簡(jiǎn)單的一種模型尝胆，其基本動(dòng)態(tài)系統(tǒng)可以用一個(gè)馬爾可夫鏈表示嗓奢，該馬爾可夫鏈建立在一個(gè)被高斯噪聲（即正態(tài)分布的噪聲）干擾的線性算子上的除呵。系統(tǒng)的狀態(tài)可以用一個(gè)元素為實(shí)數(shù)的向量表示。隨著離散時(shí)間的每一個(gè)增加轧坎，這個(gè)線性算子就會(huì)作用在當(dāng)前狀態(tài)上宏邮，產(chǎn)生一個(gè)新的狀態(tài)，并也會(huì)帶入一些噪聲缸血，同時(shí)系統(tǒng)的一些已知的控制器的控制信息也會(huì)被加入蜜氨。同時(shí)，另一個(gè)受噪聲干擾的線性算子產(chǎn)生出這些隱含狀態(tài)的可見輸出捎泻。
為了從一系列有噪聲的觀察數(shù)據(jù)中用卡爾曼濾波器估計(jì)出被觀察過程的內(nèi)部狀態(tài)飒炎，必須把這個(gè)過程在卡爾曼濾波的框架下建立模型。也就是說對(duì)于每一步 k笆豁，定義矩陣 F{k}, H{k}, Q{k}, R{k} 郎汪，有時(shí)也需要定義 B{k} ，如下闯狱∩酚卡爾曼濾波模型假設(shè)k時(shí)刻的真實(shí)狀態(tài)是從（k?1）時(shí)刻的狀態(tài)演化而來，符合下式：
其中
-
F{k}是作用在x{k?1}上的狀態(tài)變換模型（/矩陣/矢量）哄孤。 -
B{k}是作用在控制器向量u{k}上的輸入－控制模型照筑。 -
w{k}是過程噪聲，并假定其符合均值為零录豺，協(xié)方差矩陣為Q{k}的多元正態(tài)分布朦肘。w{k} ～ N(0,Q{k})
在時(shí)刻k饭弓，對(duì)真實(shí)狀態(tài) x{k} 的一個(gè)測(cè)量 z{k} 滿足下式：
其中
-
H{k}是觀測(cè)模型双饥，它把真實(shí)狀態(tài)空間映射成觀測(cè)空間 -
v{k}是觀測(cè)噪聲，其均值為零弟断，協(xié)方差矩陣為R{k}咏花，且服從正態(tài)分布。v{k} ～ N(0,R{k})
初始狀態(tài)以及每一時(shí)刻的噪聲{x0, w1, ..., wk, v1 ... vk}都認(rèn)為是互相獨(dú)立的阀趴。
2. 公式推導(dǎo)
定義兩個(gè)矩陣 P{k|k} 昏翰、 P{k|k-1} 滿足下式：
其中：
-
P{k|k}表示后驗(yàn)估計(jì)誤差協(xié)方差矩陣，x'{k|k}叫做后驗(yàn)狀態(tài)估計(jì)量刘急，表示對(duì)當(dāng)前的狀態(tài)估計(jì)量棚菊。 -
P{k|k-1}表示先驗(yàn)估計(jì)誤差協(xié)方差矩陣，x'{k|k-1}叫做先驗(yàn)狀態(tài)預(yù)測(cè)量叔汁，表示根據(jù)第k-1次狀態(tài)預(yù)測(cè)的狀態(tài)量统求。（這里一定要分清預(yù)測(cè)量與估計(jì)量）
這里說一下先驗(yàn)與后驗(yàn)狀態(tài)估計(jì)量之間的關(guān)系检碗。
我們先定義一個(gè) y'{k} 叫做測(cè)量余量，表示時(shí)刻 k 測(cè)量值與根據(jù) k-1 時(shí)刻得到的預(yù)測(cè)值在觀測(cè)空間中的差異码邻，滿足：
然后有折剃，
這里的 K{k} 叫做卡爾曼增益，下面會(huì)給出推導(dǎo)像屋。這里可以理解為估計(jì)量是預(yù)測(cè)量和測(cè)量值的線性組合怕犁，K{k} 決定了兩者的權(quán)重。
卡爾曼濾波器是一個(gè)最小均方誤差估計(jì)器己莺，后驗(yàn)狀態(tài)誤差估計(jì)是
最小化這個(gè)矢量幅度平方的期望值奏甫，
這等同于最小化后驗(yàn)估計(jì)協(xié)方差矩陣P{k|k}的跡（trace）。將式1.7中的項(xiàng)展開凌受，得到：
對(duì)未知數(shù) K{k} 求導(dǎo)扶檐，導(dǎo)數(shù)為零處跡最小，從而解出卡爾曼增益：
將解出的卡爾曼增益帶入式1.8可得：
根據(jù)式1.1胁艰、1.4可得：
至此款筑，我們就已經(jīng)完成了理論公式推導(dǎo)。
3. 總結(jié)
預(yù)測(cè)：
更新：
4. Tips
以上的公式推導(dǎo)主要是基于矩陣的性質(zhì)以及矩陣運(yùn)算的規(guī)則腾么。
協(xié)方差矩陣是對(duì)稱正定的奈梳。
- image
