自動駕駛車輛由各種裝置組成，通過該裝置收集數據並執行動作。下圖顯示了車輛中使用的少數重要部件的位置。

自主車輛的組成部分

卡爾曼濾波器使用的數據來自LIDAR和RADAR 。所以現在只關注這兩個方面。

為何使用卡爾曼濾波器？

我們可以使用卡爾曼濾波器進行有根據的猜測，在我們對某些動態系統有不確定信息的任何地方，系統將要做什麼。在自主車輛的情況下，卡爾曼濾波器可用於根據我們的車輛接收的數據預測我們的自動駕駛車輛前方的車輛將採取的下一組動作。這是一個使用兩步預測和更新的迭代過程。

由於以下因素，它主要適用於這些類型的操作：

• 它只根據以前的狀態預測下一個狀態，不需要歷史數據
• 計算速度非常快，非常適合實時問題
• 即使環境中出現大量噪聲/誤差/不確定性，卡爾曼濾波器也會使用得到很好的結果。

我將概述所有這些工作如何通過一個示例來幫助你理解它背後的數學。

卡爾曼濾波器如何工作？

卡爾曼濾波器是一個迭代過程，遵循兩步預測和更新

迭代過程的流程圖

預測：

在該步驟中，卡爾曼濾波器根據初始值預測新值，然後根據系統中存在的各種過程噪聲預測我們預測中的不確定性/誤差/方差。

在自動駕駛過程中，可以通過採用我們車輛前方移動的汽車的簡單示例來理解噪音。我們的模型將假設汽車由於零加速而以恆定速度移動，但實際上它將具有加速度，即速度將不時波動。這種汽車加速度的這種變化是不確定性/誤差/方差，我們使用過程噪聲將其引入我們的系統。

更新：

在此步驟中，我們從系統的設備中獲取實際測量值。在自動駕駛車輛的情況下，這些設備可以是雷達或激光雷達。然後我們計算預測值和測量值之間的差值，然後通過計算卡爾曼增益來決定保持哪個值，即預測值或測量值。根據 卡爾曼增益做出的決定，我們計算新值和新的不確定性/誤差/方差。

來自更新步驟的該輸出再次反饋到預測步驟，並且該過程繼續直到預測值和測量值之間的差異傾向於轉換為零。該計算值將是由卡爾曼濾波器完成的預測。

卡爾曼增益：它確定我們的預測值或測量值是否接近實際值。其值範圍為0到1。如果其值接近0則表示預測值接近實際值，或者如果該值接近1則表示最終測量值接近實際值。它的值範圍從0到1，因為它使用預測值和測量值中的不確定性/誤差，並由下面的簡單公式表示。

K =預測誤差/（預測誤差+測量誤差）

自動駕駛車中卡爾曼濾波器的實現

在此我們將嘗試根據我們迄今為止的信息，瞭解卡爾曼濾波器如何在自動駕駛汽車中使用。

讓我們假設一個2D環境，其中我們有一輛汽車和一個行人移動，如下所示：

汽車和行人的二維表示

p_x和p_y確定2D位置，v_x和v_y確定行人的2D速度。這可以表示為x =（p_x，p_y，v_x，v_y）。我們必須預測行人的2D位置（p_x'，p_y'）和2D速度（v_x'，v_y'），即x'

由於狀態向量僅模擬位置和速度，我們還需要具有不確定性/噪聲以便對加速度建模，這由過程協方差矩陣P完成。

預測步驟 ：使用以下公式計算預測值x'和預測誤差P'也稱為過程協方差矩陣：

x'= Fx +B.μ+ν

P'=FPFᵀ+ Q.

F：狀態轉換矩陣

它用於將狀態向量矩陣從一種形式轉換為另一種形式。即，假設車輛的狀態由位置p和速度v給出，並且在時間t不加速。

X = [p，v]

在特定時間之後說t + 1，新的狀態向量X'將是

p'= p + v *Δt

V'= V

在矩陣中，這可以顯示如下：

x'= F * x

那麼F矩陣將由下式給出：

B：控制輸入​​矩陣

它確定由於外力（縱向或橫向力）和內力（重力，摩擦力等）引起的控制輸入向量μ的變化。在自動駕駛汽車的背景下，我們不能模擬外力，因為它從一個區域到另一個區域變化，而內力則從汽車的一個模型到另一個模型不同。所以大多數是B.μ= 0。

過程噪聲/不確定性

它只不過是可以確定系統中可能存在的隨機噪聲。添加它以使預測位準確。

過程噪音/運動噪音

它在預測位置時給出了物體位置的不確定性。該模型假設速度在時間間隔之間是恆定的，但實際上我們知道物體的速度可以由於加速度而改變。該模型通過過程噪聲包含這種不確定性。

更新步驟：快速回顧更新步驟，我們執行以下操作：

1. 找出實際測量值與預測值之間的差異
2. 計算卡爾曼增益
3. 基於卡爾曼增益的結果計算新X和P 。

現在我們將看到這些步驟的數學實現

第1步：

y = z - Hx'

z：實際測量數據

它是來自傳感器的實際數據。

H：狀態轉移矩陣

與F相同，我們使用H來轉換狀態向量。但是在這裡我們用它來丟棄我們不需要的狀態向量的信息。例如，對於雷達所進行的測量，我們只需要關注車輛的位置，而不必擔心速度，因為雷達只擅長確定速度的位置。所以在這種情況下，對於包含車輛位置和速度的狀態向量，H矩陣將是：

第2步：

S = HP'H + + R K =P'HᵀS-1

R：測量噪聲

測量噪聲是指傳感器測量中的不確定性，這意味著設備本身具有噪聲並且不是100％準確，因此這些噪聲測量值由設備製造商自己提供，無需更改。

S：總誤差

它是預測誤差加上測量誤差的總和。正如我們在上面的卡爾曼增益公式中看到的那樣，我們需要得到總誤差，所以這裡它是S的代表。

K：卡爾曼增益

我們通過前面已經說明的簡單公式來計算卡爾曼增益。唯一的問題是在矩陣中我們不能分開所以我們在這裡採用S - 1。

第3步：

x = x'+ Ky P =（I-KH）P'

最後，我們更新了狀態向量和協方差矩陣，並將繼續下一個預測步驟以獲得其他值。

卡爾曼濾波器的問題

卡爾曼濾波器中使用的所有這些函數都是線性的，因為它們在方程中不包括任何角度參數，因此我們可以直接將這些線性函數饋送到高斯分佈以獲得高斯分佈均值和方差。但在現實生活中，大多數問題都是非線性的。因此，我們不能直接將這種非線性函數饋送到高斯分佈，因為它將導致非高斯分佈，並且我們將不再能夠對其應用卡爾曼濾波器。使用擴展卡爾曼濾波解決了卡爾曼濾波器中的非線性問題。