無人駕駛技術——擴展Kalman濾波（EKF）

基本的卡爾曼濾波及實現可參考：無人駕駛技術——初探Kalman濾波器
卡爾曼濾波器介紹還可參考：https://blog.csdn.net/u010720661/article/details/63253509

卡爾曼濾波方程和擴展卡爾曼濾波方程非常相似。主要區別在於：

基本原理

綜合預測值，直接測量值，得到更加接近真實的值。

運用範圍

運用於線性模型。比如採用恆定速度模型，然後加減速用處理噪聲來表示，這一模型用來估算行人的狀態其實已經足夠了。

以機器人移動爲例，推導過程：

1. 以當前的速度和位置，預測下一步的速度和位置，Fk爲狀態轉移矩陣：
   

2. 更新下一狀態協方差矩陣，由（4）協方差的更新規則，可以得到（5）的協方差矩陣更新：
   

3. 內部控制量的加入，也會改變狀態，比如剎車或加速，若給定的加速度爲a, 狀態變化如下式所示，Bk爲控制矩陣，uk爲控制向量：

4. 外部干擾，比如風，或者路滑等一些因素，會干擾預測的準確度，也就是會增加協方差。所以外部干擾Qk可以作爲噪聲處理，加到協方差上。

注意：此項目中採用另一種思路，即將內部控制量，以噪聲Qk的形式加入，推導過程：
由於加速度（內部控制量）存在，狀態更新爲：

由於加速度未知，同時相融合外部噪音影響，所以將其帶來的改變作爲噪音項進行處理：

獲得噪音協方差矩陣Q，其中加速度a假設爲0均值，σ方差的正態分佈，互不相關。

5. 更新，用測量值來更新預測值。預測值是具有一定方差的高斯分佈，測量值也是具有一定方差的高斯分佈。融合這兩個高斯分佈，可以得到兩分佈中都發生的的概率，則爲最優估計，仍未高斯分佈。

融合高斯分佈：

以一維分佈舉例。

得到融合分佈後的，均值和方差。

推廣到多維分佈，得到一般化公式。

案例分析：

對如下兩個分佈進行高斯融合，前者爲預測分佈，後者爲測量分佈：

預測分佈爲什麼要加上觀測矩陣（測量矩陣）Hk了，因爲傳感器讀取的數據的單位和尺度，形式（直角座標，極座標）有可能與我們要跟蹤的狀態的單位和尺度不一樣，加上Hk將預測值轉換到傳感器尺度。

根據(14),(15)的規則，可以得到更新後的分佈：

將其化簡，可以得到更新後的狀態和協方差：

將新的狀態和協方差，放到下一個預測和更新方程中不斷迭代。

卡爾曼濾波器數據流直觀圖：

擴展卡爾曼濾波器：

卡爾曼濾波器用於處理線性系統，擴展卡爾曼濾波器用於處理非線性系統。將非線性系統轉化爲線性系統，其他的就和卡爾曼濾波器一樣了。

系統的線性和非線性：

線性系統：

線性，指量與量之間按比例、成直線的關係。在空間和時間上代表規則和光滑的運動。

非線性系統：

非線性則指不按比例、不成直線的關係。如問：兩個眼睛的視敏度是一個眼睛的幾倍？很容易想到的是兩倍，可實際是 6－10倍！
主要區別：是否能夠採用疊加原理來進行分析。
線性系統的表達式中只有狀態變量的一次項，高次、三角函數以及常數項都沒有，只要有任意一個非線性環節就是非線性系統。

爲什麼要採用線性系統：

預測狀態是高斯分佈，如果將其映射到非線性函數上，則將不會保持高斯分佈，這樣破壞了高斯分佈的基本前提，就不可以用卡爾曼濾波器了。

將非線性系統轉化爲線性系統：

泰勒展開，去掉一階項後面的部分，就變成線性系統了。多維泰勒展開，一階偏導爲雅可比（jacobian）矩陣，二階偏導爲hessian矩陣。
雷達案例：需要將預測狀態空間，轉換到雷達測量空間。

然而h(x)爲非線性函數，所以需要將其線性化，根據多維泰勒展開，可得jacobian矩陣Hj：

雅可比矩陣的推導和代碼實現可參考：
雅克比矩陣

然後其他部分和卡爾曼部分一樣，只是稍作更新：

注意事項1：預測部分，如果是線性的，則不需要更改。如果不是，則和求jacbian矩陣Hj一樣，求Fj。
注意事項2：擴展卡爾曼濾波器，y值的求得，直接採用右邊式子即可。只是變換了Hj.
注意事項3：上面對比圖片公式，和上面卡爾曼推導公式(18),(19)是一樣的，只是略作展開。
稱K爲卡爾曼增益。
案例分析：詳細c++代碼見GitHub：https://github.com/godloveliang/Extended-Kalman-Filter

If f and h are linear functions, then the Extended Kalman Filter generates exactly the same result as the standard Kalman Filter. Actually, if f and h are linear then the Extended Kalman Filter F_j turns into f and H_j turns into h. All that’s left is the same ol’ standard Kalman Filter!

In our case we have a linear motion model, but a nonlinear measurement model when we use radar observations. So, we have to compute the Jacobian only for the measurement function.