手把手教用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