CS373無人駕駛汽車編程課程講義翻譯二：卡爾曼濾波器

01-26

本文翻譯自Udacity上Sebastian Thrun開設的Artificial Intelligence for Robotics（機器人人工智慧）課程筆記Lesson2Notes，正文如下：

簡介

斯坦福最新的自動駕駛汽車Junior配備了以每秒十次進行距離掃描的激光測距儀——約一百萬個數據點。收集所有這些數據的主要功能是定位其他車輛。來自測距儀的數據為卡爾曼濾波器提供了關鍵輸入，這是第二單元的主題！

自動駕駛汽車

知道如何檢測其他車輛會使我們的自動駕駛汽車不會撞上其他車輛。我們不僅需要知道其他汽車的位置，如定位案例，還要知道為了避免碰撞，它們移動的速度有多快。

追蹤

卡爾曼濾波器是用於估計系統狀態的流行技術。卡爾曼濾波器估計連續狀態並給出單模分布。

蒙特卡羅定位方法是你在第一個單元中學到的方法，雖然我們當時沒有使用該名稱。

例子：

一輛汽車（大矩形）在t = 0，t = 1，t = 2，t = 3時感測到空間中的一個物體（*），當t = 4時，你會認為物體在哪裡？從三個選項框中選擇一個。

解答：

速度根據矢量（紅色箭頭）而變化。假設沒有速度的急劇變化，你可以預期t = 4時沿著相同的矢量線。

在這個課程中，我們將學習編寫一個軟體，即使它們不確定，就像在這個例子中一樣，你也可以獲得這樣的點，並估算未來的位置。你還可以識別物體在運動中的速度。Google自動駕駛汽車基於激光和雷達數據使用這些方法來了解交通。

高斯

直方圖將連續空間分割成離散區域。這是一種逼近連續分布的方法。

在卡爾曼濾波器中，分布是以所謂的高斯形式給出的，它是位置空間上的連續函數。高斯下面的區域加起來等於1。

如果我們稱空間（x軸）為x，那麼高斯的特徵是兩個參數——均值（希臘字母mu， $mu$ ）和高斯的寬度，稱為方差（通常寫為二次變數 $sigma^{2}$ ）。

一維參數空間中的任何高斯的特徵在於（ $mu$ ， $sigma^{2}$ ）。使用卡爾曼濾波器時，我們的任務是保持 $mu$ 和 $sigma^{2}$ ，這是我們正在嘗試找到的對象位置的最佳估計。

確切的公式是二次函數的指數：

$f(x) = frac{1}{sqrt{2pi(sigma^{2})}}e^{-frac{1}{2}frac{(x-mu)^{2}}{sigma^{2}}}$

問題1（高斯介紹）：

檢查哪些函數是高斯：

答案1：高斯介紹

這裡有三個函數是高斯的。這三個函數說明高斯的顯著特徵是有一個峰值且兩邊對稱。一個峰值表示函數是單峰的（unimodal）。

「unimodal」這個詞是什麼？ Uni表示1個。Modal是指分布的眾數。如果你回想起概率剛開始時，你可能還記得學習一組數據的平均值，中位數和眾數。眾數是最常顯示的數字。以概率語言表示，它是最可能的值，它在概率圖上顯示為峰值。這就是為什麼單峰分布只有一個峰，而多峰分布有許多峰。

問題2（方差比較）：

選擇能準確描述高斯的協方差的框。

答案：方差比較

大擴散=大協方差;小擴散=小協方差

請注意，術語「方差」和「協方差」在這裡可以或多或少地互換使用。現在，你可以把它們視為同樣的東西。

協方差是不確定性的量度。 $sigma^{2}$ 越大，我們對實際狀態的不確定性越大。

問題3（最優高斯）：

我們更喜歡哪種高斯追蹤自動駕駛汽車？

答案：最優高斯

第三個是最確定的，這使得再次撞到另一輛車的機會變小了

尋找高斯峰值

問題4（計算高斯）：

計算給定x的高斯的值（你將需要一個計算器）

$mu$ = 10
$sigma^{2}$ = 4
x = 8

答案：計算高斯

對於這些值：

$mu$ = 10
$sigma^{2}$ = 4
x = 8

$f(x) = ？$

$f(x) = frac{1}{sqrt{2pi(sigma^{2})}}e^{-frac{1}{2}frac{(x-mu)^{2}}{sigma^{2}}}$

$f(x) = frac{1}{sqrt{2pi4}}e^{-frac{1}{2}frac{(8-10)^{2}}{4}}$

$f(x) = frac{1}{sqrt{8pi}}e^{-frac{1}{2}frac{(-2)^{2}}{4}}$

$f(x) = frac{1}{sqrt{8pi}}e^{-0.5}$

$f(x) = 0.19947ast0.60653$

$f(x) = 0.120985$

編程一個高斯

使用上面的值並從下面的源代碼開始，你將如何完成程序，以便返回0.12的輸出——高斯的峰值：

from math import *def f(mu, sigma2, x): return #complete this line so that it returns the Gaussian function, given the argumentsprint f(10.,4.,8.)

解答：

from math import *def f(mu, sigma2, x): return 1/sqrt(2. * pi * sigma2) * exp(-.5 * (x - mu) ** 2 / sigma2) #use the formula as we did manually aboveprint f(10., 4., 8.)

問題5（最大化高斯）：

如何修改x，也就是8，以獲得函數f的最大返回值？

答案：最大化高斯

答案基本上是與mu相同的答案，所以我們得到高斯的峰值。因此，將x設置為與mu相同的值10。從我們迄今所看到的高斯圖中，我們可以看到，峰值總是出現在均值處。這就是為什麼當我們想最大化的時候，我們設置x = $mu$ 。

測量運動

卡爾曼濾波器代表高斯的所有分布，並且迭代了兩件事情：

測量更新
運動更新

問題6（測量運動1）：

測量或運動的兩個步驟中的每一個都需要卷積或乘積。

你記得哪個需要哪個嗎？選擇相應的框：

答案：測量運動1

還記得第一單元，當我們進行測量時，我們將信度乘以一定的因子（我們之前稱之為pHit和pMiss）。當我們進行運動時，我們進行了卷積（本質上是概率的加法）。如果你需要複習，請返回第一單元。

問題7（測量運動2）：

檢查貝葉斯規則或總概率是否適用於測量或運動：

答案：測量運動2

在單元1中，我們使用貝葉斯規則來計算給定測量Z的後驗分布。我們將其寫為P（X|Z）。在運動中，當我們通過追蹤移動之前的概率來計算移動後每個單元格中的概率時，運用了總概率。如果你需要複習，請查看單元1。

卡爾曼濾波器

當使用卡爾曼濾波器時，測量周期通常被稱為測量更新，而運動周期通常稱為預測。測量更新將使用貝葉斯規則——乘積，乘法。預測更新將使用總概率——卷積，加法。

我們可以用高斯說這兩個周期。

假設你正在定位另一輛車，並且有一個均值為mu的先驗分布，如下所示：

然後我們得到一個均值為nu的藍色的測量，告訴我們一些關於車輛的定位。

問題8（改變均值）：

後面的高斯的新均值在哪裡？選擇正確位置。

答案：改變均值

在兩個均值之間，先前的均值和測量的均值。輕微的靠近測量一面，因為與之前相比，車輛的位置更加確定。我們越肯定，我們越是傾向於某個答案的方向

問題9（預測峰值）：

在兩個高斯相乘後，正確的頂部在哪裡？這個問題很難，抓住機會吧。

答案：預測峰值

所得到的高斯比兩個構成的高斯更確定。也就是說，協方差小於獨立的兩個協方差中的任何一個。直觀地說，這是因為我們實際上可以從兩個高斯上獲取信息。

起初這個結果可能看起來不是很直觀。它有助於想到這兩個測量同時進行（可能一個來自激光，另一個來自雷達感測器）。這些感測器中的每一個可能會出錯，但是通過使用兩個感測器，一定比我們單獨使用一個更加確定我們的位置（否則為什麼我們甚至會使用第二個感測器？）。這意味著產生的峰值必須更高。

證明

警告：在數學上這個證明很難！如果你真的想了解它，你應該自行工作，在遇到困擾時諮詢本文檔。

步驟1：乘以高斯（記住測量總是意味著乘法）

$e^{-frac{1}{2}left[ frac{(x-mu)^{2}}{sigma^{2}}+frac{(x- u)^{2}}{r} ight]}$

為了我們的理智，讓我們忽略指數前面的規範化部分。他們不會影響我們感興趣的這個計算。

當我們進行乘法運算時，剩下的是與之成比例的東西：

$e^{-frac{1}{2}left[ frac{(x-mu)^{2}}{sigma^{2}}+frac{(x- u)^{2}}{r^{2}} ight]}$

注意括弧中的表達式是二次的。讓我們解決這個表達式來尋找新的均值和協方差。

如果我們想找到均值，我們希望最大化整個函數（因為峰值總是出現均值處）。我們通過最小化二次表達式來完成這一步，這是通過設置一階導數等於零來完成的。如果你從未學過微積分，那麼就採取這一步驟，並繼續進行論證。當我們取一階導數時，我們得到：

$frac{d}{dx}=2frac{(x-mu)^{2}}{sigma^{2}}+frac{x- u}{r^{2}}$

我們把這設為0並求x。我們發現：

$x=frac{mu r^{2}+ u sigma^{2}}{r^{2}+sigma^{2}}$

這是一個漂亮的結果！我們來談談吧

首先，不要過分關注分母。有趣的東西在上面。我們在分子中有一個加權和，但是注意到第一個高斯的均值是由第二個高斯的方差加權的。這有道理嗎？

它是！如果第二次分布有很大的方差，那麼這不是一個很好的測量，我們寧願強調第一次分布。這正是我們的方程式所說的。

好的，方差如何？讓我們利用指數中二次表達式的二階導數等於二除以方差這一事實。應用這一點，我們發現如下：

$frac{d^{2}}{dx^{2}}=frac{2}{sigma^{2}}+frac{2}{r^{2}}=frac{2}{sigma^{2$

其中 $sigma^{2$ 是我們的新方差。當我們解決這個 $sigma^{2$ 時，我們發現:

$sigma^{2$

注意，這個方差總是小於我們初始方差中的任何一個。

相等方差

假設我們乘兩高斯，如貝葉斯規則，現有的 $mu$ 和概率 $sigma^{2}$ ，以及測量的均值 $u$ (nu)和協方差 $r^{2}$ ，這樣新的均值 $mu$ 是舊均值的加權和，其中 $mu$ 由 $r^{2}$ 加權， $u$ 由 $sigma^{2}$ 加權，並通過加權因子的總和歸一化。

更新後的新方差項由下式給出：

$frac{1}{sigma^{2$

我們可以在圖上確定新均值的位置，因為我們知道：

先前的高斯具有更高的不確定性，更長的較低的斜率，因此 $sigma^{2}$ 較大
這意味著 $u$ 的加權比 $mu$ 大得多
所以均值將比 $mu$ 更接近 $u$ （藍色刻度）

問題10（參數更新）：

使用以下高斯函數和給定方程，計算更新後的新均值和新的 $sigma^{2$ 。

$mu$

$sigma^{2$

$mu=10$

$u=12$

$sigma^{2}=4$

$r^{2}=4$

答案：參數更新

$mu$

$sigma^{2$

不相等方差

問題11（參數更新2）：

再來一次，使用以下高斯函數和給定方程，計算更新後的新均值和新的 $sigma^{2$ 。

$mu$

$sigma^{2$

$mu=10$

$u=13$

$sigma^{2}=8$

$r^{2}=2$

答案：參數更新2

$mu$

$sigma^{2$

問題12（分離高斯）：

假設我們有一個先前的概率和一個測量的概率彼此距離很遠，兩者都具有相同的協方差。新的均值在哪裡？

答案：分離高斯

新的均值在中間，因為協方差是相同的。

問題13（分離高斯2）：

高斯看起來像什麼？

答案：分離高斯2

是最高峰的高斯，因為新的 $sigma^{2}$ 是獨立於均值而獲得的。再次，這似乎很奇怪，但是當我們有兩個信息來源時，我們期望我們的知識更準確。這解釋了更高的峰值。

新的均值方差

編寫一個實現以下等式的Python程序：

$mu$

$sigma^{2$

這是一個骨架函數，它有一個update函數，並將第一個分布的均值和方差以及第二個分布的均值和方差作為其輸入。該函數輸出兩個分布的乘積的新均值和新方差。用提供的值進行測試：

def update(mean1, var1, mean2, var2): new_mean = #enter your code here new_var = #enter your code here return [new_mean, new_var]print update(10., 8., 13., 2.)

你應該得到結果 → 12.4, 1.6

解答：

def update(mean1, var1, mean2, var2): new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2) new_var = 1/ (1/ var1 + 1/ var2) return [new_mean, new_var}print update(10., 8., 13., 2.)

12.4, 1.6

以相等的方差並使用10和12作為均值再次運行：

def update(mean2, var1, mean2, var2): new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2) new_var = 1/ (1/ var1 + 1/ var2) return [new_mean, new_var]print update(10., 4., 12., 4.)

11.0, 2.0

恭喜！你剛剛編寫了卡爾曼濾波器的基本更新。

運動更新

下圖顯示了你所在位置的最佳預測（x軸上的藍色刻度）和你的不確定性。假設你向右移動（綠線），這個運動本身就有自己的不確定性。然後，你可以得到一個預測，它將運動命令添加到均值中，並且與初始不確定度相比具有增加的不確定性（紅線）。

直觀地說，這是有道理的，當你向右邊移動時，你的不確定性會增加，因為你失去了關於你所在位置的信息（正如不確定性所表現的那樣，綠色高斯）。

數學：新的均值是舊的均值加上運動u。新的 $sigma^{2}$ 是舊的 $sigma^{2}$ 加運動高斯的方差 $r^{2}$ 。這與第一單元非常相似，運動減少了我們的知識，更新增加了我們的知識。這是定位的一個基本方面。

問題14（高斯運動）：

給出如下，找到新的高斯值 $mu$ 和 $sigma^{2$

$mu=8$

$sigma^{2}=4$

Move：

$u=10$

$r^{2}=6$

$mu$

$sigma^{2$

答案：高斯運動

$mu$

$sigma^{2$

問題15（預測函數）：

下面是一些基本代碼。使用predict函數計算新更新的均值和方差，使用給定的值得到結果[22.0, 8.0]

def update(mean1, var1, mean2, var2): new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2) new_var = 1/ (1/ var1 + 1/ var2) return [new_mean, new_var]

答案：預測函數

只需加上兩個均值和兩個方差：

def update(mean2, var1, mean2, var2): new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2) new_var = 1/ (1/ var1 + 1/ var2) return [new_mean, new_var]def predict(mean1, var1, mean2, var2): new_mean = mean1 + mean2 new_var = var1 + var2 return [new_mean, new_var]print predict(10., 4., 12., 4.)

22.0, 8.0

上面的整個程序實現了一維卡爾曼濾波器。

卡爾曼濾波器代碼

編寫一個主程序，它具有兩個函數，update和predict，並提供給它們測量和運動序列。

將初始測量的 $mu$ 設置為0，並有一個非常大的不確定性為10,000。

假設測量不確定性measurement_sig為4，運動不確定性motion_sig為2。

def predict (mean1, var1, mean2, var2): new_mean = mean1 + mean2、 new_var = car1 + var2 return [new_mean, new_var] measurements = [5., 6., 7., 9., 10.] motion = [1., 1., 2., 1., 1.] measurement_sig = 4. motion_sig = 2. mu = 0. sig = 10000

你應該得到這些輸出：

update: [4.99, 3.99] predict: [5.99, 5.99] update: [5.99, 2.39] predict: [6.99, 4.399] update:[6.99, 2.09] predict: [8.99, 4.09] update: [8.99, 2.02] predict: [9.99, 4.02] update: [9.99, 2.00] predict: [10.99, 4.00]

以下是我們的卡爾曼濾波器代碼：

for n in range(len(measurements)): [mu, sig] = update(mu, sig, measurements[n], measurement_sig) print "update: ", [mu, sig] [mu, sig] = predict(mu, sig, motion[n], motion_sig) print "predict: ", [mu, sig]

我們明白我們在這裡做什麼是非常重要的，所以讓我們一起來看看：

這段代碼是說「對於我們已有的每次測量，重複兩個步驟」。這兩個步驟是更新和預測步驟。在更新步驟中，我們將新的測量結合到我們以前的信度中，以產生一個新的信度（這比我們以前的信度更確定）。在預測步驟中，我們用推斷的速度再一次創造出一個新的信度（這比我們以前的信度不太確定）。

這正是卡爾曼濾波器真正令人驚奇的地方：他們從來不必擔心兩件事情！在更新步驟的情況下，這兩件事情是以前的信度和測量。通過預測，它們是先前的信度和推斷的速度。這使得卡爾曼濾波器所需的計算能力相對較低，並允許其實時運行。

多維卡爾曼濾波器

例子：

假設你有一個二維的狀態空間，就像攝像機的圖像。假設在時間t = 0，你觀察到感興趣的對象處於特定的坐標。稍後你會看到對象在另一個點。

問題16（卡爾曼預測）：

你希望在t = 3時對象在哪裡？填寫最可能的位置。

答案：卡爾曼預測

在多維空間（例如像現實世界）中，卡爾曼濾波器不僅可以讓你估算位置，還可以從這些位置測量中推斷出速度。這些推測的速度可以讓你預測你未來的位置。

請注意，感測器本身只能看到位置，而不能看到實際速度，速度可以從觀察多個位置推斷出。

通過能夠計算出物體的速度，卡爾曼濾波器可以對包含速度的未來位置進行預測。這就是為什麼卡爾曼濾波器在人工智慧中如此受歡迎的原因。

多元高斯

到目前為止，我們已經討論了一維運動，儘管這涉及到卡爾曼濾波器的所有要素，但我們至少應該簡要地討論當我們從一維走向更高維時會發生什麼。

均值將不是一個數字，而成為一個向量，每個維度都有一個元素。
方差被協方差代替，當空間具有d維時，它是具有d行和d列的矩陣。
這給了我們一個複雜的公式，但它不是你需要記住的東西。

這裡有一個多元高斯的插圖。在該圖像中，均值由 $x_{0}$ 和 $y_{0}$ 表示，協方差是輪廓線：

具有較小不確定性的高斯將具有較小的輪廓線：

另外也有可能在一個維度上有一個很小的不確定性，另一個維度有很大的不確定性：

當高斯傾斜時，這意味著x和y是相關的：

建立一個二維的估計，其中x軸是位置，y軸是速度，我們將其表示為v，但是請記住，Sebastian使用帶有點的x。（在其上具有單個點的x表示相對於t（時間）的x（位置）的一階導數，這是表示速度的另一種方式）。

如果，最初你知道位置，而不是速度，你可以用一個細長的高斯來代表它的正確的位置，但在速度空間真的很寬泛。

在預測步驟中，由於你不知道速度，你不能預測你將要處在什麼位置。然而，有一些有趣的相關性。

假設速度是0，預測後的位置在哪裡？

如果你知道你從位置1開始，那麼速度為零會使你處於完全相同的地方（1，0）。

問題17（卡爾曼濾波器預測）：

使用相同的圖，假設速度是1，速度1從位置1（1，0）開始，預測將會在哪個時間點？選擇最有意義的一個。

答案：卡爾曼濾波器預測

問題18（另一個預測）：

考慮與之前相同的兩個速度。你期望最可信的預測在哪裡？

答案：另一個預測

卡爾曼濾波器關鍵

當你找到多個數據點時，你會發現在一維高斯（藍色）上的所有可能性都鏈接到二維高斯（紅色）。

摺疊第二個觀察t＝2，這與速度無關，只關乎於位置。

乘以先前（紅色高斯）和測量（綠色高斯），以獲得對物體速度和位置的真正好的估計。我們不會涵蓋涉及將多維高斯相乘的數學，但如果你想或諮詢補充材料以獲取有關該主題的更多信息，你可以自己嘗試。當我們做數學時，我們發現我們在x和y方向都有一個高峰分布。多麼方便！我們現在以很高的確定性知道我們的位置和速度！

到目前為止，我們只能觀察一個變數，並且使用該觀察結果來推斷另一個變數，使用一組方程式表示一個時間步長之後的位置是舊位置加上速度。

我們對我們的新位置 $x$ 的預測，將僅僅是我們先前的位置信度x和我們的運動u的總和。

$x$

卡爾曼濾波器的變數通常被稱為「狀態」，因為它們反映物理世界的狀態，如物體在何處以及移動的速度。在卡爾曼濾波器中有兩個子集：

觀測值瞬時位置
隱藏值 - 在我們的例子中是速度。我們不能直接測量它，只能從我們的位置測量中推斷出來。

當這兩個子集相互作用時，觀測值的後續觀測會給我們提供關於隱藏變數的信息，以便我們能夠估計隱藏變數是什麼。

從物體位置的多個觀測結果，我們可以發現物體移動的速度有多快。其他濾波器可以生成這些信息，但卡爾曼濾波器是最有效的。

卡爾曼濾波器設計

在設計卡爾曼濾波器時，你實際上需要兩個東西，一個狀態轉換函數和一個測量函數，它們均被表示為矩陣。我們稱之為狀態轉換矩陣F和測量矩陣H。

如果你對線性代數有點生疏，請隨時查閱可汗學院或麻省理工學院開放式課件的視頻來刷新你的理解。如果你有一點數學背景，MITOCW的鏈接可能會更有幫助，而如果你以前從未見過矩陣，可汗學院會是更好的。

我們將卡爾曼濾波器分為兩個步驟：更新步驟和預測步驟。這些步驟的方程以及矩陣和變數的名稱如下。你可能還想看看CS373課程卡爾曼濾波器矩陣的第一次迭代中的一個學生的優秀寫作。

這只是我們一直在討論的簡單的一維案例的泛化。如果你想了解更多有關真實卡爾曼濾波器的詳細信息，建議你查看麻省理工學院的Identification, Estimation, and Learning這門課程的講義。講義5-8與本次討論有關。

卡爾曼矩陣

這裡我們有一個具有挑戰性的編程問題。你將要編寫一個多維卡爾曼濾波器！你可以使用由Thrun教授實現的矩陣類來開始。這個類需要利用我們在前面問題中討論過的公式，以執行我們所需要的所有矩陣操作。

你可以在庫中找到一個小版本。使用如下命令創建矩陣：

a = matrix([[10.],[10]])

括弧中的參數是二維矩陣，在這種情況下，它是一個垂直向量。你可以使用show命令列印出垂直向量的結果：

a = matrix([[10.],[10]])a.show()

你可以計算轉置以找到水平向量：

a = matrix([[10.],[10]])b = a.transpose()a.show()

如果要將矩陣乘以一個向量：

a = matrix([[10.],[10]])F = matrix([[12., 8.], [6.,2.]])b = a.transpose()F.show()

你還可以乘以F和a，並將其設置為等於b，以顯示向量的結果：

a = matrix([[10.],[10]])F = matrix([[12., 8.],[6.,2.]])b = F * aF.show()

[200.0] 80.0]

下面是一個矩陣庫的樣子：

measurements = [1,2,3]x = matrix([[0.], [0.]]) # initial state (location and velocity)p = matrix([[1000., 0.], [0., 1000.]]) # initial uncertaintyu = matrix([[0.], [0.]]) # external motionF = matrix([[1., 1], [0,1.]]) # next state functionH = matrix([[1., 0]]) #measurement functionR = matrix([[1.]]) # measurement uncertaintyI = matrix([[1., 0.], [0., 1.]]) # identity matrixfilter(x, P)

當你使用給定的測量值運行過濾器時，你可以估計速度以進行更好的預測。

運行過濾器時，你希望首先返回測量更新，然後返回運動更新。

填寫這個空的過程，列印出x和P的結果估計值：

def filter(x, P): for n in range(len(measurements)): #measurement update #prediction print "x= " x.show() print "P= " P.show()

一旦你填寫它，你會得到一個輸出。該輸出在更新時迭代顯示x和P。注意，在x的第一個輸出處，我們對速度一無所知。這是因為我們需要多個位置測量來估計速度。隨著我們繼續進行測量和運動，我們的估計會收斂到正確的位置和速度。

問題19（卡爾曼矩陣）：

編寫一個輸出精確值的演算法濾波器（x，p），通過實現給定的方程並熟悉矩陣類。根據前一個視頻中多變數卡爾曼濾波器顯示的內容填寫該值。

答案：卡爾曼矩陣

這是Sebastian寫的代碼：

Z = matrix(measurements[n])y = Z - (H * x)S = H * P * H.transpose() + RK = P * H.transpose() * S.inverse()x = x + (K * y)P = (I - (K * H)) * P# predictionx = (F * x) + uP = F * P * F.transpose()

這段代碼不是平凡的！它是Google自動駕駛汽車跟蹤其他車輛的核心。這是你以前所見過的測量更新和預測的逐行實現。

首先，測量更新n。誤差計算 $(y = Z-(H * x))$ ，矩陣S，卡爾曼增益K與逆，然後回到下一個預測x和測量更新P。最後，你有一個預測步驟來實現你學到的方程式。

如果你能夠編寫這個程序，這真的真的表明你現在理解了卡爾曼濾波器，並且你已經使用為你編寫的相當機械的矩陣類，自己實現了一個多維卡爾曼濾波器。

運行該程序返回預測和速度更新。這些是剛剛實現的方程：

對於Google自動駕駛汽車，這是如何預測其他車輛的位置以及速度有多快的必要模型。汽車在前保險杠上使用雷達來測量與其他車輛的距離，並且還會對速度進行嘈雜的估計。此外，汽車使用其位於車頂的激光器來測量與其他車輛的距離，並獲得速度的嘈雜估計。

因此，當Google汽車行駛在路上時，它利用雷達和激光器使用卡爾曼濾波器來估計道路上其他車輛的距離和速度，其中輸入來自激光器的範圍數據，並使用相對距離x和y的狀態空間以及x和y的相對速度。

恭喜你，你已經完成了第2單元！！！