卡爾曼濾波（一）
我們將從理論和實(shí)際案例兩個(gè)方面討論卡爾曼濾波器，以下的討論可能需要一些線性代數(shù)和概率論知識(shí)。
首先是理論方面：
1. 模型建立
卡爾曼濾波建立在線性代數(shù)和隱馬爾可夫模型（hidden Markov model）上。我們在這里只討論最簡單的一種模型，其基本動(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è)過程在卡爾曼濾波的框架下建立模型。也就是說對于每一步 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，對真實(shí)狀態(tài) x{k} 的一個(gè)測量 z{k} 滿足下式：
其中
-
H{k}是觀測模型，它把真實(shí)狀態(tài)空間映射成觀測空間 -
v{k}是觀測噪聲，其均值為零，協(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ì)量，表示對當(dāng)前的狀態(tài)估計(jì)量。 -
P{k|k-1}表示先驗(yàn)估計(jì)誤差協(xié)方差矩陣，x'{k|k-1}叫做先驗(yàn)狀態(tài)預(yù)測量，表示根據(jù)第k-1次狀態(tài)預(yù)測的狀態(tài)量。（這里一定要分清預(yù)測量與估計(jì)量）
這里說一下先驗(yàn)與后驗(yàn)狀態(tài)估計(jì)量之間的關(guān)系。
我們先定義一個(gè) y'{k} 叫做測量余量，表示時(shí)刻 k 測量值與根據(jù) k-1 時(shí)刻得到的預(yù)測值在觀測空間中的差異，滿足：
然后有，
這里的 K{k} 叫做卡爾曼增益，下面會(huì)給出推導(dǎo)。這里可以理解為估計(jì)量是預(yù)測量和測量值的線性組合，K{k} 決定了兩者的權(quán)重。
卡爾曼濾波器是一個(gè)最小均方誤差估計(jì)器，后驗(yàn)狀態(tài)誤差估計(jì)是
最小化這個(gè)矢量幅度平方的期望值，
這等同于最小化后驗(yàn)估計(jì)協(xié)方差矩陣P{k|k}的跡（trace）。將式1.7中的項(xiàng)展開，得到：
對未知數(shù) K{k} 求導(dǎo)，導(dǎo)數(shù)為零處跡最小，從而解出卡爾曼增益：
將解出的卡爾曼增益帶入式1.8可得：
根據(jù)式1.1、1.4可得：
至此，我們就已經(jīng)完成了理論公式推導(dǎo)。
3. 總結(jié)
預(yù)測：
更新：
4. Tips
以上的公式推導(dǎo)主要是基于矩陣的性質(zhì)以及矩陣運(yùn)算的規(guī)則。
協(xié)方差矩陣是對稱正定的。
- image
