卡爾曼濾波初探（一）

阿新 • • 發佈：2018-12-24

卡爾曼濾波初探

基於時域的線性模型預測

這裡先給出幾個概念（初看的時候很多部落格都沒有這方面說明，若你看到下面懵逼的時候，不妨上來再看看？）

預測：就是根據已有的①經驗、②公式、③以及上一個時間（ $t_{k-1}$ ）下檢測物件的狀態的最優估計等資訊，從而得到一個對下一個時間（ $t_{k}$ ）下狀態的一個估計。【X(k|k-1)=AX(k-1|k-1)+Bu(k).....................(1)】
更新：指到達時間（ $t_{k}$ ）後，我們的感測器就獲得了此刻時間（ $t_{k}$ ）下檢測物件的狀態資訊，這個資訊是測量資訊，同樣包含了不確定的成分（uncertainty），因此，我們需要結合

我們上面所說的預測，將測量值和預測值進行融合，得到一個最優的估計。【X(k|k)=X(k|k-1)+Kg(k)*(Z(k)-Hx(k|k-1))....(3)　】
協方差矩陣（誤差估計）：協方差矩陣來表示，簡而言之，矩陣中的每個元素 $這裡寫圖片描述$ 表示第 i 個和第 j 個狀態變數之間的相關度。

基於案例分析：

（1）假設一輛小車在路上行駛，其在時間t下的狀態如下： $x_{t}= \begin{bmatrix} p_{t} \\ v_{t} \end{bmatrix}$ , pt為t時刻下的位置，vt為t時間下的速度

（2）進一步的，如果我們有一個控制訊號 $u_{t}$ 表示油門，這個ut在物理模型上可表示為加速度a

那麼，根據經典物理學，我們可以寫出t時刻下的狀態：

即 $x_{t}= \begin{bmatrix} 1 &\Delta t \\ 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} p_{t-1} \\ v_{t-1} \end{bmatrix} +\begin{bmatrix} \frac{\Delta t^{2}}{2}\\\Delta t \end{bmatrix}\cdot u_{t}$ ，可簡寫為如下：

其中：

F為狀態轉移矩陣
B為控制矩陣

到這裡，上面的公式就是這個例子中的“預測”公式

（3）既然有了預測，那麼我們的最終目的是更新，接下來，就是推導"更新操作"的公式

1、首先要明確的是：在t時刻，我們得到了①根據預測公式計算的預測值②感測器的測量值。一般來說，這兩個都是都是高斯分佈，如圖。

2、那麼，問題就轉化為，我們應該相信誰的問題，是相信紅色部分、還是藍色部分的分佈？答案是，兩個都相信，兩個都用，那麼很自然的想到：兩個事件同時發生的可能性越大，我們越相信它！要想考察它們同時發生的可能性，就是將兩個事件單獨發生的概率相乘。

即下圖的綠色部分（也是正態分佈）：

這裡引用一下其他部落格關於這個問題的解釋：

這裡就涉及到了高斯分佈相乘的問題：

這裡再次感謝翻譯博主以及原文博主對此作出的貢獻，到了這裡，其實已經得到了“更新操作”的公式：

我們用之前沿用的 $x_{t}$ 對上面的均值 $\mu$ 進行替換，從而得到：

$x_{t,opt}=x_{(t|t-1)}+K_{t}*(x_{t|sensor}-x_{(t|t-1)})$

【這就是我們的更新公式】

其中，這些都是變數或矩陣，（因為計算機對於矩陣的運算更有效）

$x_{t,opt}$ 為我們更新操作後得到的最優值

$x_{(t|t-1)}$ 為我們的預測值（與t-1時刻的最優值相關），即基於t-1時刻的最優狀態推算出來t時刻的狀態

$x_{t|sensor}$ 為t時刻下，感測器獲得的狀態

公式中有一個 $K_{t}$ ，我們稱為卡爾曼增益，這裡先給出它的計算公式（我們用P代替了上面的 $\sum$ ）： $K_{t}=P_{(t|t-1)}*(P_{(t|t-1)}+P_{(sensor)})^{-1}$ :

其中：

$P_{(t|t-1)}$ 是協方差矩陣的估計值，是基於 $P_{(t-1|t-1)}$ 進行估計得到的。

至於 $P_{(t-1|t-1)}$ 是怎麼來的，這裡也給出公式： $P_{(t-1|t-1)}=(I-k_{t-1})*P_{(t-1|t-2)}$ 【I為單位矩陣】

很顯然， $P_{(t-1|t-1)}$ 是在t-1時刻，進行“更新操作”完成之後，再按照上面的公式計算而來，以便在t時刻進行迭代

實際上，這裡也是卡爾曼濾波迭代自迴歸的奇妙之處？

關於協方差P的解釋，請繼續看下文

（4）上文，我們已經得到了“更新操作”的公式，在此之前，我們還需要獲得“更新”操作過程所需的引數協方差矩陣

1、關於協方差，協方差是用來表示兩個維度之間的相關性

a、當兩個維度不相關：當橫軸上的變數v變化時，並不影響縱軸的變數position，對於縱軸上的變數也同理

gauss_1

b、兩個維度相關：如圖中白色部分，可想象為一條斜線，假設橫軸方向的變數v變化時，其對應的縱軸變數positon也改變，實際上，這也是現實中的大部分情況，一個例子就是：我們基於舊的位置來估計新位置position。如果速度v過高，我們可能已經移動很遠了。如果緩慢移動，則距離不會很遠。

對於這種關係，一般用協方差矩陣表示，即：

到這裡，我們獲取到了一個我們更新所需的引數，協方差矩陣P（注意是大寫的P，與狀態x中的p--->positon區分）

PS：這裡協方差矩陣有一個性質如下【下面的預測公式會用到】：

這裡寫圖片描述

2、關於不確定性因素

a、我們先整理一下現在獲得的公式：

預測公式：

$x_{(t|t-1)}=F_{t}\cdot x_{t-1,opt}+B_{t}\cdot u_{t}$

$P_{(t|t-1)}={\color{Red} F_{t}}\cdot P_{(t-1|t-1)}\cdot {\color{Red} F_{t}^{T}}$ （協方差矩陣的式子用到了上面提到的性質）

更新公式：

$x_{t,opt}=x_{(t|t-1)}+K_{t}*(x_{t|sensor}-x_{(t|t-1)})$

$P_{(t|t)}=(I-k_{t})*P_{(t|t-1)}$

$K_{t}=P_{(t|t-1)}\cdot *(P_{(t|t-1)}+P_{(sensor)})^{-1}$

【考慮到我們的感測器讀取的資料可能會經過一些變換（縮放等），因此要加上一個感測器模型矩陣H，公式變為：】

$x_{t,opt}=x_{(t|t-1)}+K_{t}*(x_{t|sensor}-{\color{Red} H}\cdot x_{(t|t-1)})$

$P_{(t|t)}=(I-k_{t}\cdot {\color{Red} H^{T}})*P_{(t|t-1)}$

$K_{t}=(P_{(t|t-1)}\cdot {\color{Red} H^{T}} )\cdot ({\color{Red} H}\cdot P_{(t|t-1)}\cdot {\color{Red} H^{T} }+P_{(sensor)})^{-1}$

b、現在來加上我們的不確定因素

預測操作的不確定因素：

如果這些狀態量是基於系統自身的屬性或者已知的外部控制作用來變化的，則不會出現什麼問題。但是如果考慮外部干擾，例如，假設我們跟蹤一個四旋翼飛行器，它可能會受到風的干擾，如果我們跟蹤一個輪式機器人，輪子可能會打滑，或者路面上的小坡會讓它減速。這樣的話我們就不能繼續對這些狀態進行跟蹤，如果沒有把這些外部干擾考慮在內，我們的預測就會出現偏差。（引用自：https://blog.csdn.net/u010720661/article/details/63253509）

如何引入？

一種做法是：我們將這些沒有被跟蹤的干擾當作協方差為 ${\color{Blue} Q_{t}}$ 的噪聲來處理

將上面總結的預測公式搬下來，並在協方差的式子上加上 ${\color{Blue} Q_{t}}$ 表示沒有跟蹤的外部干擾

新的預測式子：

${\color{Red} x_{(t|t-1)}=F_{t}\cdot x_{t-1,opt}+B_{t}\cdot u_{t}}$ ---------------------------->①

${\color{Red} P_{(t|t-1)}={\color{Red} F_{t}}\cdot P_{(t-1|t-1)}\cdot {\color{Red} F_{t}^{T}}}+{\color{Blue} Q_{t}}$ ---------------------->②

由上式可知，新的預測是根據上一最優估計預測得到的，並加上已知外部控制量的修正。

而新的不確定性由上一不確定性預測得到，並加上外部環境的干擾。

更新操作的不確定因素：

我們的感測器或多或少都有點不可靠，並且原始估計中的每個狀態可以和一定範圍內的感測器讀數對應起來。從測量到的感測器資料中，我們大致能猜到系統當前處於什麼狀態。但是由於存在不確定性，某些狀態可能比我們得到的讀數更接近真實狀態。【簡單說，就是我們的感測器測得的不一定是真值】

如何引入？

一種做法是：將這種不確定性（例如：感測器噪聲）用協方差 ${\color{Green} R_{t}}$ 表示，該分佈的均值就是我們讀取到的感測器資料，稱之為 ${\color{DarkOrange} x_{t|sensor}}$ 。

將上面總結的更新公式搬下來，我們回過頭來看卡爾曼增益 $K_{t}$ 【兩個高斯分佈相乘的部分提到的】：

${\color{Red} x_{t,opt}=x_{(t|t-1)}+K_{t}*({\color{DarkOrange} x_{t|sensor}}-{\color{Red} H}\cdot x_{(t|t-1)})}$ ---------------->③

${\color{Red} P_{(t|t)}=(I-k_{t}\cdot H)*P_{(t|t-1)}}$ ---------------------------------------->④

卡爾曼增益（前文提到的公式如下）：

$K_{t}=(P_{(t|t-1)}\cdot H^{T} )\cdot ( H\cdot P_{(t|t-1)}\cdot H^{T} +{\color{Magenta} P_{(sensor)}})^{-1}$

而由這裡的不確定性因素（噪聲）可知， ${\color{Magenta} P_{(sensor)}}={\color{Green} R_{t}}$ ，於是，卡爾曼增益公式變為：

${\color{Red} K_{t}=(P_{(t|t-1)}\cdot {\color{Red} H^{T}} )\cdot ({\color{Red} H}\cdot P_{(t|t-1)}\cdot {\color{Red} H^{T} }+{\color{Green} R_{t}})^{-1}}$ ------------------>⑤