無跡（損）卡爾曼濾波（EKF）理論講解與實例

無跡（損）卡爾曼濾波（EKF）理論講解與實例

理論講解

前兩篇博客的卡爾曼濾波（參見我的另一篇文章：卡爾曼濾波理論講解與應用（matlab和python））和擴展卡爾曼濾波（參見我的另一篇文章：擴展卡爾曼濾波（EKF）理論講解與實例（matlab、python和C++代碼））都是都將問題轉化爲線性高斯模型，所以可以直接解出貝葉斯遞推公式中的解析形式，方便運算。但對於非線性問題， 擴展卡爾曼濾波除了計算量大，還有線性誤差的影響，有沒有別的方法？EKF利用高斯假設，通過泰勒分解將模型線性化，進而求出預測模型的概率分佈（均值和方差）。而 無跡（損）卡爾曼濾波了（Unscented Kalman Filter ，UKF）則通過不敏變換（Unscented Transform，UT）來求出預測模型的均值和方差。如下圖所示：

UKF生成了一些點，來近似非線性。由這些點來決定實際$x$和$P$的取值範圍。感覺有點像粒子濾波器的概念，但還有些不同，因爲UKF裏的Sigma點的生成並沒有概率的問題。UKF的Sigma點就是把不能解決的非線性單個變量的不確定性，用多個Sigma點的不確定性近似了。

$\color{darkorange}{\textbf{ EKF通過泰勒分解將模型線性化求出預測模型的概率均值和方差}}$

$\color{darkorange}{\textbf{ UKF了則通過不敏變換來求出預測模型的均值和方差}}$

模型對比

模型	缺點	UKF對缺點改進
KF	只適用於線性系統	適用於非線性系統
EKF	線性化忽略了高階項導致強非線性系統誤差大；線性化處理需要計算Jacobian矩陣	對非線性的概率分佈近似，沒有線性化忽略高階項； 不需要計算Jacobian矩陣

UT變換

不敏變換（一種計算非線性隨機變量各階矩的近似方法）可以較好的解決非線性問題，通過一定規律的採樣和權重，可以近似獲得均值和方差。而且由於不敏變換對統計矩的近似精度較高，UKF的效果可以達到二階EKF的效果。

先來說一下什麼叫UT變換：

假設一個非線性系統$y=f(x)$，其中$x$爲$n$維狀態向量，並已知其平均值爲$\overline x$，方差爲$P_x$，則可以經過UT變換構造$2n+1$個Sigma點$x_i$，同時構造$x_i$相應的權值$W_i$，進而得到$y$的統計特性（均值和方差）。 有點類似於概率論裏已知$x$的均值和方差，求$y=f(x)$的$y$的均值和方差。

UKF算法步驟

UKF的算法步驟如下

1. 初始化系統狀態$x_k,P_k$
2. 根據狀態$x_k,P_k$生成Sigma點$X_k$
3. 根據預測模型預測未來的Sigma點$X_{k+1|k}$
4. 根據預測的Sigma點$X_{k+1|k}$生成狀態預測的Sigma點$x_{k+1|k},P_{k+1|k}$
5. 當測量值到來時，將預測的Sigma點$X_{k+1|k}$轉換成預測測量值$Z_{k+1|k}$
6. 根據預測測量值$Z_{k+1|k}$與真實測量值$z_{k+1}$的差值更新得到系統狀態$x_{k+1|k+1},P_{k+1|k+1}$（兩個高斯分佈相乘得到新的系統狀態$x_{k+1|k+1},P_{k+1|k+1}$）

預測部分

那UKF如何運用UT變化來求出預測模型的均值和方差？

1. 首先如何由初始化的系統狀態$x_{k|k}$這個點求出$2n+1$個Sigma點。

利用下圖公式可以求出$2n+1$個Sigma點，$n$代表$x_{k|k}$這個狀態向量的維度，假如$x_{k|k}$只有位置信息，即$x_{k|k}=[p_x,p_y]$，那麼$n=2$，Sigma點就有5個。同理，如果$n=5$，Sigma點就有11個。

在該公式中左邊的$X_{k|k}$代表最後的$2n+1$個Sigma點，右邊的$x_{k|k}$代表初始狀態的均值，$P_{k|k}$代表初始狀態的方差，剩下的兩個式子$x_{k|k} + \left( \sqrt {(n + \lambda)P_{k|k}}\right) \quad x_{k|k} - \left( \sqrt {(n + \lambda)P_{k|k}}\right)$是關於$x_{k|k}$這個點對稱的， $\lambda$可以決定周圍$2n$個sigma點離中心$x_{k|k}$的距離,通常取$\lambda=3-n$， $\lambda$是個經驗公式 。

2. 計算轉換後$y$的均值和方差

現在求出來了這麼多的點來描述原來的狀態分佈（即第$k$步的分佈），那麼經過非線性函數$y=f(x_k,v_k)$變換後，$y$的均值和方差怎麼求呢（即第$k+1$步的分佈）？計算過程如下圖所示：

圖片中$x_{k+1|k,i}$（ $x_{k+1|k,i}$代表Sigma點集合中的第$i$個點 ）可以根據每個Sigma點帶入非線性函數$y=f(x_k,v_k)$求出來，如下圖所示

那$w_i$（$w_i$代表權重）怎麼求呢？
$第一個x_{k|k}點的權重計算如下：\\ w^{[i]} = \frac{\lambda}{\lambda + n}, \quad i = 1\\ 剩下對稱的2n個sigma的點權重計算如下\\ w^{[i]} = \frac{1}{2(\lambda + n)}, \quad i = 2, ..., 2n+1\\ 其中\lambda=3-n$
這樣就可以求出預測後$y$的均值和方差了。下面我們需要求出測量值$\vec z$的均值和方差，然後這兩個分佈相乘就可以求出新的狀態分佈了。

更新部分

針對不同的傳感器，測量值$\vec z$求均值和方差的方式也不同，本文以CTRV模型爲基礎，通過激光雷達（Lidar）和毫米波雷達（Radar）跟蹤車輛位置爲例講解。求出測量值$\vec z$的均值和方差，然後這兩個分佈相乘就可以求出新的狀態分佈了，這個和傳統的卡爾曼濾波基本是一樣的。具體的參見下面例子解釋。

應用實例

CTRV模型

本文將使用CTRV（constant turn rate and velocity magnitude）模型。其狀態變量如下圖所示。

因假定turn rate（$\psi$）、velocity（$v$）不變，其預測噪聲包含加速度與角加速度爲：
$\nu_k = \begin{bmatrix} \nu_{a,k} \\\nu_{\ddot{\psi},k} \end{bmatrix}$
利用$\dot x$及其對時間的積分$x_{k+1}=\int \dot{x}dt$可得預測模型爲：
$x_{k+1}=x_k+\begin{bmatrix} \frac{v_k}{\dot{\psi_k}} (sin(\psi_k+\dot{\psi_k} \Delta t)-sin(\psi_k)) \\\frac{v_k}{\dot{\psi_k}} (-cos(\psi_k+\dot{\psi_k} \Delta t)+cos(\psi_k)) \\0 \\\dot{\psi_k} \Delta t \\0\end{bmatrix}$
考慮預測噪聲爲：
$x_{k+1}=x_k+\begin{bmatrix} \frac{v_k}{\dot{\psi_k}} (sin(\psi_k+\dot{\psi_k} \Delta t)-sin(\psi_k)) \\\frac{v_k}{\dot{\psi_k}} (-cos(\psi_k+\dot{\psi_k} \Delta t)+cos(\psi_k)) \\0 \\\dot{\psi_k} \Delta t \\0\end{bmatrix} + \begin{bmatrix} \frac{1}{2} \nu_{a,k} \cos(\psi_k) \Delta t^2 \\\frac{1}{2} \nu_{a,k} \sin(\psi_k) \Delta t^2 \\\nu_{a,k} \Delta t \\\frac{1}{2} \nu_{\ddot{\psi},k}\Delta t^2 \\\nu_{\ddot{\psi},k} \Delta t \end{bmatrix}$

預測處理

產生點雲

下圖的公式爲生成Sigma點的公式。 第一列就是初始化的系統狀態$x_{k|k}$現在的值，也就是從上一個狀態接手的$x$值。第二列和第三列的公式中 $\lambda$ 是一個數字。具體算法是$\lambda=3-n$，是個經驗公式。這裏$n$就是狀態變量的個數。 如果我們有5個狀態需要測量，那麼$n$就等於5。 那麼根據下面公式就可以得到[5x11]的矩陣了。生成的矩陣代表的含義就是，按照一定規律生成了環繞在x周邊的10個點。 由這10個點的平均值定義$x$的實際值（見下兩圖）。事實上，$\lambda$ 表現的是Sigma點離$x$的距離。

Sigma點之前

生成Sigma點

生成增廣矩陣

什麼叫增廣矩陣？（augmented matrix）。 因爲我們的狀態方程裏面是有噪聲$vk$的。當這個$vk$對我們的狀態轉移矩陣有影響的話，我們需要講這個噪聲$vk$考慮到我們的狀態轉移矩陣裏面的。所以，我們同時也把$vk$當作一種狀態（噪聲狀態）放進我們的狀態變量空間裏。
$x_{k+1}=F(x_k,v_k)\\z_k=H(x_k,n_k)$
其預測噪聲包含加速度與角加速度爲：
$\nu_k = \begin{bmatrix} \nu_{a,k} \\\nu_{\ddot{\psi},k} \end{bmatrix}$

考慮預測噪聲爲：
$x_{k+1}=x_k+\begin{bmatrix} \frac{v_k}{\dot{\psi_k}} (sin(\psi_k+\dot{\psi_k} \Delta t)-sin(\psi_k)) \\\frac{v_k}{\dot{\psi_k}} (-cos(\psi_k+\dot{\psi_k} \Delta t)+cos(\psi_k)) \\0 \\\dot{\psi_k} \Delta t \\0\end{bmatrix} + \begin{bmatrix} \frac{1}{2} \nu_{a,k} \cos(\psi_k) \Delta t^2 \\\frac{1}{2} \nu_{a,k} \sin(\psi_k) \Delta t^2 \\\nu_{a,k} \Delta t \\\frac{1}{2} \nu_{\ddot{\psi},k}\Delta t^2 \\\nu_{\ddot{\psi},k} \Delta t \end{bmatrix}$
也就是說，按照上面的介紹中說道，假設原來的狀態變量個數是5個。那麼由於還要顧及$\nu_{a,k}$和$\nu_{\ddot{\psi},k}$的影響，要把這兩個噪聲也放進狀態變量裏。

5個狀態–>7個狀態。 5個原來的狀態+2個噪聲狀態。

原狀態

擴展狀態

擴展狀態和協方差

生成預測點

現在我們生成了增廣的Sigma點 ，那麼因爲物體會按一定規律移動，所以我們需要預測物體的下一個狀態。這裏就是根據狀態轉移矩陣來計算的，我們只需要把每個Sigma點插入過程模型$x_{k+1} = f(x_k,\nu_k)$即可。我們對物理現象的建模過程在CTRV模型那一節已經說過，這裏就不多闡述。 需要說明的是上式的矩陣是[7x15]的，經過過程模型計算，下式的矩陣是[5x15]的。

計算預測的均值和方差

我們現在有很多預測後的Sigma點。那麼我們需要計算預測的均值和方差了。注意weight的第一個值的計算方法和其他不一樣哦。參數的解釋在理論講解裏面說過了。

更新處理

假設我們有激光雷達（Lidar）和毫米波雷達（Radar）兩個傳感器，它們分別以一定的頻率來測量如下數據：

1. 激光雷達：測量目標車輛的座標 $(x,y)$ 。這裏的$x,y$是相對於我們的車輛的座標系的，即我們的車輛爲座標系的原點，我們的車頭爲$x$軸，車的左側爲$y$軸。
2. 毫米波雷達：測量目標車輛在我們車輛座標系下與本車的距離$ρ$，目標車輛與x軸的夾角 $ψ$，以及目標車輛與我們自己的相對距離變化率 $\dot ρ$（本質上就是目標車輛的實際速度在我們和目標車輛的連線上的分量）

前面的卡爾曼濾波器中，我們使用一個測量矩陣 $H$ 將預測的結果映射到測量空間，那是因爲這個映射本身就是線性的，現在，我們使用毫米波雷達和激光雷達來測量目標車輛(我們把這個過程稱爲傳感器融合)，這個時候會有兩種情況，即：

1. 激光雷達的測量模型仍然是線性的，其測量矩陣爲：

$H_L = \left[ \begin{array}{c} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \end{array} \right]$

將預測映射到激光雷達測量空間：
$H_L\overrightarrow{x} = (x, y)^T$

2. 毫米波雷達的預測映射到測量空間是非線性的，其表達式爲：

$\left(\begin{matrix} \rho \\ \psi \\ \dot\rho \end{matrix}\right) = \left(\begin{matrix}\sqrt{x^{2} + y^{2}}\\\operatorname{atan_{2}}{\left (y,x \right )}\\\frac{v x + v y}{\sqrt{x^{2} + y^{2}}}\end{matrix}\right)$

此時我們使用 $h(x)$來表示這樣一個非線性映射

預測量測值

測量更新分爲兩個部分，Lidar測量和Radar測量，其中Lidar測量模型本身就是線性的，所以我們重點還是放在Radar測量模型的處理上面。

爲了計算我們預測的$z$和實際的傳感器數據$z$的誤差，我們需要通過測量轉移矩陣$h(x)$把狀態空間向量和傳感器可得到的數據關聯起來。因爲毫米波雷達的測量轉移矩陣$h(x)$是一個非線性的函數，這裏和我們預測步驟碰到的問題非常類似，我們也需要求出一些Sigma點，然後通過非線性函數$h(x)$把預測值$X_ {k+1|k}$（矩陣是[5x15]）轉換到量測空間$\vec z_{k+1}$（矩陣是[3x15]，$z_{k+1}$就是求的$z_{預測值}$。），這裏我們就可以偷一個懶，只需重用我們在預測步驟已經有的Sigma點即可， 我們這次可以跳過生成sigma點，我們直接用生成的預測點$x_ {k+1|k}$帶入$z_{k+1}=h(x_{k+1}+w_{k+1})$求得量測空間的值。注意，這裏$z_{k+1}$只是我們計算出來的，並不是來自傳感器的數據。

狀態轉移矩陣h(x)

矩陣大小的變化

計算預測量測值的均值和方差

1. 先利用上一步Sigma點預測值（也就是矩陣是[5x15]的Sigma點）挨個求出相對應的測量值預測值。這樣我們可以得到15列相應的測量值（矩陣是[3x15]）。這個值就是下圖中的$z_{k+1|k,i}$。
2. 這裏又一次出現了$w_i$ 。 $w_i$就是上一步中用來求$x$平均值的權重。這裏我們可以不需要額外的計算，依然用預測過程中求得的 $w_i$值。
3. 把每一個 $w_i$（[1x15]）和對應的每一個$z_{k+1|k,i}$（[3x15]）相乘並相加，最終得到預測值$z$的均值。
4. 這裏$w_{k+1}$ 是量測模型裏面的噪聲。他對系統沒有非線性影響，所以也不用被擴展測量向量空間，只需要簡單的相加就可以了。
5. 然後計算預測量測值的協方差。按照一下公式計算就可以。同樣，因爲$R$也是因爲沒有非線性影響，所以也可以直接相加。

更新狀態

現在我們有預測的狀態均值$x_{k+1|k}$和協方差$P_{k+1|k}$，以及預測的測量均值$z_{k+1|k}$和協方差$S_{k+1|k}$， 但我們還需要一個東西 就是我們從時間步 $k+1$ 收到的實際測量值 $z_{k+1}$

這是UKF的最後階段。這裏我們最終根據$x$預測值，和$z$預測值求出kalman gain$K_{k+1|k}$和cross-correlation function$T_{k+1|k}$然後最終更新狀態和協方差，這些都是單純的計算。而且state update和covariance matrix update都是跟標準卡爾曼濾波器一樣的。 UKF獨有的計算有kalman gain和cross-correlation function。不過也都是單純的計算。這裏我想說一下，我理解的cross-correlation function的作用。 通過式子我們可以看出求cross-correlation function的內部結構。他需要每個預測的Sigma點和$x$預測值的差和每個Sigma點預測的測量值和z預測值的差 。也就是說，cross-correlation 這個方程會根據Sigma點和預測值之間的差來平衡kalman gain。 而kalman gain裏面不僅用到cross-correlation function，還會用到測量值協方差預測值。這樣kalman gain就可以利用每個預測的Sigma點和x預測值的差和每個Sigma點預測的測量值和$z$預測值的差，來平衡模型的預測準確度和傳感器的預測準確度。（kalman gain 就是一種權重）

完整代碼

事實上，這個project 是融合Lidar 和Radar的算法。說是融合，但說白了就是，不同時間段處理不同傳感器input而已。因爲這個input的不一樣決定了，裏面運行的代碼是ukf還是ekf。像在這個project 裏面，因爲lidar 是線性的，所以就不需要用到UKF ，而radar因爲傳感器獲取的數據種類就要求了它要用UKF來實現（當然EKF也可以，但是精度低而已）。

完整的C++代碼：C++_UKF_CTRV 代碼

另外這裏還有一個代碼例程把EKF和UKF做了一個對比。
C++_EKF_UKF_實驗對比
仿真場景是跟蹤預測機器人的位置，實驗圖片如下：


最後，插播一條廣告，這裏有完整的優達學城（udacity）無人駕駛車完整的教程，帶中英文字幕（每一學期的中英文字幕都有喲！），需要的私信我， 嘻嘻~~。

參考鏈接

1. UKF的理念
2. 無損濾波器 UKF
3. 無跡Kalman濾波算法(matlab)
4. 無損卡爾曼濾波器-UKF
5. 無損卡爾曼濾波UKF與多傳感器融合
6. 無損卡爾曼濾波
7. 無跡卡爾曼濾波器完整公式推導