開發者說丨手把手教你寫擴展卡爾曼濾波器

時間 2020-04-07

原文原文鏈接

本文做者：Apollo開發者社區編程

卡爾曼濾波器是一種由卡爾曼(Kalman)提出的用於時變線性系統的遞歸濾波器。這個系統可用包含正交狀態變量的微分方程模型來描述，這種濾波器是將過去的測量估計偏差合併到新的測量偏差中來估計未來的偏差。數據結構

當系統狀態方程不符合線性假設時，採用卡爾曼濾波沒法得到理想的最優估計；在描述機器人狀態時經常不知足卡爾曼濾波的假設,爲了仍然可以使用卡爾曼濾波，咱們採用對非線性系統進行線性化等方法來擴大卡爾曼濾波的使用範圍。擴展卡爾曼濾波與卡爾曼濾波主要區別在於：對狀態方程和觀測方程泰勒展開。函數

本文由百度Apollo智能汽車事業部自動駕駛工程師——陳光撰寫，對擴展卡爾曼濾波器進行了詳細講解，但願感興趣的同窗能經過閱讀這篇文章有所收穫。測試

如下，ENJOY spa

前言

在以前分享的《開發者說 | 手把手教你寫卡爾曼濾波器》中，以一個勻速運動小車的模型爲例，讓讀者從感性上認識了卡爾曼濾波器的基本原理，它包含預測（Prediction）和測量值更新（Measurement update）兩大過程。預測和測量值更新的交替執行，實現了卡爾曼濾波在狀態估計中的閉環。3d

隨後，我從理性分析的角度，以無人駕駛中激光雷達測量障礙物位置的數據爲例，結合卡爾曼濾波所用到的公式，使用C++和矩陣運算庫Eigen一步步實現了卡爾曼濾波器預測和測量值更新這兩大過程的代碼。調試

在本次分享中，我將以《優達學城（Udacity）無人駕駛工程師學位》中提供的傳感器融合項目爲例，介紹如何利用擴展卡爾曼濾波解決跟蹤毫米波雷達目標的問題。因爲本次內容爲卡爾曼濾波器的進階內容，推薦讀者先閱讀《無人駕駛技術入門（十三）| 手把手教你寫卡爾曼濾波器》，瞭解預測和測量值更新這兩個步驟所使用到的公式及其含義，再看此篇。blog

正文

毫米波雷達的數據

毫米波雷達觀察世界的方式與激光雷達有所不一樣。激光雷達測量的原理是光的直線傳播，所以在測量時能直接得到障礙物在笛卡爾座標系下x方向、y方向和z方向上的距離；而毫米波雷達的原理是多普勒效應，它所測量的數據都是在極座標系下的。遞歸

以下圖所示，毫米波雷達可以測量障礙物在極座標下離雷達的距離ρ、方向角ϕ以及距離的變化率（徑向速度）ρ'，以下圖所示。圖片

圖片出處：優達學城（Udacity）無人駕駛工程師學位

毫米波雷達的測量原理及更爲詳細的數據介紹可參看《無人駕駛技術入門（七）| 量產必備的毫米波雷達》。

擴展卡爾曼濾波器理論

咱們再次祭出卡爾曼老先生給咱們留下的寶貴財富，即狀態估計時預測和測量值更新時所用到的7個公式，以下圖所示。擴展卡爾曼濾波的理論和編程依舊須要使用到這些公式，相比於原生的卡爾曼濾波，只在個別地方有所不一樣。

卡爾曼濾波器公式

圖片出處：優達學城（Udacity）無人駕駛工程師學位

完成擴展卡爾曼濾波器C++代碼的過程，實際上就是結合上面的公式，一步步完成初始化、預測、觀測的過程。因爲公式中涉及大量的矩陣轉置和求逆運算，咱們使用開源的矩陣運算庫Eigen輔助咱們代碼的編寫。

代碼：初始化（Initialization）

擴展卡爾曼濾波的初始化，須要將各個變量進行設置，對於不一樣的運動模型，狀態向量是不同的。爲了保證代碼對不一樣狀態向量的兼容性，咱們使用Eigen庫中非定長的數據結構。

以下所示，咱們新建了一個ExtendedKalmanFilter類，定義了一個叫作x_的狀態向量（state vector）。代碼中的VerctorXd表示X維的列向量，元素的數據類型爲double。

初始化擴展卡爾曼濾波器時須要輸入一個初始的狀態量x_in，用以表示障礙物最初的位置和速度信息，通常直接使用第一次的測量結果。

代碼：預測（Prediction)

完成初始化後，咱們開始寫Prediction部分的代碼。首先是公式

這裏的x爲狀態向量，經過左乘一個矩陣F，再加上外部的影響u，獲得預測的狀態向量x'。這裏的F被稱做狀態轉移矩陣（state transistion matrix）。以2維的勻速運動爲例，這裏的x爲

根據中學物理課本中的公式s1 = s0 + vt，通過時間△t後的預測狀態向量應該是

對於二維的勻速運動模型，加速度爲0，並不會對預測後的狀態形成影響，所以

若是換成加速或減速運動模型，能夠引入加速度ax和ay，根據s1 = s0 + vt + at^2/2這裏的u會變成：

做爲入門課程，這裏不討論太複雜的模型，所以公式

最終將寫成

再看預測模塊的第二個公式

該公式中P表示系統的不肯定程度，這個不肯定程度，在卡爾曼濾波器初始化時會很大，隨着愈來愈多的數據注入濾波器中，不肯定程度會變小，P的專業術語叫狀態協方差矩陣（state covariance matrix）；這裏的Q表示過程噪聲（process covariance matrix），即沒法用x'=Fx+u表示的噪聲，好比車輛運動時忽然到了上坡，這個影響是沒法用以前的狀態轉移方程估計的。

毫米波雷達測量障礙物在徑向上的位置和速度相對準確，不肯定度較低，所以能夠對狀態協方差矩陣P進行以下初始化：

因爲Q對整個系統存在影響，但又不能太肯定對系統的影響有多大。對於簡單的模型來講，這裏能夠直接使用單位矩陣或空值進行計算，即

根據以上內容和公式

咱們就能夠寫出預測模塊的代碼了

實際編程時x'及P'不須要申請新的內存去存儲，使用原有的x和P代替便可。

代碼：觀測（Measurement）

觀測的第一個公式是

這個公式的目的是計算觀測到的觀測值z與預測值x'之間差值y。

前面提到過毫米波雷達觀測值z的數據特性，以下圖所示：

圖片出處：優達學城（Udacity）無人駕駛工程師學位

由圖可知觀測值z的數據維度爲3×1，爲了可以實現矩陣運算，y和Hx'的數據維度也都爲3×1。

使用基本的數學運算能夠完成預測值x'從笛卡爾座標系到極座標系的座標轉換，求得Hx'，再與觀測值z進行計算。須要注意的是，在計算差值y的這一步中，咱們經過座標轉換的方式避開了未知的旋轉矩陣H的計算，直接獲得了Hx’的值。

爲了簡化表達，咱們用px，py以及vx和vy表示預測後的位置及速度，以下所示：

其中測量值z和預測後的位置x'都是已知量，所以咱們很容易計算獲得觀測與預測的差值y。

毫米波雷達在轉換時涉及到笛卡爾座標系和極座標系的位置、速度轉換，這個轉化過程是非線性的。於是在處理相似毫米波雷達這種非線性的模型時，習慣於將計算差值y的公式寫成以下，以做線性和非線性模型的區分。

對應上面的式子，這裏的h(x')爲：

再看卡爾曼濾波器接下來的兩個公式

這兩個公式求的是卡爾曼濾波器中一個很重要的量——卡爾曼增益K（Kalman Gain），用人話講就是求差值y的權值。第一個公式中的R是測量噪聲矩陣（measurement covariance matrix），這個表示的是測量值與真值之間的差值。通常狀況下，傳感器的廠家會提供。若是廠家未提供，咱們也能夠經過測試和調試獲得。S只是爲了簡化公式，寫的一個臨時變量，不用太在乎。

因爲求得卡爾曼增益K須要使用到測量矩陣H，所以接下來的任務就是獲得H。

毫米波雷達觀測z是包含位置、角度和徑向速度的3x1的列向量，狀態向量x'是包含位置和速度信息的4x1的列向量，根據公式y=z-Hx'可知測量矩陣（Measurement Matrix）H的維度是3行4列。即：

從上面的公式很容易看出，等式兩邊的轉化是非線性的，並不存在一個常數矩陣H，可以使得等式兩邊成立。

若是將高斯分佈做爲輸入，輸入到一個非線性函數中，獲得的結果將再也不符合高斯分佈，也就將致使卡爾曼濾波器的公式再也不適用。所以咱們須要將上面的非線性函數轉化爲近似的線性函數求解。

圖片出處：優達學城（Udacity）無人駕駛工程師學位

在大學課程《高等數學》中咱們學過，非線性函數y=h(x)可經過泰勒公式在點(x0,y0)處展開爲泰勒級數：

忽略二次以上的高階項，便可獲得近似的線性化方程，用以替代非線性函數h(x)，即：

將非線性函數h(x)拓展到多維，即求各個變量的偏導數，即：

對x求偏導數所對應的這一項被稱爲雅可比（Jacobian）式。

咱們將求偏導數的公式與咱們的以前推導的公式對應起來看x的係數，會發現這裏的測量矩陣H其實就是泰勒公式中的雅可比式。

多維的雅可比式的推導過程有興趣的同窗能夠本身研究一下，這裏咱們直接使用其結論：

求得非線性函數h(x')對px，py，vx，vy的一階偏導數，並排列成的矩陣，最終獲得雅克比（Jacobian）矩陣H：

其中

接下來就是考驗各位高等數學求偏導數功底的時候了！

通過一系列計算，最終獲得測量矩陣H爲：

根據以上公式可知，在每次預測障礙物的狀態後，須要根據預測的位置和速度計算出對應的測量矩陣H，這個測量矩陣爲非線性函數h(x')在x'所在位置進行求導的結果。

再看卡爾曼濾波器的最後兩個公式

這兩個公式，實際上完成了卡爾曼濾波器的閉環，第一個公式是完成了當前狀態向量x的更新，不只考慮了上一時刻的預測值，也考慮了測量值，和整個系統的噪聲，第二個公式根據卡爾曼增益K，更新了系統的不肯定度P，用於下一個週期的運算，該公式中的I爲與狀態向量同維度的單位矩陣。

完成以上推導後，咱們將推導的過程寫成代碼，以下所示：

再補上一些必要的代碼，一個擴展卡爾曼濾波器的雛形就出來了，代碼以下所示：

以毫米波雷達數據爲例，使用以上濾波器，代碼以下：

其中GetRadarData函數除了獲取毫米波雷達障礙物的距離、角度和徑向速度外，還獲取了信息採集時刻的時間戳，用於計算先後兩幀的時間差delta_t。

以上就是使用擴展卡爾曼濾波器跟蹤毫米波雷達障礙物的例子。

擴展卡爾曼（EKF）與經典卡爾曼（KF）的區別在於測量矩陣H的計算。EKF對非線性函數進行泰勒展開後，進行一階線性化的截斷，忽略了其他高階項，進而完成非線性函數的近似線性化。正是因爲忽略了部分高階項，使得EKF的狀態估計會損失一些精度。

只要你可以運用卡爾曼濾波器的7個經典公式，寫出模型的F、P、Q、H、R矩陣，任何狀態跟蹤的問題都將迎刃而解。

結語

以上就是整個擴展卡爾曼濾波器的公式推導和代碼編寫過程。你會發現真正進行工程開發時，除了具有基本的寫代碼能力外，《高等數學》和《線性代數》中的理論知識也是必不可少的。下一期我會將經典卡爾曼濾波和擴展卡爾曼濾波結合起來實現激光雷達數據和毫米波雷達數據的融合。

本文部份內容參考連接：

*《無人駕駛技術入門（十三）| 手把手教你寫卡爾曼濾波器》

【https://zhuanlan.zhihu.com/p/45238681】

*《優達學城（Udacity)無人駕駛工程師學位》

【http://link.zhihu.com/target=https%3A//cn.udacity.com/course/self-drivingcar-engineer--nd013】

*《無人駕駛技術入門（七）| 量產必備的毫米波雷達》

【https://zhuanlan.zhihu.com/p/34675392】

* 以上內容爲開發者原創，不表明百度官方言論。

內容來自開發者知乎專欄：

https://zhuanlan.zhihu.com/p/63641680，歡迎你們收藏點贊。已獲開發者受權，原文地址請戳閱讀原文。

原文連接地址：https://developer.baidu.com/topic/show/290454