狀態(tài)預(yù)測
卡爾曼濾波首先考慮的問題是目標(biāo)狀態(tài)指么，這也叫系統(tǒng)狀態(tài)仙逻。以小車在高速公路上行駛為例，小車的狀態(tài)可以用位置和速度來表示涧尿。用數(shù)學(xué)符號表示即![x_{t} =\left[ \begin{matrix} p_{t}\\ v_{t}\\ \end{matrix} \right]](https://math.jianshu.com/math?formula=x_%7Bt%7D%20%3D%5Cleft%5B%20%5Cbegin%7Bmatrix%7D%20%20%20p_%7Bt%7D%5C%5C%20%20%20v_%7Bt%7D%5C%5C%20%20%20%20%20%5Cend%7Bmatrix%7D%20%20%5Cright%5D%20)
,其中
表示位置，
表示速度檬贰。
小車在行駛姑廉，既可以做勻速直線運動，也可以做加速運動翁涤。假定
表示小車的加速度桥言，那么小車進(jìn)行勻速運動時,
。
時刻葵礼，小車的位移和速度可以分別表示為：
因為當(dāng)前時刻的位置和速度分別是上一時刻的線性組合号阿，因此可以用矩陣的形式來描述上述兩個公式， 即:![\left[ \begin{matrix} p_{t}\\ v_{t}\\ \end{matrix} \right] ＝\left[ \begin{matrix} 1&\Delta t\\ 0&1\\ \end{matrix} \right] \left[ \begin{matrix} p_{t-1}\\ v_{t-1}\\ \end{matrix} \right] +\left[ \begin{matrix} \frac{\Delta t^2 }{2} \\ \Delta t\\ \end{matrix} \right] u_{t}](https://math.jianshu.com/math?formula=%5Cleft%5B%20%5Cbegin%7Bmatrix%7D%20p_%7Bt%7D%5C%5C%20v_%7Bt%7D%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%5D%20%EF%BC%9D%5Cleft%5B%20%5Cbegin%7Bmatrix%7D%201%26%5CDelta%20t%5C%5C%200%261%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%5D%20%5Cleft%5B%20%5Cbegin%7Bmatrix%7D%20p_%7Bt-1%7D%5C%5C%20v_%7Bt-1%7D%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%5D%20%2B%5Cleft%5B%20%5Cbegin%7Bmatrix%7D%20%5Cfrac%7B%5CDelta%20t%5E2%20%7D%7B2%7D%20%5C%5C%20%5CDelta%20t%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%5D%20u_%7Bt%7D%20)
令![F_{t} =\left[ \begin{matrix} 1&\Delta t\\ 0&1\\ \end{matrix} \right]](https://math.jianshu.com/math?formula=F_%7Bt%7D%20%3D%5Cleft%5B%20%5Cbegin%7Bmatrix%7D%201%26%5CDelta%20t%5C%5C%200%261%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%5D%20)
,![B_{t} =\left[ \begin{matrix} \frac{\Delta t^2 }{2} \\ \Delta t\\ \end{matrix} \right]](https://math.jianshu.com/math?formula=B_%7Bt%7D%20%3D%5Cleft%5B%20%5Cbegin%7Bmatrix%7D%20%5Cfrac%7B%5CDelta%20t%5E2%20%7D%7B2%7D%20%5C%5C%20%5CDelta%20t%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%5D)
,那么上面公式就轉(zhuǎn)換為：
劃重點鸳粉，此公式也就是卡爾曼濾波器的第一個公式扔涧，叫狀態(tài)預(yù)測公式。
就是系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣届谈，表示上一個時刻狀態(tài)怎么變換到下一個時刻的狀態(tài)枯夜，
是控制矩陣，表示控制量是怎么作用于當(dāng)前狀態(tài)艰山。
就是我們的預(yù)測值湖雹。細(xì)心的可以發(fā)現(xiàn)，這里的
帶有帽子曙搬，沒有寫錯摔吏，這說明了這里的是系統(tǒng)狀態(tài)的估計值，這個值是通過上一個時刻的狀態(tài)（卡爾曼濾波修正后的狀態(tài)值）計算估計出來的纵装。既然是估計值征讲，那么就存在噪聲，我們用協(xié)方差矩陣表示當(dāng)前估計值和真實值相比搂擦，帶來多大的不確定性稳诚，那么就有卡爾曼濾波的第二個公式,也就是不確定性在各個時刻的傳遞公式：
表示協(xié)方差矩陣,
是狀態(tài)轉(zhuǎn)移矩陣，考慮在估計下一時刻的狀態(tài)瀑踢，本身存在的噪聲扳还，這個噪聲用
來表示才避。這樣下一時刻的協(xié)方差矩陣的估計值，就得到了氨距。綜上桑逝，卡爾曼濾波器的狀態(tài)預(yù)測兩個公式就說明清楚了。
觀測矩陣
小車在行駛俏让，那么這個觀察矩陣是什么東西楞遏。這個跟實際的需求有關(guān)，可以是位置首昔，可以是速度寡喝，也可以是速度和位置等。在這個例子中以小車的位置為觀察量勒奇，用數(shù)學(xué)符號
表示预鬓。因為預(yù)測值
和觀測值之間是線性關(guān)系（卡爾曼濾波器處理線性問題），那么轉(zhuǎn)換關(guān)系就可以用矩陣
表示赊颠，叫觀測矩陣格二。需要考慮的是估計值
和觀測值的維度不總是相同，那么觀測矩陣是根據(jù)實際情況構(gòu)造的竣蹦。
小車行駛例子,估計值)
顶猜，)
,那么)
,![H=\left[ \begin{matrix} 1&0 \end{matrix} \right]](https://math.jianshu.com/math?formula=H%3D%5Cleft%5B%20%5Cbegin%7Bmatrix%7D%20%201%260%20%5Cend%7Bmatrix%7D%20%20%5Cright%5D%20)
,據(jù)此，得到卡爾曼濾波的第三公式：
痘括。
表示測量噪聲长窄，因為測量值不是１００％準(zhǔn)確，即實際的測量值跟你測出來的測量值還是存在誤差纲菌，那么用
表示這個誤差抄淑，專業(yè)術(shù)語就是觀測噪聲的協(xié)方差矩陣。小車?yán)佑^測值是一個標(biāo)量驰后，那么就只代表方差了肆资。
狀態(tài)更新
有了小車狀態(tài)的估計值和測量值，就可以用卡爾曼濾波器來修正測量值了灶芝。
?????? )
???? %7D%5E%7B-1%7D)
???? 其中)
是測量值和估計值之間的殘差郑原，
是卡爾曼系數(shù)，用來衡量預(yù)測狀態(tài)協(xié)方差和觀測狀態(tài)協(xié)方差的大小進(jìn)而相信預(yù)測模型多一點還是觀察模型多一點夜涕。相信預(yù)測模型多一點犯犁，則殘差的權(quán)重小一點，反之女器，殘差的權(quán)重大一點酸役。
噪聲協(xié)方差矩陣更新：
卡爾曼濾波器的最后一個公式就是對最佳估計值的噪聲分布的更新了：
P_%7Bt%7D%5E-)
以上，卡爾曼濾波器的所有的公式和參數(shù)就解釋完啦，下集將會給出自己的實戰(zhàn)例子涣澡。
最后編輯于 ：
?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者
