对于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对应雅可比矩阵,这样看程序就很简单了。
运行结果动态图如下: