高级运动模型和扩展卡尔曼滤波 MATLAB实现

高级运动模型和扩展卡尔曼滤波 MATLAB实现

转载自

无人驾驶汽车系统入门（二）——高级运动模型和扩展卡尔曼滤波_AdamShan的博客-CSDN博客
修正了其中的几个矩阵错误

前言：上一篇文章的最后我们提到卡尔曼滤波存在着一个非常大的局限性——它仅能对线性的处理模型和测量模型进行精确的估计，在非线性的场景中并不能达到最优的估计效果。所以之前为了保证我们的处理模型是线性的，我们上一节中使用了恒定速度模型，然后将估计目标的加减速用处理噪声来表示，这一模型用来估算行人的状态其实已经足够了，但是在现实的驾驶环境中，我们不仅要估计行人，我们除了估计行人状态以外，我们还需要估计其他车辆，自行车等等状态，他们的状态估计显然不能使用简单的线性系统来描述，这里，我们介绍非线性系统情况下的一种广泛使用的滤波算法——扩展卡尔曼滤波(Extended Kalman Filter, EKF)。

本节主要讲解非线性系统中广泛使用的扩展卡尔曼滤波算法，我们通常将该算法应用于实际的车辆状态估计（或者说车辆追踪）中。另外，实际的车辆追踪运动模型显然不能使用简单的恒定速度模型来建模，在本节中会介绍几种应用于车辆追踪的高级运动模型。并且已经其中的CTRV模型来构造我们的扩展卡尔曼滤波。最后，在代码实例中，我会介绍如何使用EKF做多传感器融合。

应用于车辆追踪的高级运动模型

首先要明确的一点是，不管是什么运动模型，本质上都是为了帮助我们简化问题，所以我们可以根据运动模型的复杂程度（次数）来给我们常用的运动模型分一下类。

一次运动模型（也别称为线性运动模型）：

• 恒定速度模型（Constant Velocity, CV）
• 恒定加速度模型（Constant Acceleration, CA）

这些线性运动模型假定目标是直线运动的，并不考虑物体的转弯。

二次运动模型：

• 恒定转率和速度模型（Constant Turn Rate and Velocity，CTRV）
• 恒定转率和加速度模型（Constant Turn Rate and Acceleration，CTRA）

CTRV目前多用于机载追踪系统（飞机），这些二次运动模型大多假定速度$v$和 偏航角速度（yaw rate）$\omega$ 没有关系，因此，在这类运动模型中，由于偏航角速度测量的扰动（不稳定），即使车辆没有移动，我们的运动模型下的角速度也会发生细微的变化。

为了解决这个问题，速度$v$和 偏航角速度 $\omega$ 的关联可以通过设定转向角$\Phi$ 恒定来建立，这样就引出了 恒定转向角和速度模型（Constant Steering Angle and Velocity，CSAV）, 另外，速度可以别假定为线性变化的，进而引出了常曲率和加速度模型（Constant Curvature and Acceleration，CCA）。
这些运动模型的关系如图：

运动模型的状态转移公式

由于除CCA以外，以上的运动模型都非常著名，故本文不提供详细的推导过程。本文提供CV和CTRV模型的状态转移公式。

状态转移公式：就是我们的处理模型由上一状态的估计计算下一个状态的先验分布的计算公式，可以理解为我们基于一定的先验知识总结出来的运动公式。

1. CV模型

CV模型的状态空间可以表示为：
$$\overrightarrow{x}=(xy{v}_x{v}_y{)}^T$$
那么转移函数为：
$$\overrightarrow{x}(t+\Delta t)=\left(\begin{array}{c}x(t)+\Delta t{v}_x\\ y(t)+\Delta t{v}_y\\ v_x\\ v_y\end{array}\right)$$

2. CVRV模型

在CTRV中，目标的状态量为：
$$\overrightarrow{x}=(xyv\theta \omega {)}^T$$
其中，$\theta$ 为偏航角，是追踪的目标车辆在当前车辆坐标系下与x轴的夹角，逆时针方向为正，取值范围是$[0,2\pi )$, $\omega$是偏航角速度。CTRV的状态转移函数为：

$$\overrightarrow{x}(t+\Delta t)=\left(\begin{array}{c}\frac{v}{\omega }sin(\omega \Delta t+\theta )-\frac{v}{\omega }sin(\theta )+x(t)\\ -\frac{v}{\omega }cos(\omega \Delta t+\theta )+\frac{v}{\omega }cos(\theta )+y(t)\\ v\\ \omega \Delta t+\theta \\ \omega \end{array}\right)$$
本文下面的内容将以CTRV模型作为我们的运动模型。使用CTRV还存在一个问题，那就是$\omega =0$ 的情况，此时我们的状态转移函数公式中的 $(x,y)$ 将变成无穷大。为了解决这个问题，我们考察一下$\omega =0$ 的情况，此时我们追踪的车辆实际上是直线行驶的，所以我们的$(x,y)$ 的计算公式就变成了：

$$\begin{gathered} x(t+\Delta t)=vcos(\theta )\Delta t+x(t) \\ y(t+\Delta t)=vsin(\theta )\Delta t+y(t) \end{gathered}$$
那么现在问题来了，我们知道，卡尔曼滤波仅仅用于处理线性问题，那么很显然我们现在的处理模型是非线性的，这个时候我们就不能简单使用卡尔曼滤波进行预测和更新了，此时预测的第一步变成了如下非线性函数：
$$x_{k+1}=g(x_k,u)$$
其中，$g()$ 表示CTRV运动模型的状态转移函数，$u$ 表示处理噪声。为了解决非线性系统下的问题，我们引入扩展卡尔曼滤波（Extended Kalman Filter，EKF）

扩展卡尔曼滤波

雅可比矩阵

扩展卡尔曼滤波使用线性变换来近似非线性线性变换，具体来说，EKF使用一阶泰勒展式来进行线性化:
$$h(x)\approx h(u)+\frac{\partial h(u)}{\partial x}(x-u)$$

数学中，泰勒公式是一个用函数在某点的信息描述其附近取值的公式。如果函数足够平滑的话，在已知函数在某一点的各阶导数值的情况之下，泰勒公式可以用这些导数值做系数构建一个多项式来近似函数在这一点的邻域中的值。泰勒公式还给出了这个多项式和实际的函数值之间的偏差。

回到我们的处理模型中，我们的状态转移函数为：
$$\overrightarrow{x}(t+\Delta t)=g(x(t))=\left(\begin{array}{c}\frac{v}{\omega }sin(\omega \Delta t+\theta )-\frac{v}{\omega }sin(\theta )+x(t)\\ -\frac{v}{\omega }cos(\omega \Delta t+\theta )+\frac{v}{\omega }cos(\theta )+y(t)\\ v\\ \omega \Delta t+\theta \\ \omega \end{array}\right),\omega \ne 0$$

$$\overrightarrow{x}(t+\Delta t)=g(x(t))=\left(\begin{array}{c}vcos(\theta )\Delta t+x(t)\\ vsin(\theta )\Delta t+y(t)\\ v\\ \omega \Delta t+\theta \\ \omega \end{array}\right),\omega =0$$

那么，对于这个多元函数，我们需要使用多元泰勒级数：
$$T(x)=f(u)+(x-u)Df(u)+\frac{1}{2!}(x-u{)}^2{D}^{2}f(u)+...$$
其中，$Df(a)$叫雅可比矩阵，它是多元函数中各个因变量关于各个自变量的一阶偏导数构成的矩阵。

$$J=\left[\begin{array}{ccc}\frac{\partial f}{\partial x_1}& \cdots & \frac{\partial f}{\partial x_n}\end{array}\right]=\left[\begin{array}{ccc}\frac{\partial f}{\partial x_1}& \cdots & \frac{\partial f}{\partial x_n}\\ ⋮& \ddots & \cdots \\ \frac{\partial f_m}{\partial x_1}& \cdots & \frac{\partial f_m}{\partial x_n}\end{array}\right]$$

在向量微积分中，雅可比矩阵是一阶偏导数以一定方式排列成的矩阵，其行列式称为雅可比行列式。雅可比矩阵的重要性在于它体现了一个可微方程与给出点的最优线性逼近。因此，雅可比矩阵类似于多元函数的导数。

在扩展卡尔曼滤波中，由于$(x-u)$本身数值很小，那么$(x-u{)}^2$ 就更小了，所以更高阶的级数在此问题中忽略不计，我们只考虑到利用雅可比矩阵进行线性化。

那么接下来就是求解雅可比矩阵，在CTRV模型中，对各个元素求偏导数可以得到雅可比矩阵$(\omega \ne 0)$：

$$J_A=\left[\begin{array}{ccccc}1& 0& \frac{1}{\omega }(-sin(\theta )+sin(\omega \Delta t+\theta ))& \frac{v}{\omega }(cos(\omega \Delta t+\theta )-cos(\theta ))& \frac{\Delta tv}{\omega }cos(\omega \Delta t+\theta )-\frac{v}{{\omega }^2}(sin(\omega \Delta t+\theta )-sin(\theta ))\\ 0& 1& \frac{1}{\omega }(cos(\theta )-cos(\omega \Delta t+\theta ))& \frac{v}{\omega }(sin(\omega \Delta t+\theta )-sin(\theta ))& \frac{\Delta tv}{\omega }sin(\omega \Delta t+\theta )-\frac{v}{{\omega }^2}(cos(\theta )-cos(\omega \Delta t+\theta ))\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& \Delta t\\ 0& 0& 0& 0& 1\end{array}\right]$$
当 $\omega =0$时，雅可比矩阵为：

$$J_A=\left[\begin{array}{ccccc}1& 0& cos(\theta )\Delta t& -sin(\theta )\Delta tv& 0\\ 0& 1& sin(\theta )\Delta t& cos(\theta )\Delta tv& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& \Delta t\\ 0& 0& 0& 0& 1\end{array}\right]$$
在我们后面的MATLAB实现中，我们将直接计算雅可比矩阵，而不需要我们使用代码写这个雅可比矩阵。在得到我们CTRV模型的雅可比矩阵以后，我们的处理模型就可以写成：
$$\begin{gathered} x_{k+1}=g(x_k,u) \\ {P}_{k+1}=J_A{P}_k{J}_A^T+Q \end{gathered}$$

处理噪声

处理噪声模拟了运动模型中的扰动，我们引入运动模型的出发点就是要简化我们要处理的问题，这个简化是建立在多个假设的基础上（在CTRV中，这些假设就是恒定偏航角速度和速度），但是在现实问题中这些假设就会带来一定的误差，处理噪声实际上描述了当我们的系统在运行一个指定时间段以后可能面临多大的这样的误差。在CTRV模型中噪声的引入主要来源于两处：直线加速度和偏航角加速度噪声，我们假定直线加速度和偏航角加速度满足均值为 $0$，方差分别为${\sigma }_a^2$
,${\sigma }_{\omega }^2$的高斯分布，由于均值为$0$, 我们在状态转移公式中的$u$就可以不予考虑，我们来看噪声带来的不确定性$Q$，直线加速度和偏航角加速度将影响我们的状态量$(x,y,v,\theta ,\omega )$，这两个加速度量对我们的状态的影响的表达式如下：
$$noise\_term=\left[\begin{array}{c}\frac{1}{2}{\Delta t}^2{\mu }_{a}cos(\theta )\\ \frac{1}{2}{\Delta t}^2{\mu }_{a}sin(\theta )\\ \Delta t{\mu }_a\\ \frac{1}{2}\Delta t^2{\mu }_{\omega }\\ \Delta t{\mu }_{\omega }\end{array}\right]$$
其中${\mu }_a,{\mu }_{\omega }$为直线上和转角上的加速度（在这个模型中，我们把把它们看作处理噪声），我们分解这个矩阵：

$$noise\_term=\left[\begin{array}{cc}\frac{1}{2}{\Delta t}^{2}cos(\theta )& 0\\ \frac{1}{2}{\Delta t}^{2}sin(\theta )& 0\\ \Delta t& 0\\ 0& \frac{1}{2}\Delta t^2\\ 0& \Delta t\end{array}\right]\cdot \left[\begin{array}{c}{\mu }_a\\ {\mu }_{\omega }\end{array}\right]=G\cdot \mu$$
我们知道$Q$ 就是处理噪声的协方差矩阵，其表达式为：
$$Q=E[noise\_term\cdot noise\_ter{m}^T]=E[G\mu {\mu }^T{G}^T]=G\cdot E[\mu {\mu }^T]\cdot G^T$$
其中：
$$E[\mu {\mu }^T]=\left(\begin{array}{cc}{\sigma }_a^2& 0\\ 0& {\sigma }_{\omega }^2\end{array}\right)$$
所以，我们在CTRV模型中的处理噪声的协方差矩阵$Q$ 的计算公式就是：
$$Q=\left[\begin{array}{ccccc}(\frac{1}{2}\Delta t^2{\sigma }_a\cos (\theta ){)}^2& \frac{1}{4}\Delta t^4{\sigma }_a^2\sin (\theta )\cos (\theta )& \frac{1}{2}\Delta t^3{\sigma }_a^2\cos (\theta )& 0& 0\\ \frac{1}{4}\Delta t^4{\sigma }_a^2\sin (\theta )\cos (\theta )& (\frac{1}{2}\Delta t^2{\sigma }_a\sin (\theta ){)}^2& \frac{1}{2}\Delta t^3{\sigma }_a^2\sin (\theta )& 0& 0\\ \frac{1}{2}\Delta t^3{\sigma }_a^2\cos (\theta )& \frac{1}{2}\Delta t^3{\sigma }_a^2\sin (\theta )& \Delta t^2{\sigma }_a^2& 0& 0\\ 0& 0& 0& (\frac{1}{2}\Delta t^2{\sigma }_{\dot{\omega }}{)}^2& \frac{1}{2}\Delta t^3{\sigma }_{\dot{\omega }}^2\\ 0& 0& 0& \frac{1}{2}\Delta t^3{\sigma }_{\dot{\omega }}& \Delta t^2{\sigma }_{\dot{\omega }}\end{array}\right]$$

测量

假设我们有激光雷达和雷达两个传感器，它们分别以一定的频率来测量如下数据：

• 激光雷达：测量目标车辆的坐标 $(x,y)$ 。这里的x,y是相对于我们的车辆的坐标系的，即我们的车辆为坐标系的原点，我们的车头为x轴，车的左侧为y轴。
• 雷达：测量目标车辆在我们车辆坐标系下与本车的距离$rho$，目标车辆与x轴的夹角$\psi$，以及目标车辆与我们自己的相对距离变化率 $\dot{\rho }$ （本质上就是目标车辆的实际速度在我们和目标车辆的连线上的分量)

前面的卡尔曼滤波器中，我们使用一个测量矩阵$H$ 将预测的结果映射到测量空间，那是因为这个映射本身就是线性的，现在，我们使用雷达和激光雷达来测量目标车辆(我们把这个过程称为传感器融合)，这个时候会有两种情况，即：

1. 激光雷达的测量模型仍然是线性的，其测量矩阵为：
   $$H_L=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\end{array}\right]$$

将预测映射到激光雷达测量空间：
$$H_L\overrightarrow{x}=(x,y{)}^T$$

2. 雷达的预测映射到测量空间是非线性的，其表达式为：

$$\left(\begin{array}{c}\rho \\ \psi \\ \dot{\rho }\end{array}\right)=\left[\begin{array}{c}\sqrt{x^2+y^2}\\ atan2(y,x)\\ \frac{vcos(yaw)x+vsin(yaw)y}{\sqrt{x^2+y^2}}\end{array}\right]$$

此时我们使用 $h(x)$ 来表示这样一个非线性映射，那么在求解卡尔曼增益时我们也要将该非线性过程使用泰勒公式将其线性化，参照预测过程，我们也只要求解$h(x)$的雅可比矩阵：
$$J_H=\left[\begin{array}{ccccc}\frac{x}{\sqrt{x^2+y^2}}& \frac{y}{\sqrt{x^2+y^2}}& 0& 0& 0\\ -\frac{y}{x^2+y^2}& \frac{x}{x^2+y^2}& 0& 0& 0\\ \frac{vcos(yaw)}{\sqrt{x^2+y^2}}-\frac{x(vcos(yaw)x+vsin(yaw)y)}{(x^2+y^2{)}^{\frac{3}{2}}}& \frac{vsin(yaw)}{\sqrt{x^2+y^2}}-\frac{y(vcos(yaw)x+vsin(yaw)y)}{(x^2+y^2{)}^{\frac{3}{2}}}& \frac{cos(yaw)x+sin(yaw)y}{\sqrt{(}{x}^2+y^2)}& \frac{-v\ast x\ast sin(yaw)+v\ast y\ast cos(yaw)}{\sqrt{(}{x}^2+y^2)}& 0\end{array}\right]$$
虽然这个雅可比矩阵看似非常复杂，但是我们待会编程的时候并不需要完整的推导出这个雅可比矩阵的表达式，在本篇中，我们采用symbolic这个工具来直接求解雅可比矩阵。

综上，EKF的整个过程为：

MATLAB实现

和之前一样，为了实现交互式的代码，实例的代码为了便于理解，我们仍然使用大家熟悉的MATLAB来实现，当然，实际无人车项目肯定是需要用C来实现的，要将下面的示例代码使用C来改写是非常简单快速的。

首先我们读取我们的数据集，该数据集包含了追踪目标的LIDAR和RADAR的测量值，以及测量的时间点，同时为了验证我们追踪目标的精度，该数据还包含了追踪目标的真实坐标。（数据下载链接见文章末尾）

其中第一列表示测量数据来自LIDAR还是RADAR，LIDAR的2，3列表示测量的目标$(x,y)$,第4列表示测量的时间点，第5，6，7，8表示真实的$(x,y,v_x,v_y)$, RADAR测量的(前三列)是$(\rho ,\psi ,\dot{\rho })$，其余列的数据的意义和LIDAR一样。

首先我们读取整个数据：

# read the measurement data, use 0.0 to stand LIDAR data
# and 1.0 stand RADAR data
LidarDataset = [];
RadarDataset = [];
f = fopen('data_synthetic.txt');
Dataset = [];
while feof(f) == 0
    line = fgetl(f);
    infos = strsplit(line);
    % length(infos) 
    result = [];
    if cell2mat(infos(1))=='R'
      result = [1];
      for i = 2:length(infos)
        result = [result,str2num(cell2mat(infos(i)))];
      endfor
      RadarDataset = [RadarDataset;result];
      Dataset = [Dataset;result];
    else
      result = [0];
      for i = 2:length(infos)
        result = [result,str2num(cell2mat(infos(i)))];
      endfor
      LidarDataset = [LidarDataset;result];
      Dataset = [Dataset;[result,0]];
    endif
    
end
save('Data_Lidar_Radar.mat','Dataset');
fclose(f);

初始化$P$，激光雷达的测量矩阵（线性）$H_L$，测量噪声$R$, 处理噪声的中直线加速度项的标准差${\sigma }_a$ ，转角加速度项的标准差${\sigma }_{\dot{\omega }}$:

P = eye(5); 
H_lidar = [[ 1.,  0.,  0.,  0.,  0.];
       [ 0.,  1.,  0.,  0.,  0.]];
R_lidar = [[0.0225, 0.];[0., 0.0225]];
R_radar = [[0.09, 0., 0.];[0., 0.0009, 0.]; [0., 0., 0.09]];
% process noise standard deviation for a
std_noise_a = 2.0
% process noise standard deviation for yaw acceleration
std_noise_yaw_dd = 0.3

在整个预测和测量更新过程中，所有角度量的数值都应该控制在 $[-\pi ,\pi ]$，我们知道角度加减$2\pi$ 不变，所以用如下函数表示函数来调整角度：

function [psi] = control_psi(psi)
  while (psi >pi) || (psi <-pi)
    if psi >pi
      psi = psi-2*pi;
    endif
    if psi <-pi
      psi = psi+2*pi;
    endif
end

使用第一个雷达（或者激光雷达）的测量数据初始化我们的状态，对于激光雷达数据，可以直接将测量到的目标的$(x,y)$ 坐标作为初始$(x,y)$，其余状态项初始化为0，对于雷达数据，可以使用如下公式由测量的 $\rho ,\psi ,\dot{\rho }$得到目标的坐标$(x,y)$：

$$\begin{gathered} x=\rho \ast cos(\psi ) \\ y=\rho \ast sin(\psi ) \end{gathered}$$
具体状态初始化代码为：

state = zeros(5,1);

init_measurement = Dataset(1,:);

% states;
px = [];
py = [];
vx = [];
vy = [];
 gpx = [];
 gpy = [];
 gvx = [];
 gvy = []; 
 mx = [];
 my = [];

if init_measurement(1) == 0
  disp("Initialize with LIDAR measurement!");
  
  current_time = init_measurement(4);
  state(1) = init_measurement(2);
  state(2) = init_measurement(3);
else
  disp("Initialize with RADAR measurement!");
  init_measurement = RadarDataset(1,:);
  current_time = init_measurement(5);
  init_rho = init_measurement(2);
  init_psi = init_measurement(3);
  init_psi = control_psi(init_psi);
  state(1) = init_rho * cos(init_psi);
  state(2) = init_rho * sin(init_psi);
endif

写一个辅助函数用于保存数值：

function [px,py,vx,vy, gpx,gpy,gvx,gvy, mx,my] = savestates_ekf(px,py,vx,vy, gpx,gpy,gvx,gvy, mx,my,ss, gx, gy, gv1, gv2, m1, m2)
    px = [px;ss(1)];
    py = [py;ss(2)];
    vx = [vx;cos(ss(4)) * ss(3)];
    vy = [vy;sin(ss(4)) * ss(3)];

    gpx = [gpx;(gx)];
    gpy = [gpy;(gy)];
    gvx = [gvx;(gv1)];
    gvy = [gvy;(gv2)];
    mx = [mx;(m1)];
    my = [my;(m2)];
endfunction

定义状态转移函数和测量函数，使用sysbolic工具来计算其对应的雅可比矩阵，这里我们先设$\Delta t=0.05$，只是为了占一个位置，当实际运行EKF时会计算出前后两次测量的时间差，一次来替换这里的$\Delta t$。

function [state] = predict_omega(state,dt)
  % state = x,y,v,theta,omega
  state(1) = state(1) + (state(3) / state(5)) * (sin(state(4) + state(5) * dt) - sin(state(4)));
  state(2) = state(2) + (state(3) / state(5)) *(-cos(state(4) + state(5) * dt) + cos(state(4)));
  state(3) = state(3);
  state(4) = state(4) + state(5) * dt;
  state(5) = state(5);
end

function [state] = predict_omega(state,dt)
    % state = x,y,v,theta,omega
  state(1) = state(1) + state(3)*cos(state(4))*dt;
  state(2) = state(2) + state(3)*sin(state(4))*dt;
  state(3) = state(3);
  state(4) = state(4);
  state(5) = state(5);
end

function [JF] = jacobian_omega(state,dt)
JF = zeros(5,5);
%  state(1) = state(1) + (state(3) / state(5)) * (sin(state(4) + state(5) * dt) - sin(state(4)));
%  state(2) = state(2) + (state(3) / state(5)) *(-cos(state(4) + state(5) * dt) + cos(state(4)));
%  state(3) = state(3);
%  state(4) = state(4) + state(5) * dt;
%  state(5) = state(5);
% syms x y vel theta omega dt
% diff(stf,v)
x = state(1);
y = state(2);
vel = state(3);
theta = state(4);
omega = state(5);

% sft(x,y,v,theta,omega,dt) = x+ (vel / omega) * (sin(theta + omega * dt) - sin(theta));
% sft(x,y,v,theta,omega,dt) = y+ (vel / omega) * (-cos(theta + omega * dt) + cos(theta));


JF(1,:) = [1,...
           0,...
           1/omega*(-sin(theta) + sin(dt*omega + theta)),...
           vel/omega*(-cos(theta) + cos(dt*omega + theta)),...
           dt*vel/omega*cos(dt*omega + theta)-vel/(omega)^2*(-sin(theta) + sin(dt*omega + theta))...
           ];
JF(2,:) = [0,...
           1,...
           1/omega*( cos(theta) - cos(dt*omega + theta)),...
           vel/omega*(-sin(theta) + sin(dt*omega + theta)),...
           dt*vel/omega*sin(dt*omega + theta)- vel/(omega)^2*(cos(theta) - cos(dt*omega + theta))...
           ];
JF(3,:) = [ 0,0,1,0,0]; 
JF(4,:) = [ 0,0,0,1,dt];
JF(5,:) = [ 0,0,0,0,1];         
           
end

function [JF] = jacobian_no_omega(state,dt)
JF = zeros(5,5);

x = state(1);
y = state(2);
vel = state(3);
theta = state(4);
omega = state(5);
%  state(1) = state(1) + state(3)*cos(state(4))*dt;
%  state(2) = state(2) + state(3)*sin(state(4))*dt;
%  state(3) = state(3);
%  state(4) = state(4);
%  state(5) = state(5);
JF(1,:) = [1,...
           0,...
           dt*cos(theta),...
           -dt*vel*sin(theta),...
           0];
JF(2,:) = [0,...
           1,...
           dt*sin(theta),...
           dt*vel*cos(theta),...
           0];
JF(3,:) = [ 0,0,1,0,0]; 
JF(4,:) = [ 0,0,0,1,dt];
JF(5,:) = [ 0,0,0,0,1]; 
end

function [map_pred] = measurement_func(state)
    x = state(1);
  y = state(2);
  vel = state(3);
  theta = state(4);
  omega = state(5);
  map_pred = zeros(3,1);
  map_pred(1) = sqrt(x^2 + y^2);
  map_pred(2) = atan2(y, x);
  map_pred(3) = (x * vel * cos(theta) + y * vel * sin(theta)) / sqrt(x^2 + y^2);
end

function [JH] = jacobian_measurement(state)
  x = state(1);
  y = state(2);
  vel = state(3);
  theta = state(4);
  omega = state(5);
  JH = zeros(3,5);
  
  map_pred(1) = sqrt(x^2 + y^2);
  map_pred(2) = atan2(y, x);
  map_pred(3) = (x * vel * cos(theta) + y * vel * sin(theta)) / sqrt(x^2 + y^2);
  
%  syms x y vel theta omega
%  frng(x,y,vel,theta,omega) = sqrt(x^2 + y^2);
%  fazi(x,y,vel,theta,omega)  = atan2(y, x);
%  fvel(x,y,vel,theta,omega)  = (x * vel * cos(theta) + y * vel * sin(theta)) / sqrt(x^2 + y^2);
JH(1,:) = [...
           x/(sqrt(x^2+y^2)),...
           y/(sqrt(x^2+y^2)),...
           0,...
           0,...
           0];
JH(2,:) = [...
           -y/(x^2+y^2),...
           x/(x^2+y^2),...
           0,...
           0,...
           0];
JH(3,:) = [...
           vel*cos(theta)/(sqrt(x^2+y^2))- x*(vel*x*cos(theta) + vel*y*sin(theta))/(x^2+y^2)^(3/2),...
           vel*sin(theta)/(sqrt(x^2+y^2))- y*(vel*x*cos(theta) + vel*y*sin(theta))/(x^2+y^2)^(3/2),...
           (x*cos(theta) + y*sin(theta))/(sqrt(x^2+y^2)),...
           (-vel*x*sin(theta) + vel*y*cos(theta))/(sqrt(x^2+y^2)),...
           0];
  
end

EKF的过程代码：

measurement_step = length(Dataset);
dt = 0.05;

I = eye(5);
G = zeros(5,2);
for i=2:measurement_step
  % prediction
  t_measurement = Dataset(i,:);
  if t_measurement(1) == 0
        % lidar
        m_x = t_measurement(2);
        m_y = t_measurement(3);
        z = [m_x;m_y];
        
        dt = (t_measurement(4)-current_time)/1000000;
        current_time = t_measurement(4);
        
        % true position
        g_x = t_measurement(5);
        g_y = t_measurement(6);
        g_v_x = t_measurement(7);
        g_v_y  = t_measurement(8);
  else
        % radar
        m_rho = t_measurement(2);
        m_psi = t_measurement(3);
        m_dpl = t_measurement(4);
        
        z = [m_rho;m_psi;m_dpl];
        
        dt = (t_measurement(5)-current_time)/1000000;
        current_time = t_measurement(5);
        
        % true position
        g_x = t_measurement(6);
        g_y = t_measurement(7);
        g_v_x = t_measurement(8);
        g_v_y  = t_measurement(9);      
  end
  if abs(state(5))<0.0001 % omega==0,driving is straight
      state = predict_no_omega(state,dt);
      state(4) = control_psi(state(4));
      JA= jacobian_no_omega(state,dt);
  else
      state = predict_omega(state,dt);
      state(4) = control_psi(state(4));
      JA = jacobian_omega(state,dt);
  end
  G(1,1) = 0.5*dt*dt*cos(state(4));
  G(2,1) = 0.5*dt*dt*sin(state(4));
  G(3,1) = dt;
  G(4,2) = 0.5*dt*dt;
  G(5,2) = dt;
  Q_v = diag([std_noise_a^2,std_noise_yaw_dd^2]);
  Q = G*Q_v*G';
  
  % Project the error covariance ahead
   P = JA*P*JA'+Q;
   
   % measurement update(correction)
if t_measurement(1) == 0
    %liadar
    S = H_lidar*P*H_lidar'+R_lidar;
    K = P*H_lidar'*(S)^-1;

    y = z-H_lidar*state;
    state = state + K*y;
    state(4) = control_psi(state(4));
    %Update the error covariance
    P = (I-K*H_lidar)*P;
    [px,py,vx,vy, gpx,gpy,gvx,gvy, mx,my] = savestates_ekf(px,py,vx,vy, gpx,gpy,gvx,gvy, mx,my,state, g_x, g_y, g_v_x, g_v_y, m_x, m_y);
else
    % radar
    JH = jacobian_measurent(state);
    
    S = JH*P*JH'+R_radar;
    K = P*JH'*(S)^-1;
    map_pred = measurement_func(state);
   if abs(map_pred(1))<0.0001 % rho is 0
       map_pred(3) = 0;
   end
   y = z-map_pred;
   y(2)= control_psi(y(2)); % 防止翻转
   state = state + K*y;
   state(4) = control_psi(state(4));
   %Update the error covariance
   P = (I-K*JH)*P;
   [px,py,vx,vy, gpx,gpy,gvx,gvy, mx,my] = savestates_ekf(px,py,vx,vy, gpx,gpy,gvx,gvy, mx,my,state, g_x, g_y, g_v_x, g_v_y, m_x, m_y);
end
end

sqrt(mean((px-gpx).^2)) 
sqrt(mean((py-gpy).^2)) 
sqrt(mean((vx-gvx).^2)) 
sqrt(mean((vy-gvy).^2)) 

plot(px,py,'.r');
hold on;
plot(gpx,gpy,'-k');
hold on;
plot(mx,my,'.g');

最后我们来看一下我们的EKF在追踪目标的时候的均方误差：

ans = 0.073634
ans = 0.080460
ans = 0.2292
ans = 0.3100

把EKF的估计结果可视化：

其中，红色的点为我们的EKF对目标位置的估计，绿色的点为来自LIDAR和RADAR的测量值，黑色的点是目标的真实位置，由此可知，我们的测量是不准确的，因此我们使用EKF在结合运动模型的先验知识以及融合两个传感器的测量的基础上做出了非常接近目标正是状态的估计。

EKF的魅力其实还不止在此！我们使用EKF，不仅能够对目标的状态（我们关心的）进行准确的估计，从这个例子我们会发现，EKF还能估计我们的传感器无法测量的量（比如本例中的$v,\psi ,\dot{\psi }$ ）

附数据下载链接：https://download.csdn.net/download/west_gege/44324956