網(wǎng)上很多關(guān)于卡爾曼的講解拼岳，自從看到Y(jié)outube上的這個視頻枝誊，才煥然大悟！這才是真正的深入淺出呀惜纸！
以下內(nèi)容摘錄于視頻叶撒，僅作學(xué)習(xí)記錄。
卡爾曼公式一
整個過程如下圖：
- x 表示當(dāng)前時刻的狀態(tài)
- p 表示位置
- v 表示速度
- u 表示對車的控制量耐版。不踩油門祠够，速度為0
如果已知上一時刻的狀態(tài)X(t-1) ,那么X(t)會是什么呢？如下圖：
而速度如下圖：
通過觀察這兩個公式粪牲，發(fā)現(xiàn)輸出變量是輸入變量的線性組合古瓤。卡爾曼最佳的線性濾波器腺阳，它只能描述狀態(tài)與狀態(tài)之間的線性關(guān)系落君。上面兩個公式是線性的關(guān)系滴须，那么寫成矩陣的形式就是這樣：
進一步把狀態(tài)變換矩陣提取出來：
F：狀態(tài)轉(zhuǎn)移矩陣，表示如何從上一時刻狀態(tài)如何推測當(dāng)前狀態(tài)
B：控制矩陣叽奥，表示控制量u是如何作用于當(dāng)前狀態(tài)
最后公式可以簡化成：
這個就是卡爾曼濾波器的第一個公式：狀態(tài)預(yù)測公式。所有的推測都是包含噪聲的痛侍，噪聲越大朝氓，不確定性就越大。如何表示這次推測帶來了多少不確定性呢主届？就需要使用協(xié)方差矩陣來表示赵哲。
如何表示推測的不確定性呢？
卡爾曼公式二
在卡爾曼濾波器中君丁，所有不確定性的表述都需要用到協(xié)方差矩陣枫夺。
協(xié)方差用P表示，如何讓不確定性在每個時刻里面?zhèn)鬟f呢绘闷？答案使用上面的狀態(tài)轉(zhuǎn)移矩陣橡庞。
預(yù)測模型本身也不是百分百準(zhǔn)確的，本身也包含噪聲印蔗。
Q ： 表示預(yù)測模型本身帶來的噪聲扒最。
上面的公式表示不確定性在各個時刻的傳遞關(guān)系。
卡爾曼公式三
- z 是汽車在某一時刻觀察的值华嘹。 它們之間的變換關(guān)系是線性的吧趣。
- 因為觀察值也不是百分百正確的，所有用v來表示觀察的噪聲耙厚。
- 噪聲的協(xié)方差矩陣用R表示强挫，由于觀察值是一個一維的值，所有R不是矩陣薛躬，而是一個單獨的值俯渤。
卡爾曼公式四
它表示實際觀測值和預(yù)期觀測值之間的殘差。參數(shù)乘以系數(shù)K就可以用來修正Xt 的值型宝。而K是什么稠诲？
K 叫做卡爾曼系數(shù)。作用主要是：
權(quán)衡預(yù)測狀態(tài)協(xié)方差P 和觀察量協(xié)方差矩陣R的大小诡曙，來決定我們是相信預(yù)測模型多一點（殘差權(quán)重型涡稹），還是觀察模型多一點（殘差權(quán)重大）价卤。
把殘差的表行形式劝萤，從觀察域轉(zhuǎn)換到狀態(tài)域。
卡爾曼公式五
更新最佳估計值的噪聲分布慎璧，留給下一輪迭代使用床嫌。
Demo
Ref：
