基于MATLAB&SIMULINK開發(fā)自動駕駛系統(tǒng)第四講之線性卡爾曼濾波

卡爾曼濾波是目前行業(yè)中做感知算法和感知融合算法必用的算法，因此小明師兄和大家一起來學習一下。

當使用卡爾曼濾波器跟蹤物體時，會使用一連串的探測量或測量量來構(gòu)建物體運動的模型。物體運動是由物體狀態(tài)的變化來定義的?？柭鼮V波器是一種最佳的、遞歸的算法，用于估計物體的運動跟蹤。濾波器之所以是遞歸的，是因為它使用之前的狀態(tài)，使用可能在該區(qū)間內(nèi)進行的測量來更新當前狀態(tài)?？柭鼮V波器將這些新的測量值納入其中，以保持狀態(tài)估計的盡可能準確。該濾波器是最佳的，因為它使狀態(tài)的均方誤差最小化?？梢允褂迷摓V波器來預測未來的狀態(tài)或估計當前狀態(tài)或過去的狀態(tài)。

4．1 狀態(tài)方程

對于自動駕駛工具箱中跟蹤的大多數(shù)類型的物體，狀態(tài)向量由一維、二維或三維的位置和速度組成。

從物體在x方向以恒定加速度運動的牛頓方程開始，將這些方程轉(zhuǎn)換為空間狀態(tài)形式：

如果定義狀態(tài)作為：

可以將牛頓定律寫成如下狀態(tài)方程：

當對物體遵循這種類型的運動有信心時，會使用線性動態(tài)模型。有時，模型包括過程噪聲，以反映運動模型的不確定性。在這種情況下，牛頓方程有一個附加項：

vk是加速度的未知噪聲擾動。只有噪聲的統(tǒng)計量是已知的。假設(shè)它是零均值的高斯白噪聲。

可以將這種類型的方程擴展到一個以上的維度。在二維中，方程的形式為：

右側(cè)的4×4矩陣是狀態(tài)轉(zhuǎn)換模型矩陣。對于獨立的x和y運動，這個矩陣是塊對角線。

當過渡到離散時間時，會在時間間隔的長度上整合運動方程。在離散形式中，對于T的樣本區(qū)間，狀態(tài)表示變成了：

量xk＋1是離散時間k＋1時的狀態(tài)，xk是更早的離散時間k時的狀態(tài)。如果加入噪聲，方程就會變得更復雜，因為噪聲的積分并不簡單。

狀態(tài)方程可以概括為：

Fk是狀態(tài)轉(zhuǎn)換矩陣，Gk是控制矩陣?？刂凭仃嚳紤]了作用在物體上的任何已知力。這兩個矩陣均已給出。最后一個項代表動態(tài)模型的類似噪聲的隨機擾動。噪聲被假定為零均值高斯白噪聲。

有輸入噪聲的連續(xù)時間系統(tǒng)由線性隨機微分方程描述。有輸入噪聲的離散時間系統(tǒng)用線性隨機微分方程描述。狀態(tài)空間表示法是一個物理系統(tǒng)的數(shù)學模型，其中輸入、輸出和狀態(tài)變量由一階耦合方程相關(guān)。

4．2 測量模型

測量是對系統(tǒng)的觀察。測量值取決于狀態(tài)矢量，但并不總是與狀態(tài)矢量相同。例如，在雷達系統(tǒng)中，測量值可以是球面坐標，如距離、方位角和海拔，而狀態(tài)矢量是笛卡爾坐標的位置和速度。對于線性卡爾曼濾波器，測量值總是狀態(tài)矢量的線性函數(shù)，排除了球面坐標。要使用球面坐標，需要使用擴展卡爾曼濾波器。

測量模型假設(shè)任何時候的實際測量值都與當前狀態(tài)有關(guān)，其方法是：

wk代表當前時間步的測量噪聲。測量噪聲也是零均值白高斯噪聲，其協(xié)方差矩陣Q由以下方式描述：

4．3 線性卡爾曼濾波等式

如果沒有噪聲，則卡爾曼濾波的等式應(yīng)該為：

同樣，測量模型也沒有測量噪聲貢獻。在每個實例中，過程和測量噪聲都是不知道的。只有噪聲統(tǒng)計數(shù)據(jù)是已知的。噪聲統(tǒng)計數(shù)據(jù)是已知的。

可以把這些方程放到一個遞歸循環(huán)中，來估計狀態(tài)如何演化，也可以估計狀態(tài)分量的不確定性如何演化。

4．4 濾波環(huán)

從狀態(tài)的最佳估計值x0／0和狀態(tài)協(xié)方差P0／0開始，濾波器在不斷的循環(huán)中執(zhí)行這些步驟。

1 使用運動方程將狀態(tài)傳播到下一步

將協(xié)方差矩陣也傳播出去。

下標符號k＋1｜k表示該量是在k＋1步時從k步開始傳播的最優(yōu)估計，這個估計通常被稱為先驗估計。

然后預測更新時間的測量結(jié)果。

2 利用實際測量和預測測量的差值來修正更新時間的狀態(tài)。修正需要計算卡爾曼增益。要做到這一點，首先要計算測量預測協(xié)方差（創(chuàng)新）。

然后卡爾曼濾波的增益為：

并由使用最優(yōu)條件得出。

3 用測量值修正預測估計值。假設(shè)估計值是預測狀態(tài)與測量值的線性組合。修正后的估計值使用下標符號，k＋1｜k＋1．是由以下內(nèi)容計算出來的

其中Kk＋1為卡爾曼增益。修正后的狀態(tài)通常被稱為狀態(tài)的后驗估計，因為它是在包含測量之后得出的。

修正狀態(tài)協(xié)方差矩陣

最后，可以根據(jù)修正后的狀態(tài)來計算測量值。這不是對測量值的修正，而是對測量值的最佳估計，是基于狀態(tài)的最佳估計。將其與實際測量值進行比較，就可以了解濾波器的性能。

此圖總結(jié)了卡爾曼環(huán)的操作：

4．5 恒速模型

線性卡爾曼濾波器包含一個內(nèi)置的線性恒速運動模型。另外，也可以指定線性運動的過渡矩陣。下一個時間步的狀態(tài)更新是當前時間狀態(tài)的線性函數(shù)。在這個濾波器中，測量值也是測量矩陣所描述的狀態(tài)的線性函數(shù)。對于一個在三維空間中運動的物體，狀態(tài)由x、y、z坐標中的位置和速度來描述。恒速運動的狀態(tài)轉(zhuǎn)換模型為

測量模型是狀態(tài)向量的線性函數(shù)。最簡單的情況是，測量值是狀態(tài)的位置分量。

trackingKF 函數(shù)實例：

創(chuàng)建一個使用二維恒速運動模型的線性卡爾曼濾波器。假設(shè)測量由物體的X－Y位置組成。指定初始狀態(tài)估計的速度為零。x ＝ 5．3；
y ＝ 3．6；
initialState ＝ ［x；0；y；0］；
KF ＝ trackingKF（＇MotionModel＇，＇2D Constant Velocity＇，＇State＇，initialState）；

從恒定速度的軌跡中創(chuàng)建測量的位置：

vx ＝ 0．2；
vy ＝ 0．1；
T ＝ 0．5；

pos ＝ ［0：vx＊T：2；5：vy＊T：6］＇；

預測并糾正物體的狀態(tài)：

for k ＝ 1：size（pos，1）
pstates（k，：） ＝ predict（KF，T）；
cstates（k，：） ＝ correct（KF，pos（k，：））；
end

繪制跟蹤曲線圖：

plot（pos（：，1），pos（：，2），＇k．＇， pstates（：，1），pstates（：，3），＇＋＇， ．．．
cstates（：，1），cstates（：，3），＇o＇）
xlabel（＇x ［m］＇）
ylabel（＇y ［m］＇）
grid
xt ＝ ［x－2 pos（1，1）＋0．1 pos（end，1）＋0．1］；
yt ＝ ［y pos（1，2） pos（end，2）］；
text（xt，yt，｛＇First measurement＇，＇First position＇，＇Last position＇｝）
legend（＇Object position＇， ＇Predicted position＇， ＇Corrected position＇）

4．6 恒加速模型

線性卡爾曼濾波器包含一個內(nèi)置的線性恒加速運動模型。另外，也可以指定恒加速度線性運動的過渡矩陣。線性加速度的過渡模型是

最簡單的情況是，測量的是狀態(tài)的位置分量。

原文標題:基于MATLAB&SIMUlink開發(fā)自動駕駛系統(tǒng)第四講之線性卡爾曼濾波