自動駕駛定位系統-卡爾曼濾波Kalman Filter

時間 2020-02-16

標籤自動駕駛定位系統卡爾濾波 kalman filter 欄目系統網絡简体版

原文原文鏈接

Kalman Filter是處理連續變化的動態不肯定系統的理想方法，而且因爲內存佔用小(不須要記錄歷史狀態)，運行速度快，被普遍應用在機器人實時多傳感器融合系統中。算法

What can we do with a Kalman filter

首先看一個簡單的例子: 假設有一個能夠在樹林中自由漫步的機器人，這個機器人配備了一個精度爲10m的GPS傳感器和自身狀態的測量設備(輪速記等)。segmentfault

對於機器人而言，除了可以經過GPS獲取位置信息外，它還準確知道本身下達的全部指令，好比向前前進10m，向右前進5m等等。可是因爲受到外部環境的影響(風向、地面打滑，測量偏差等)，機器人自身得到的測量數據與實際行駛的距離並不徹底吻合。函數

樹林中溝壑、懸崖遍及，不許確的定位信息使得機器人時時都有墜落懸崖的危險。
網站

GPS的測量信息和機器人自身的測量信息都不許確，如何利用這些不肯定的信息獲取更加肯定的、更加準確的信息。Kalman Filter能夠用來解決這個問題。spa

Describing the problem with matrices

機器人在k時刻的State的矩陣形式以下:3d

$$ \begin{aligned} \mathbf{\hat{x}}_k &= \begin{bmatrix} \text{position}\\ \text{velocity} \\ \end{bmatrix}\\ \mathbf{P}_k &= \begin{bmatrix} \Sigma_{pp} & \Sigma_{pv} \\ \Sigma_{vp} & \Sigma_{vv} \\ \end{bmatrix} \end{aligned} $$blog

假設已知機器人在k-1時刻的State，要預測k時刻的State。內存

預測的過程能夠表述爲矩陣變換的過程，變化矩陣爲$\mathbf{F_k}$。
ci

根據基礎的動力學知識:rem

$$ \begin{aligned} {p_k} &= {p_{k-1}} + \Delta t {v_{k-1}} \\ {v_k} &= {v_{k-1}} \end{aligned} $$

用矩陣表示:

$$ \begin{aligned} {\mathbf{\hat{x}}_k} &= \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} {\mathbf{\hat{x}}_{k-1}} \\ &= \mathbf{F}_k {\mathbf{\hat{x}}_{k-1}} \end{aligned} $$

隨機變量的乘以矩陣以後，對協方差矩陣的影響以下：

$$ \begin{aligned} Cov(x) &= \Sigma \\ Cov({\mathbf{A}}x) &= {\mathbf{A}} \Sigma {\mathbf{A}}^T \end{aligned} $$

所以:

$$ \begin{aligned} {\mathbf{\hat{x}}_k} &= \mathbf{F}_k {\mathbf{\hat{x}}_{k-1}} \\ {\mathbf{P}_k} &= \mathbf{F_k} {\mathbf{P}_{k-1}} \mathbf{F}_k^T \end{aligned} $$

External influence

在機器人運動過程當中，外力會對系統的State產生影響。好比系統會發出指令進行加速、減速等。這些外力是明確已知的，如何對系統產生影響也是明確的。這些信息通通被放進$\vec{u}_k$中。

假設咱們已知系統發出的加速的指令，產生的加速度爲${a}$，基於基礎的動力學知識:

$$ \begin{aligned} {p_k} &= {p_{k-1}} + {\Delta t} {v_{k-1}} + \frac{1}{2} {a} {\Delta t}^2 \\ {v_k} &= {v_{k-1}} + {a} {\Delta t} \end{aligned} $$

寫成矩陣形式:

$$ \begin{aligned} {\mathbf{\hat{x}}_k} &= \mathbf{F}_k {\mathbf{\hat{x}}_{k-1}} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} {a} \\ &= \mathbf{F}_k {\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k {\vec{\mathbf{u}}_k} \end{aligned} $$

$\mathbf{B}_k$ 被稱爲Control Matrix，${\vec{\mathbf{u}}_k}$被稱爲Control Vector。

External uncertainty

在運動過程當中，除了機器人自身的屬性(位置、速度)和已知的外力做用以外，還有一些未知的外部環境因素影響帶來新的uncertainty。

這些Untracked Influence能夠用協方差爲${\mathbf{Q}_k}$的Noise來表達。

機器人State中的全部隨機變量的Noise均服從均值相同、方差不一樣的正態分佈。

增長External uncertainty以後的Prediction方程以下：

$$ \begin{aligned} {\mathbf{\hat{x}}_k} &= \mathbf{F}_k {\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k {\vec{\mathbf{u}_k}} \\ {\mathbf{P}_k} &= \mathbf{F_k}{\mathbf{P}_{k-1}} \mathbf{F}_k^T + {\mathbf{Q}_k} \end{aligned} $$

能夠看出:

New Best Estimate = Previous Best Estimate + Known External Influences

New Uncertainty = Old Uncertainty + Additional Uncertainty From The Environment

Refining Estimate With Measurements

傳感器能夠產生一系列的測量結果，這些測量數據用來對Estimate State進行校準。

傳感器讀數和Trace State的Unit和Scale可能不一樣，因此須要用矩陣$\mathbf{H}_k$進行變換。

傳感器讀數的分佈以下:

$$ \begin{aligned} \vec{\mu}_{\text{expected}} &= \mathbf{H}_k {\mathbf{\hat{x}}_k} \\ \mathbf{\Sigma}_{\text{expected}} &= \mathbf{H}_k {\mathbf{P}_k} \mathbf{H}_k^T \end{aligned} $$

Kalman Filter的一個強大之處就在於，它能夠處理傳感器噪聲(Sensor Noise)。以下圖所示，傳感器的讀數是不許確的，在必定範圍內波動，服從正態分佈。

至此，咱們獲得兩個高斯分佈，一個是咱們預測的值(Predicted Measurement)，另一個是從傳感器設備讀取的值(Observed Measurement).

咱們記傳感器的噪聲的協方差爲:${\mathbf{R}_k}$，均值爲:${\vec{\mathbf{z}}_k}$。兩個高斯分佈以下圖所示:

將兩個分佈相乘就獲得兩種狀況同時發生的機率。以下圖重疊區域所示，事實上，重疊區域仍然服從高斯分佈。

高斯分佈融合

一維高斯分佈的機率密度函數以下:

$$ \mathcal{N}(x, \mu,\sigma) = \frac{1}{ \sigma \sqrt{ 2\pi } } e^{ -\frac{ (x – \mu)^2 }{ 2\sigma^2 } } $$

兩個高斯函數的乘積仍然服從高斯分佈:

$$ \mathcal{N}(x, {\mu_0}, {\sigma_0}) \cdot \mathcal{N}(x, {\mu_1}, {\sigma_1}) \stackrel{?}{=} \mathcal{N}(x, {\mu’}, {\sigma’}) $$

其中:

$$ {\mathbf{k}} = \frac{\sigma_0^2}{\sigma_0^2 + \sigma_1^2} $$

$$ \begin{aligned} {\mu’} &= \mu_0 + {\mathbf{k}} (\mu_1 – \mu_0)\\ {\sigma’}^2 &= \sigma_0^2 – {\mathbf{k}} \sigma_0^2 \\ \end{aligned} $$

一樣的，對於多維高斯分佈，有：

$$ {\mathbf{K}} = \Sigma_0 (\Sigma_0 + \Sigma_1)^{-1} $$

$$ \begin{aligned} {\vec{\mu}’} &= \vec{\mu_0} + {\mathbf{K}} (\vec{\mu_1} – \vec{\mu_0})\\ {\Sigma’} &= \Sigma_0 – {\mathbf{K}} \Sigma_0 \end{aligned} $$

${\mathbf{K}}$被稱爲Kalman Gain.

Putting all together

如今咱們有兩個高斯分佈的測量結果:Predicted Measurement和Observed Measurement。

1） Predicted Measurement

$$ ({\mu_0}, {\Sigma_0}) = ({\mathbf{H}_k \mathbf{\hat{x}}_k},{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T}) $$

2） Observed Measurement

$$ {\mu_1}, {\Sigma_1}) = ({\vec{\mathbf{z}}_k}, {\mathbf{R}_k}) $$

根據多維高斯分佈融合理論:

$$ \begin{aligned} \mathbf{H}_k {\mathbf{\hat{x}}_k’} &= {\mathbf{H}_k \mathbf{\hat{x}}_k} + {\mathbf{K}} ( {\vec{\mathbf{z}_k}} – {\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ \mathbf{H}_k {\mathbf{P}_k’} \mathbf{H}_k^T &= {\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} –{\mathbf{K}} {\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} \end{aligned} $$

其中Kalman gain以下:

$$ {\mathbf{K}} = {\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} ( {\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + {\mathbf{R}_k})^{-1} $$

咱們對上述方程進行化簡，去除頭部的$H_K$和尾部的$H_k^T$，獲得以下的更新方程:

$$ \begin{aligned} {\mathbf{\hat{x}}_k’} &= {\mathbf{\hat{x}}_k} + {\mathbf{K}’} ( {\vec{\mathbf{z}_k}} – {\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ {\mathbf{P}_k’} &= {\mathbf{P}_k} – {\mathbf{K}’} {\mathbf{H}_k \mathbf{P}_k} \end{aligned} $$

$$ {\mathbf{K}’} = {\mathbf{P}_k \mathbf{H}_k^T} ( {\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + {\mathbf{R}_k})^{-1} $$

Kalman Filter的運行流程圖

Kalman Filter與Recursive Least Square

Least Square解決的是靜態參數估計的問題，Kalman Filter能夠解決動態變化的狀態的估計和更新問題。

對比KF與RLS的過程:

KF:

預測

$$ \begin{aligned} {\mathbf{\hat{x}}_k} &= \mathbf{F}_k {\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k {\vec{\mathbf{u}_k}} \\ {\mathbf{P}_k} &= \mathbf{F_k} {\mathbf{P}_{k-1}} \mathbf{F}_k^T + {\mathbf{Q}_k} \end{aligned} $$

測量更新

$$ {\mathbf{K}’} = {\mathbf{P}_k \mathbf{H}_k^T} ( {\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + {\mathbf{R}_k})^{-1} $$

$$ \begin{aligned} {\mathbf{\hat{x}}_k’} &= {\mathbf{\hat{x}}_k}+{\mathbf{K}’} ( {\vec{\mathbf{z}_k}} – {\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ {\mathbf{P}_k’} &= {\mathbf{P}_k}–{\mathbf{K}’} {\mathbf{H}_k \mathbf{P}_k} \end{aligned} $$

RLS：

$$ \begin{aligned} K_k=&P_{k-1}H_k^T(R_k + H_kP_{k-1}H_k^T)^{-1} \end{aligned} $$

$$ \hat{x}_{k} = \hat{x}_{k-1} + K_k(y_k - H_k \hat{x}_{k-1}) $$

$$ \begin{aligned} P_k=(I-K_kH_k)P_{k-1} \end{aligned} $$

能夠看出，KF比RLS相比，增長了基於Motion Model的Prediction過程，用於跟蹤State是如何隨時間變化的。