卡爾曼--目標跟蹤詳解

卡爾曼濾波器介紹

卡爾曼濾波器又叫最佳線性濾波器,他的好處有很多,比如實現簡單而且又是純時域的濾波器不需要進行頻域變換所以在工程上有很多應用。

卡爾曼濾波器原理

假設有一輛汽車在公路上行駛我們用他的位置和速度來表示他當前的狀態,在這裏插入圖片描述
寫成矩陣的形式狀態 xtx_t
xt=[ptvt] x_t = \begin{bmatrix} p_t \\ v_t \end{bmatrix}

狀態轉移

駕駛員可以踩油門或者踩剎車,所以車還有一個加速度 utu_t,如果我們已經知道了上一時刻的狀態xt1x_{t-1},那麼當前狀態 xtx_t會是什麼呢?
位置ptp_t和速度vtv_t與上一時刻的關係就如下公式所示:
pt=pt1+Δt×vt1+ut2×Δt2vt=vt1+Δt×ut p_t=p_{t-1}+\Delta_t \times v_{t-1}+{\frac {u_t}2} \times \Delta_t^2 \\ v_t=v_{t-1}+\Delta_t \times u_t
pt1p_{t-1}上一時刻的位置
vt1v_{t-1}上一時刻的速度
我們觀察這兩個公式發現輸出變量都只是輸入變量的線性組合,這就是爲什麼說卡爾曼濾波器是最佳的線性濾波器,因爲它只能描述狀態與狀態的線性關係,既然是線性關係就可以寫成矩陣的形式,如下所示
[ptvt]=[1Δt01][pt1vt1]+[Δt22Δt]ut \begin{bmatrix} p_t \\ v_t \end{bmatrix}= \begin{bmatrix} 1&\Delta_t\\ 0&1 \end{bmatrix} \begin{bmatrix} p_{t-1}\\ v_{t-1} \end{bmatrix}+ \begin{bmatrix} {\frac {\Delta_t^2}2}\\ \Delta_t \end{bmatrix}u_t
在進一步把兩個狀態變換矩陣提取出來,變成
Ft=[1Δt01],Bt=[Δt22Δt] F_t= \begin{bmatrix} 1&\Delta_t\\ 0&1 \end{bmatrix}, B_t= \begin{bmatrix} {\frac {\Delta_t^2}2}\\ \Delta_t \end{bmatrix} 公式就可以簡化爲
(1)x^=Ftx^t1+Btut {\hat x}^-=F_t{\hat x_{t-1}}+B_tu_t \tag{1}
公式(1)就是卡爾曼濾波器中的第一個公式 狀態預測公式,其中FtF_t就叫做狀態轉移矩陣,它表示我們如何從上一時刻的狀態來推測當前時刻的狀態,BtB_t就控制矩陣,他表示控制量utu_t如何來作用於當前狀態。

公式中的xx帶有帽子表示是xx估計量而不是真實值,因爲汽車的真實值我們是永遠無法知道,只能根據觀測來儘可能的估計xx的值。等號左邊的x還加了一個減號的上標表示這個值是根據上一時刻的狀態推測而來的,後面會根據觀測量去修正這個x^{\hat x}^-的值,修正之後纔是最佳的估計值也就是沒有減號上標的xx的值。

協方差矩陣

有了狀態預測公式就可以推測當前時刻的狀態,但是我們只能所有的推測都是包含噪聲的,噪聲越大不確定性就越大,如何來表示推測帶有多少不確定性呢?這就要用協方差矩陣來表示。

假設我們有一個一維的包含噪聲的數據,每次測量的值都不同,但都是圍繞在一箇中心值的周圍,那我們表示它的分佈狀況最簡單的方法就是記下它的中心值和方差,這實際上是假設了它是一個高斯的分佈。
在這裏插入圖片描述
在這裏插入圖片描述
二維包含噪聲的數據看起來如下所示
在這裏插入圖片描述
分別對兩個座標軸進行投影,在兩個軸上都是高斯分佈。
在這裏插入圖片描述
那我們在表示它的分佈時是不是分別記下兩個高斯分佈的中心值和方差就可以了呢,如果兩個維度的噪聲是獨立的時候可以這麼表示,但是在兩個維度上噪聲有相關性的時候就不可以。如下圖
在這裏插入圖片描述
爲了表示這兩個維度的相關性除了要記住兩個維度的方差之外,還要有一個協方差來表示兩個維度的相關程度,寫成矩陣的形式
cov(x,x)=[σ11σ12σ12σ22] cov(x,x)= \begin{bmatrix} \sigma_{11} & \sigma_{12}\\ \sigma_{12} & \sigma_{22} \end{bmatrix} 對角線上的兩個值是方差,反對角線上的兩個值是協方差他們兩個是相等的,
在卡爾曼濾波器中所有有關不確定的表述都要用到協方差矩陣

噪聲協方差矩陣的傳遞

在我們的小汽車的例子中每一個時刻的狀態的不確定性都是由協方差矩陣PP來表示
Pt=FPt1FT P^-_t=FP_{t-1}F^T
當前狀態的協方差就等於上一時刻的協方差兩邊乘以狀態轉移矩陣,爲什麼要乘兩邊這是協方差矩陣的性質 協方差矩陣的性質:cov(Ax,Bx)=Acov(x,x)BTcov(Ax,Bx)=Acov(x,x)B^T
預測模型並不是百分之百準確的,所以我們要在後面加上一個協方差矩陣QQ來表示預測模型本身帶來的噪聲
(2)Pt=FPt1FT+Q P^-_t=FP_{t-1}F^T+Q \tag{2}
公式(2)就是卡爾曼濾波器中的第二個公式 ,它表示不確定性在各個時刻之間的傳遞關係。

觀察矩陣

假設我們這公路的一端放了一個激光測距儀,在每個時刻都可以觀測到汽車的位置。
在這裏插入圖片描述
觀測到的值我們記爲ZtZ_t,那麼從汽車本身的狀態xtx_tZtZ_t之間有一個變換關係我們記爲HH,當然這個變換關係也只能是線性關係,所以要把HH寫成矩陣的形式也就是觀測矩陣
H=[10]H= \begin{bmatrix} 1&0 \end{bmatrix}
xxZZ的維度不一定是相同的,在我們的例子裏xtx_t是一個二維的列向量,ZZ只是一個標量的值,所以HH應該是一個一行兩列的矩陣,元素爲1和0,這樣當HHxtx_t相乘的時候就得到了一個標量的值ZZZZ也就是汽車的位置,他和xtx_t的第一個元素是相等的,觀測值也不是百分之百可靠的,所以也要加上一個觀測的噪聲VV
Zt=Hxt+VZ_t=Hx_t+V
而這個噪聲的協方差矩陣用RR來表示,由於在我們的例子裏觀測值是一個一維的值,所以這個RR的形式也不是一個矩陣而是一個單獨的值僅僅表示ZtZ_t的方差。假設我們除了激光測距儀之外還有其他的測量方法可以觀測到汽車的某項特徵,那麼ZZ就會變成一個多維的列向量,他會包含每一種測量方式的測量值,而每一種測量值都只是真實狀態的不完全的表現,我們可以從幾種不完全的表述裏面推斷真實的狀態,卡爾曼的數據融合的功能正是在這個測量矩陣中體現出來的。

狀態更新

前面已經得到了帶有減號的xtx_t現在我們只要在他後面加上一項來修正它的值就可以得到我們的最佳估計值了
(3)x^t=x^t+Kt(ZtHx^t)\hat x_t={\hat x_t}^-+K_t(Z_t-H{\hat x_t}^-)\tag{3}
括號裏面的表示實際的觀測值和預期的觀測值的殘差,這個殘差乘上係數KtK_t就可以修正xtx_t的值了,KtK_t叫卡爾曼係數他也是一個矩陣
(4)Kt=PtHT(HPtHT+R)1K_t=P^-_tH^T(HP^-_tH^T+R)^{-1} \tag{4}公式的推導比較複雜,這裏只是用來定性的分析一下,卡爾曼係數的作用主要有兩個方面,一是權衡預測狀態協方差PP和觀測量的協方差矩陣RR的大小來決定我們是相信預測模型多一點還是相信觀測模型多一點,如果相信預測模型多一點那個這個殘差的權重就會小一點,如果相信觀測模型多一點這個殘差的權重就會大一點。
二是把殘差的表現形式從觀測域轉換到狀態域。我們前面講到觀測值ZZ只是一個一維的向量狀態xx是一個二維的向量,他們所用的單位甚至是描述的特徵都可能是不同的那我們怎麼用觀測值的殘差去更新狀態值,實際那個卡爾曼係數KtK_t就是在替我們做這樣的轉換。在我們的例子裏我們只觀測到汽車的位置,但是KtK_t裏面已經包含了協方差矩陣PP的信息,所以它利用速度和位置這兩個維度的相關性從位置的殘差裏面推算出速度的殘差,從而讓我們可以對狀態xx的兩個維度同時進行修正。

噪聲協方差矩陣的更新

最後一步就是更新最佳估計值的噪聲分佈,這個值是留給下一輪迭代時用的。在這一步裏狀態的不確定性是減小的,而在下一輪中由於傳遞噪聲的引入不確定性又會增大。
(5)Pt=(IKtH)PtP_t=(I-K_tH)P^-_t \tag{5}
卡爾曼濾波器就是在這樣一種不確定性變化中尋求一種平衡的。

總結

x^=Fx^t1+But1Pt=FPt1FT+Q 預測公式:\\ {\hat x}^-=F{\hat x_{t-1}}+Bu_{t-1}\\ P^-_t=FP_{t-1}F^T+Q \\
Kt=PtHT(HPtHT+R)1x^t=x^t+Kt(ZtHx^t)Pt=(IKtH)Pt 更新公式:\\ K_t=P^-_tH^T(HP^-_tH^T+R)^{-1} \\ \hat x_t={\hat x_t}^-+K_t(Z_t-H{\hat x_t}^-)\\ P_t=(I-K_tH)P^-_t
FtF_t叫狀態轉移矩陣
BtB_t叫控制矩陣,他表示控制量utu_t如何來作用於當前狀態。
PP叫協方差矩陣表示每一個時刻的狀態的不確定性,預測狀態協方差
QQ表示預測模型本身帶來的噪聲
HH觀測矩陣
RR觀測量的協方差矩陣
ZZ觀測值
II爲單位矩陣

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章