狀態(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)系作者
【社區(qū)內(nèi)容提示】社區(qū)部分內(nèi)容疑似由AI輔助生成，瀏覽時請結(jié)合常識與多方信息審慎甄別。
平臺聲明：文章內(nèi)容（如有圖片或視頻亦包括在內(nèi)）由作者上傳并發(fā)布，文章內(nèi)容僅代表作者本人觀點，簡書系信息發(fā)布平臺，僅提供信息存儲服務(wù)。
