手把手教用matlab做无人驾驶（七）-扩展Kalman滤波（extended_kalman_filter）

对于Kalman滤波在我看来就是五个公式，kalman的计算过程在我的前面博客里面也给出了，不了解kalman的可以看看我前面的博客，这里讲解扩展卡尔曼滤波（extended_kalman_filter）。

为什么要用EKF

KF的假设之一就是高斯分布的xx预测后仍服从高斯分布，高斯分布的xx变换到测量空间后仍服从高斯分布。可是，假如F、H是非线性变换，那么上述条件则不成立。

将非线性系统线性化

既然非线性系统不行，那么很自然的解决思路就是将非线性系统线性化。

对于一维系统，采用泰勒一阶展开即可得到：

 

f(x)≈f(μ)+∂f(μ)∂x(x−μ)f(x)≈f(μ)+∂f(μ)∂x(x−μ)

 

对于多维系统，仍旧采用泰勒一阶展开即可得到：

 

T(x)≈f(a)+(x−a)TDf(a)T(x)≈f(a)+(x−a)TDf(a)

 

其中，Df(a)Df(a)是Jacobian矩阵。

这里F是雅可比矩阵，下面给出一个例子：

motion model
	x_{t+1} = x_t+v*dt*cos(yaw)
	y_{t+1} = y_t+v*dt*sin(yaw)
	yaw_{t+1} = yaw_t+omega*dt
	v_{t+1} = v{t}

状态变量为[x y yaw v]'

根据上面motion model，可以在matlab 中定义一个motion_model函数，如下：

function x= motion_model(x, u)

global DT;

F=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,0];
B=[DT*cos(x(3,1)),0;DT*sin(x(3,1)),0;0,DT;1,0];
x=F*x+B*u;

matlab 主程序main：

%  edit:Robert.cao
%   2018.9.27
fprintf ("start extended_kalman_filter")
clc;
clear;
close all;


global DT;
global Qsim;
global Rsim;
global Q;
global R;

Q=[0.1 0 0 0;0 0.1 0 0;0 0 1*3.1415926/180 0;0 0 0 0.1]^2;
R=[1 0;0 40*3.1415926/180]^2;
    

Qsim=(0.5*eye(2))^2;
Rsim = [1 0;0 30*3.141592654/180]^2;

SIM_TIME=50;
DT=0.1;
time=0;


xEst=zeros(4,1);%估计路线
xTrue=zeros(4,1);%真实路线
PEst=eye(4);  % P矩阵
xDR=zeros(4,1);


hxEst=[];
hxTrue=[];
hxDR=[];
hz=[];


while SIM_TIME >=time
    time=time+DT;
    v=1;
    yawrate=0.1;
    u=[v;yawrate];
    [xTrue,z,xDR,ud]=observation(xTrue,xDR,u);
    [xEst, PEst] = ekf_estimation(xEst, PEst, z, ud);
    hxTrue=[hxTrue,xTrue];
    hz=[hz,z];
    hxDR=[hxDR xDR];
    hxEst=[hxEst xEst];
    plot(hxTrue(1,:),hxTrue(2,:)) 
    hold on 
    plot(hz(1,:),hz(2,:),'r-') 
    hold on
    plot(hxDR(1,:),hxDR(2,:),'b*') 
    hold on 
    plot(hxEst(1,:),hxEst(2,:),'k*') 
    pause(0.1)
end
    

这里的画图第一个plot(hxTrue(1,:),hxTrue(2,:)) 是画出真实轨迹，plot(hz(1,:),hz(2,:),'r-')是输出加了噪声的轨迹（可以看为GPS或激光雷达数据），第三个就是推测轨迹（dead reckoning trajectory），第四个EKF 滤波后轨迹。

计算扩展kalman滤波函数：

function [xEst, PEst] = ekf_estimation(xEst, PEst, z, u)

global DT;
global Q;
global R;
xPred = motion_model(xEst, u)
yaw = xPred(3,1);
v = u(1,1);
jF=[1.0, 0.0, -DT * v * sin(yaw), DT * cos(yaw);  %雅可比矩阵
    0.0, 1.0, DT * v * cos(yaw), DT * sin(yaw);
    0.0, 0.0, 1.0, 0.0;
    0.0, 0.0, 0.0, 1.0];
PPred = jF * PEst * jF' + Q

jH = [1, 0, 0, 0;
      0, 1, 0, 0];
 H=[1,0,0,0;0,1,0,0];
 zPred=H*xPred;
 y=z- zPred;
 S = jH * PPred * jH' + R;  
 K = PPred * jH' * inv(S);
 
 xEst = xPred + K * y;
 
 PEst = (eye(length(xEst)) - K * jH) * PPred;
 
  
  

这个程序对应上面的图公式，jF对应雅可比矩阵，这样看程序就很简单了。

运行结果动态图如下：

 

 

代码下载地址：https://download.csdn.net/my/uploads