一、簡介

擴展卡爾曼濾波器理論

如下圖所示。擴展卡爾曼濾波的理論和編程依舊需要使用到這些公式，相比于原生的卡爾曼濾波，只在個別地方有所不同。

1、代碼：初始化(Initialization)?

擴展卡爾曼濾波的初始化，需要將各個變量進行設置，對于不同的運動模型，狀態向量是不一樣的。為了保證代碼對不同狀態向量的兼容性，我們使用Eigen庫中非定長的數據結構。

如下所示，我們新建了一個ExtendedKalmanFilter類，定義了一個叫做x_的狀態向量(state vector)。代碼中的VerctorXd表示X維的列向量，元素的數據類型為double。

初始化擴展卡爾曼濾波器時需要輸入一個初始的狀態量x_in，用以表示障礙物最初的位置和速度信息，一般直接使用第一次的測量結果。?

2、代碼：預測(Prediction)?

完成初始化后，我們開始寫Prediction部分的代碼。首先是公式

這里的x為狀態向量，通過左乘一個矩陣F，再加上外部的影響u，得到預測的狀態向量x'。這里的F被稱作狀態轉移矩陣(state transistion matrix)。以2維的勻速運動為例，這里的x為

根據中學物理課本中的公式s1 = s0 + vt，經過時間△t后的預測狀態向量應該是

對于二維的勻速運動模型，加速度為0，并不會對預測后的狀態造成影響，因此

如果換成加速或減速運動模型，可以引入加速度ax和ay，根據s1 = s0 + vt + at^2/2這里的u會變成：

作為入門，這里不討論太復雜的模型，因此公式

最終將寫成

再看預測模塊的第二個公式

該公式中P表示系統的不確定程度，這個不確定程度，在卡爾曼濾波器初始化時會很大，隨著越來越多的數據注入濾波器中，不確定程度會變小，P的專業術語叫狀態協方差矩陣(state covariance matrix)；這里的Q表示過程噪聲(process covariance matrix)，即無法用x'=Fx+u表示的噪聲，比如車輛運動時突然到了上坡，這個影響是無法用之前的狀態轉移方程估計的。

毫米波雷達測量障礙物在徑向上的位置和速度相對準確，不確定度較低，因此可以對狀態協方差矩陣P進行如下初始化：

由于Q對整個系統存在影響，但又不能太確定對系統的影響有多大。對于簡單的模型來說，這里可以直接使用單位矩陣或空值進行計算，即

根據以上內容和公式

?代碼：觀測(Measurement)?

觀測的第一個公式是

這個公式的目的是計算觀測到的觀測值z與預測值x'之間差值y。?

前面提到過毫米波雷達觀測值z的數據特性，如下圖所示：

?

由圖可知觀測值z的數據維度為3×1，為了能夠實現矩陣運算，y和Hx'的數據維度也都為3×1。?

使用基本的數學運算可以完成預測值x'從笛卡爾坐標系到極坐標系的坐標轉換，求得Hx'，再與觀測值z進行計算。需要注意的是，在計算差值y的這一步中，我們通過坐標轉換的方式避開了未知的旋轉矩陣H的計算，直接得到了Hx’的值。?(ps：Hx’就是傳感器坐標系下的預測值坐標)

為了簡化表達，我們用px，py以及vx和vy表示預測后的位置及速度，如下所示：

其中測量值z和預測后的位置x'都是已知量，因此我們很容易計算得到觀測與預測的差值y。?

毫米波雷達在轉換時涉及到笛卡爾坐標系和極坐標系的位置、速度轉換，這個轉化過程是非線性的。因而在處理類似毫米波雷達這種非線性的模型時，習慣于將計算差值y的公式寫成如下，以作線性和非線性模型的區分。

?對應上面的式子，這里的h(x')為：

?再看卡爾曼濾波器接下來的兩個公式

這兩個公式求的是卡爾曼濾波器中一個很重要的量——卡爾曼增益K(Kalman Gain)，用人話講就是求差值y的權值。第一個公式中的R是測量噪聲矩陣(measurement covariance matrix)，這個表示的是測量值與真值之間的差值。一般情況下，傳感器的廠家會提供。如果廠家未提供，我們也可以通過測試和調試得到。S只是為了簡化公式，寫的一個臨時變量，不用太在意。?

由于求得卡爾曼增益K需要使用到測量矩陣H，因此接下來的任務就是得到H。?

毫米波雷達觀測z是包含位置、角度和徑向速度的3x1的列向量，狀態向量x'是包含位置和速度信息的4x1的列向量，根據公式y=z-Hx'可知測量矩陣(Measurement Matrix)H的維度是3行4列。即：

從上面的公式很容易看出，等式兩邊的轉化是非線性的，并不存在一個常數矩陣H，能夠使得等式兩邊成立。

如果將高斯分布作為輸入，輸入到一個非線性函數中，得到的結果將不再符合高斯分布，也就將導致卡爾曼濾波器的公式不再適用。因此我們需要將上面的非線性函數轉化為近似的線性函數求解。

在大學課程《高等數學》中我們學過，非線性函數y=h(x)可通過泰勒公式在點(x0,y0)處展開為泰勒級數：

忽略二次以上的高階項，即可得到近似的線性化方程，用以替代非線性函數h(x)，即：

將非線性函數h(x)拓展到多維，即求各個變量的偏導數，即：

對x求偏導數所對應的這一項被稱為雅可比(Jacobian)式。

我們將求偏導數的公式與我們的之前推導的公式對應起來看x的系數，會發現這里的測量矩陣H其實就是泰勒公式中的雅可比式。

多維的雅可比式的推導過程有興趣的同學可以自己研究一下，這里我們直接使用其結論：

求得非線性函數h(x')對px，py，vx，vy的一階偏導數，并排列成的矩陣，最終得到雅克比(Jacobian)矩陣H：

其中

接下來就是考驗各位高等數學求偏導數功底的時候了！

經過一系列計算，最終得到測量矩陣H為：

?根據以上公式可知，在每次預測障礙物的狀態后，需要根據預測的位置和速度計算出對應的測量矩陣H，這個測量矩陣為非線性函數h(x')在x'所在位置進行求導的結果。

再看卡爾曼濾波器的最后兩個公式

這兩個公式，實際上完成了卡爾曼濾波器的閉環，第一個公式是完成了當前狀態向量x的更新，不僅考慮了上一時刻的預測值，也考慮了測量值，和整個系統的噪聲，第二個公式根據卡爾曼增益K，更新了系統的不確定度P，用于下一個周期的運算，該公式中的I為與狀態向量同維度的單位矩陣。

二、部分代碼

``` clear; clf; dt = 0.1; Data = csvread('RadarLidarData1.csv',1,1); % Data = csvread('RadarLidarData2.csv',1,1); RadarMeasurement = []; LidarMeasurement = []; EKF_Path = [];

F = [[1, 0, dt, 0]; [0, 1, 0, dt]; [0, 0, 1, 0]; [0, 0, 0, 1]];

u = 0; B = [(dt^2)/2 (dt^2)/2 dt dt]';

P = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1000, 0]; [0, 0, 0, 1000]];

R_l = [[0.0025, 0]; [0, 0.0025]];

R_r = [[0.09, 0, 0]; [0, 0.005, 0]; [0, 0, 0.09]];

Q = [(dt^2)/4 0 (dt^3)/2 0; 0 (dt^2)/4 0 (dt^3)/2; (dt^3/2) 0 (dt^2) 0; 0 (dt^3)/2 0 (dt^2)];

H = [[1, 0, 0, 0]; [0, 1, 0, 0]];

I = eye(4);

if (Data(1,1) == 1) x = [Data(1,2); Data(1,3); 0; 0]; else x = [Data(1,2); Data(1,3); Data(1,4); 0]; end

for i = 1:length(RadarMeasurement) RadarMeasurementCart(i,:) = [[RadarMeasurement(i,1),0];[0, RadarMeasurement(i,1)]]*[cos(RadarMeasurement(i,2));sin(Radar_Measurement(i,2))]; end

hold on;

plot(Data(:,6),Data(:,7),'linewidth', 2); scatter(EKFPath(:,1),EKFPath(:,2),25,'filled','r'); scatter(LidarMeasurement(:,1),LidarMeasurement(:,2),5,'filled','blue'); scatter(RadarMeasurementCart(:,1),RadarMeasurementCart(:,2),5,'filled','g');

legend('Grundtruth','EKF Path result','Lidar Measurement','Radar Measurement','Location','northwest'); axis square; hold off; ```

三、結果

?

總結

以上是生活随笔為你收集整理的【目标融合】基于拓展卡尔曼滤波实现车载激光雷达和雷达的数据融合matlab源码的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。