基于MATLAB&SIMULINK開發(fā)自動(dòng)駕駛系統(tǒng)第五講之?dāng)U展卡爾曼濾波

當(dāng)物體運(yùn)動(dòng)遵循非線性狀態(tài)方程或測(cè)量值是狀態(tài)的非線性函數(shù)時(shí)，使用擴(kuò)展卡爾曼濾波器。一個(gè)簡單的例子是，當(dāng)物體的狀態(tài)或測(cè)量值是以球面坐標(biāo)計(jì)算的，如方位角、海拔和范圍。

5．1狀態(tài)更新模型

擴(kuò)展的卡爾曼濾波公式將狀態(tài)方程線性化。更新后的狀態(tài)和協(xié)方差矩陣仍然是之前狀態(tài)和協(xié)方差矩陣的線性函數(shù)。然而，線性卡爾曼濾波器中的狀態(tài)轉(zhuǎn)換矩陣被狀態(tài)方程的雅各布矩陣所取代。 雅各布矩陣不是恒定的，而是可以取決于狀態(tài)本身和時(shí)間。要使用擴(kuò)展卡爾曼濾波器，必須同時(shí)指定狀態(tài)轉(zhuǎn)換函數(shù)和狀態(tài)轉(zhuǎn)換函數(shù)的Jacobian。

假設(shè)有一個(gè)預(yù)測(cè)狀態(tài)的閉式表達(dá)式，作為之前狀態(tài)、控制、噪聲和時(shí)間的函數(shù)：

預(yù)測(cè)狀態(tài)相對(duì)于前一狀態(tài)的Jacobian為：

預(yù)測(cè)狀態(tài)相對(duì)于噪聲的Jacobian為：

當(dāng)噪聲線性地進(jìn)入狀態(tài)更新方程時(shí)，這些函數(shù)采取更簡單的形式。

在此狀態(tài)下，

．

5．2 測(cè)量模型

在擴(kuò)展卡爾曼濾波器中，測(cè)量可以是狀態(tài)和測(cè)量噪聲的非線性函數(shù)。

相對(duì)于狀態(tài)的測(cè)量的雅各布值為

相對(duì)于測(cè)量噪聲，測(cè)量的雅各布值為

當(dāng)噪聲以線性方式進(jìn)入測(cè)量方程時(shí)，這些函數(shù)采取更簡單的形式。

此時(shí) ，

5．3 擴(kuò)展卡爾曼濾波環(huán)路

這種擴(kuò)展卡爾曼濾波環(huán)路與線性卡爾曼濾波環(huán)路幾乎相同，只是。

－ 盡可能使用精確的非線性狀態(tài)更新和測(cè)量函數(shù)狀態(tài)轉(zhuǎn)換矩陣被狀態(tài)雅各布取代

－ 測(cè)量矩陣由相應(yīng)的雅各布代替。

5．4 預(yù)定義的擴(kuò)展卡爾曼濾波函數(shù)

自動(dòng)駕駛工具箱提供了預(yù)定義的狀態(tài)更新和測(cè)量功能，用于擴(kuò)展卡爾曼濾波器。

運(yùn)動(dòng)模型

函數(shù)名字

函數(shù)目的

Constant velocity

constvel

恒速狀態(tài)更新模型

constveljac

恒速狀態(tài)更新雅各布函數(shù)

cvmeas

恒速測(cè)量模型

cvmeasjac

定速測(cè)量Jacobian函數(shù)

Constant acceleration

constacc

恒加速狀態(tài)更新模型

constaccjac

恒加速狀態(tài)更新雅各布函數(shù)

cameas

恒加速度測(cè)量模型

cameasjac

恒定加速度測(cè)量Jacobian函數(shù)

Constant turn rate

constturn

恒定轉(zhuǎn)率狀態(tài)更新模型

constturnjac

恒定轉(zhuǎn)速狀態(tài)更新雅各布函數(shù)

ctmeas

恒定轉(zhuǎn)速測(cè)量模型

ctmeasjac

恒速測(cè)量雅各布函數(shù)

trackingKF 函數(shù)實(shí)例：

創(chuàng)建一個(gè)二維追蹤EKF對(duì)象，并使用名－值對(duì)來定義StateTransitionJacobianFcn和MeasurementJacobianFcn屬性。使用預(yù)定義的恒速運(yùn)動(dòng)和測(cè)量模型以及它們的雅各布系數(shù)。

EKF ＝ trackingEKF（＠constvel，＠cvmeas，［0；0；0；0］， ．．．
＇StateTransitionJacobianFcn＇，＠constveljac， ．．．
＇MeasurementJacobianFcn＇，＠cvmeasjac）；

運(yùn)行過濾器。使用預(yù)測(cè)和糾正函數(shù)來傳播狀態(tài)。你可以按任何順序調(diào)用預(yù)測(cè)和糾正，次數(shù)不限。在笛卡爾坐標(biāo)中指定測(cè)量。

measurement ＝ ［1；1；0］；
［xpred， Ppred］ ＝ predict（EKF）；
［xcorr， Pcorr］ ＝ correct（EKF，measurement）；
［xpred， Ppred］ ＝ predict（EKF）；

［xpred， Ppred］ ＝ predict（EKF）

xpred ＝ 4×1
1．2500
0．2500
1．2500
0．2500
Ppred ＝ 4×4
11．7500 4．7500 0 0
4．7500 3．7500 0 0
0 0 11．7500 4．7500
0 0 4．7500 3．7500

原文標(biāo)題:基于MATLAB&SIMUlink開發(fā)自動(dòng)駕駛系統(tǒng)第五講之?dāng)U展卡爾曼濾波