參考：https://blog.csdn.net/young_gy/article/details/78468153
Extended Kalman Filter（擴展卡爾曼濾波）是卡爾曼濾波的非線性版本。在狀態轉移方程確定的情況下，EKF已經成為了非線性系統狀態估計的事實標准。本文將簡要介紹EKF，並介紹其在無人駕駛多傳感器融合上的應用。
KF與EKF
本文假定讀者已熟悉KF，若不熟悉請參考卡爾曼濾波簡介。
KF與EKF的區別如下：
- 預測未來：x′=Fx+u用x′=f(x,u)代替；其余F用Fj代替。
- 修正當下：將狀態映射到測量的Hx′用h(x′)代替；其余H用Hj代替。
其中，非線性函數f(x,u)，h(x′)用非線性得到了更精准的狀態預測值、映射后的測量值；線性變換Fj，，Hj通過線性變換使得變換后的x，z仍滿足高斯分布的假設。
Fj，Hj計算方式如下：
為什么要用EKF
KF的假設之一就是高斯分布的x預測后仍服從高斯分布，高斯分布的x變換到測量空間后仍服從高斯分布。可是，假如F、H是非線性變換，那么上述條件則不成立。
將非線性系統線性化
既然非線性系統不行，那么很自然的解決思路就是將非線性系統線性化。
對於一維系統，采用泰勒一階展開即可得到：
對於多維系統，仍舊采用泰勒一階展開即可得到：
其中，Df(a)是Jacobian矩陣。
多傳感器融合
lidar與radar
本文將以汽車跟蹤為例，目標是知道汽車時刻的狀態
。已知的傳感器有lidar、radar。
- lidar：笛卡爾坐標系。可檢測到位置，沒有速度信息。其測量值
。
- radar：極坐標系。可檢測到距離，角度，速度信息，但是精度較低。其測量值
，圖示如下。
傳感器融合步驟
步驟圖如上所示，包括：
- 收到第一個測量值，對狀態xx進行初始化。
- 預測未來
- 修正當下
初始化
初始化，指在收到第一個測量值后，對狀態x進行初始化。初始化如下，同時加上對時間的更新。
對於radar來說，
對於radar來說，
預測未來
預測主要涉及的公式是：
需要求解的有三個變量：F、P、Q。
F表明了系統的狀態如何改變，這里僅考慮線性系統，F易得：
P表明了系統狀態的不確定性程度，用x的協方差表示，這里自己指定為：
Q表明了x′=Fx未能刻畫的其他外界干擾。本例子使用線性模型，因此加速度變成了干擾項。x′=Fx中未衡量的額外項目v為：
v服從高斯分布N(0,Q)。
修正當下
lidar
lidar使用了KF。修正當下這里牽涉到的公式主要是：
需要求解的有兩個變量：H、R。
H表示了狀態空間到測量空間的映射。
R表示了測量值的不確定度，一般由傳感器的廠家提供，這里lidar參考如下：
radar
radar使用了EKF。修正當下這里牽涉到的公式主要是：
區別與上面lidar的主要有：
- 狀態空間到測量空間的非線性映射f(x)
- 非線性映射線性化后的Jacob矩陣
- radar的
狀態空間到測量空間的非線性映射f(x)如下
非線性映射線性化后的Jacob矩陣Hj
R表示了測量值的不確定度，一般由傳感器的廠家提供，這里radar參考如下：
傳感器融合實例
多傳感器融合的示例如下，需要注意的有：
- lidar和radar的預測部分是完全相同的
- lidar和radar的參數更新部分是不同的，不同的原因是不同傳感器收到的測量值是不同的
- 當收到lidar或radar的測量值，依次執行預測、更新步驟
- 當同時收到lidar和radar的測量值，依次執行預測、更新1、更新2步驟
多傳感器融合的效果如下圖所示，紅點和藍點分別表示radar和lidar的測量位置，綠點代表了EKF經過多傳感器融合后獲取到的測量位置，取得了較低的RMSE。