【目标融合】基于拓展卡尔曼滤波实现车载激光雷达和雷达的数据融合matlab源码

一、简介

扩展卡尔曼滤波器理论

如下图所示。扩展卡尔曼滤波的理论和编程依旧需要使用到这些公式，相比于原生的卡尔曼滤波，只在个别地方有所不同。

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; ```

三、结果

​