www.gusucode.com > MATLAB编程毕业设计 EKF SLAM仿真全部源代码 > SLAM_sim.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%主函数
function SLAM_sim(handles)
%载入重要变量数据
configFile;
%控制仿真结束的变量
global closeClickCount;
%%
% axes(handles.axes1);
% axis equal;
% hold on;
% %绘制墙体、挡板
% for i = 1:length(wall(:,1))/2
% line([wall(i*2-1,1) wall(i*2,1)],[wall(i*2-1,2) wall(i*2,2)],'LineWidth',4,'Color','k');
% end
%
% %绘制真实车辆的句柄
% H_robot = line([0 0],[0 0],'EraseMode','xor','Color','m','LineWidth',6);
% %绘制车辆真实轨迹的句柄
% H_path_true = line([0 0],[0 0],'EraseMode','none','linestyle','.','Color',[rand(1) rand(1) rand(1)],'LineWidth',0.1);
% %绘制车辆预计轨迹的句柄
% H_path_predict = line([0 0],[0 0],'EraseMode','none','linestyle','.','Color',[rand(1) rand(1) rand(1)],'LineWidth',0.1);
axes(handles.axes5);
hold on;
%绘制与真实值距离
% distFrPreToTrue = [1:600];
distFrPreToTrue = [];
H_PreTrue_Dis = line([0 1],[0 1],'EraseMode','none','Color','k','linestyle','.');
% line([0 0],[1 1],'EraseMode','xor','Color','k','linestyle','.');
% plot([0 1],[0 1]);
%%
axes(handles.axes2);
axis([-30 350 0 350]);
axis equal;
hold on;
grid on;
%绘制激光数据点的句柄
H_raser_point = line([0 0],[0 0],'EraseMode','xor','linestyle','.','Color','k');
%绘制兴趣点的句柄
H_interest_point = line([0 0],[0 0],'EraseMode','xor','linestyle','s','Color','b','LineWidth',3);
%扫描范围的半圆
raserCirX = 350*cos(0:pi/15:pi);
raserCirY = 350*sin(0:pi/15:pi);
line(raserCirX,raserCirY,'linestyle','--');
line(raserCirX*265/350,raserCirY*265/350,'linestyle','--');
line([-350 350],[30 30],'linestyle','--');
%%
axes(handles.axes3);
axis equal;
axis([-50 1050 -150 650]);
hold on;
H_true_robot = line([0 0],[0 0],'EraseMode','xor','Color','r','LineWidth',6);
H_predict_robot = line([0 0],[0 0],'EraseMode','xor','Color','b','LineWidth',6);
H_probably_robot = line([0 0],[0 0],'EraseMode','xor','Color','k');
H_probably_interest_point = [];
%绘制墙体、挡板
for i = 1:length(wall(:,1))/2
line([wall(i*2-1,1) wall(i*2,1)],[wall(i*2-1,2) wall(i*2,2)],'LineWidth',4,'Color','k');
end
%绘制全局地图兴趣点的句柄
H_map_interest_point = line([0 0],[0 0],'EraseMode','xor','linestyle','s','Color','m','LineWidth',5);
%绘制激光扫描线的句柄
H_laser_line = line([0 0],[0 0],'EraseMode','xor','Color','y','LineWidth',2);
%%
axes(handles.axes6);
axis([-50 1050 -150 650]);
H_wall_predict = line([0 0],[0 0],'EraseMode','none','color','b');
H_wall_true = line([0 0],[0 0],'EraseMode','none','color','r');
hold on;
%%
%打开文件
fid = fopen('fileData.txt','r');
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% junmpNum = 400;
% for i=1:junmpNum
% [robx,roby,robphi,G,pointNum,px,py,splitNum,splitIndex] = getDataFrFile(fid);
% end
% x = [robx,roby,robphi];
for i=1:1:1200
%设置循环位置的显示
set(handles.loopNum,'String',num2str(i));
%暂停仿真
while(closeClickCount)
pause(0.2);
if(closeClickCount == -1)
return;
end
end
%%
%从文件中获取参数
[robx,roby,robphi,G] = getDataFrFile1(fid);
%计算真实位置
xtrue = vehicle_model([robx;roby;robphi],V,G,WB,dt);
%给控制量V和G融入噪声
[Vn,Gn] = add_control_noise(V,G,Q);
%EKF预测步骤
[x,P] = predict(x,P,Vn,Gn,Q,WB,dt);
% x = vehicle_model(x,Vn,Gn,WB,dt);
%%
if(mod(i,2)==0)
%从文件中获取参数
[pointNum,px,py,splitNum,splitIndex] = getDataFrFile2(fid);
%从激光测距仪的输出中提取直线
[lineNum,linePara] = getLine(px,py,pointNum,splitIndex,splitNum);
closePoint = getClosePoint(lineNum,linePara,px,py);
%在小坐标中提取兴趣点
interestPoint = extractInterestPoint(closePoint,lineNum,linePara,px,py);
%在大坐标中提取兴趣点
mapInterestPoint = changeCoordinate(interestPoint,xtrue(1),xtrue(2),xtrue(3));
%大坐标下，周围环境实际情况的反映和预计的反映
cpNum = size(closePoint,1);
mapClosePoint = zeros(cpNum*2,2);
for tt = 1:cpNum
mapClosePoint(tt*2-1,:) = closePoint(tt,1:2);
mapClosePoint(tt*2,:) = closePoint(tt,3:4);
end
map_wall_true = changeCoordinate(mapClosePoint,xtrue(1),xtrue(2),xtrue(3));
map_wall_predict = changeCoordinate(mapClosePoint,x(1),x(2),x(3));
[map_wall_true_x,map_wall_true_y] = getWallXY(map_wall_true);
[map_wall_predict_x,map_wall_predict_y] = getWallXY(map_wall_predict);
%获取观测值
z = interestPoint2z(interestPoint);
%数据匹配
[zf,idf,zn]= data_associate(x,P,z,R,GATE_REJECT,GATE_AUGMENT);
%利用匹配成功的lm，zf更新机器人状态x和状态协方差P
[x,P]= batch_update(x,P,zf,R,idf);
%利用新发现的lm，zn更新更新机器人状态x和状态协方差P
[x,P]= augment(x,P,zn,R);
end
%%
if(mod(i,2)==0)
axes(handles.axes3);
%【绘制】机器人真实位置
% set(H_robot,'xdata',[xtrue(1)+25*cos(xtrue(3)) xtrue(1)-25*cos(xtrue(3))],'ydata',[xtrue(2)+25*sin(xtrue(3)) xtrue(2)-25*sin(xtrue(3))]);
%【绘制】激光测距仪的散点
set(H_raser_point,'xdata',px,'ydata',py);
% %【绘制】大坐标兴趣点
% if(~isempty(interestPoint))
% set(H_interest_point,'xdata',interestPoint(:,1),'ydata',interestPoint(:,2));
% else
% set(H_interest_point,'xdata',[0],'ydata',[0]);
% end
%【绘制】小坐标兴趣点
if(~isempty(mapInterestPoint))
set(H_map_interest_point,'xdata',mapInterestPoint(:,1),'ydata',mapInterestPoint(:,2));
else
set(H_map_interest_point,'xdata',[0],'ydata',[0]);
end
%在第三幅图上作画
%预计的轨迹
line(x(1),x(2),'linestyle','.','MarkerSize',10);
set(H_predict_robot,'xdata',[x(1)+25*cos(x(3)) x(1)-25*cos(x(3))],'ydata',[x(2)+25*sin(x(3)) x(2)-25*sin(x(3))]);
%真实的轨迹
line(xtrue(1),xtrue(2),'linestyle','.','color','r','MarkerSize',15);
set(H_true_robot,'xdata',[xtrue(1)+25*cos(xtrue(3)) xtrue(1)-25*cos(xtrue(3))],'ydata',[xtrue(2)+25*sin(xtrue(3)) xtrue(2)-25*sin(xtrue(3))]);
%机器人位置的不确定性
ptmp= make_covariance_ellipses(x(1:3),P(1:3,1:3));
set(H_probably_robot,'xdata',ptmp(1,1:11),'ydata',ptmp(2,1:11));
%画出landmark的不确定性
landmarkNum = (length(x)-3)/2;
delete(H_probably_interest_point);
H_probably_interest_point = [];
for nm = 1:landmarkNum
ptmp= make_covariance_ellipses([x(2*nm+2:2*nm+3)],P(2*nm+2:2*nm+3,2*nm+2:2*nm+3));
tempHandle = line(ptmp(1,1:11),ptmp(2,1:11));
H_probably_interest_point = [H_probably_interest_point tempHandle];
end
%绘制激光扫描线
plines = make_laser_lines(z,x(1:3));
if(~isempty(plines))
set(H_laser_line,'xdata',plines(1,:),'ydata',plines(2,:));
end
% %绘制距离差距
% axes(handles.axes5);
% distFrPreToTrue = sqrt((x(1)-xtrue(1))^2+(x(2)-xtrue(2))^2);
% line([fix(i/2) fix(i/2)],[0 distFrPreToTrue]);
%绘制真实的墙和预估的墙
% set(H_wall_true,'xdata',map_wall_true_x,'ydata',map_wall_true_y);
% set(H_wall_predict,'xdata',map_wall_predict_x,'ydata',map_wall_predict_y);
axes(handles.axes6);
if(i<570)
line(map_wall_true_x,map_wall_true_y,'color','r');
line(map_wall_predict_x,map_wall_predict_y,'color','b','LineStyle',':');
else
line(map_wall_predict_x,map_wall_predict_y,'color','m');
end
% set(H_PreTrue_Dis,'xdata',[fix(i/2) fix(i/2)],'ydata',[0 distFrPreToTrue]);
% if(~isempty(zf))
% xyf = z2xy(zf);
% mapxyf = changeCoordinate(xyf,x(1),x(2),x(3));
% line(mapxyf(:,1),mapxyf(:,2),'linestyle','p','color','r');
% end
% if(~isempty(zn))
% xyn = z2xy(zn);
% mapxyn = changeCoordinate(xyn,x(1),x(2),x(3));
% line(mapxyn(:,1),mapxyn(:,2),'linestyle','p','color','k');
% end
drawnow;
end
%%
pause(0.01);
end
fclose(fid);
end
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%从文件组中获取参数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [robx,roby,robphi,G] = getDataFrFile1(fid)
robx = fread(fid,1,'double');
roby = fread(fid,1,'double');
robphi = fread(fid,1,'double');
G = fread(fid,1,'double');
end
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%从文件组中获取参数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [pointNum,px,py,splitNum,splitIndex] = getDataFrFile2(fid)
pointNum = fread(fid,1,'uchar');
px = fread(fid,pointNum,'double');
py = fread(fid,pointNum,'double');
splitNum = fread(fid,1,'uchar');
splitIndex = [];
splitIndex(:,1) = fread(fid,splitNum,'uchar');
splitIndex(:,2) = fread(fid,splitNum,'uchar');
end
%%
function p= make_covariance_ellipses(x,P)
% compute ellipses for plotting state covariances
N= 10;
inc= 2*pi/N;
phi= 0:inc:2*pi;
lenx= length(x);
lenf= (lenx-3)/2;
p= zeros (2,(lenf+1)*(N+2));
ii=1:N+2;
p(:,ii)= make_ellipse(x(1:2), P(1:2,1:2), 2, phi);
ctr= N+3;
for i=1:lenf
ii= ctr:(ctr+N+1);
jj= 2+2*i; jj= jj:jj+1;
p(:,ii)= make_ellipse(x(jj), P(jj,jj), 2, phi);
ctr= ctr+N+2;
end
end
%
%
function p= make_ellipse(x,P,s, phi)
% make a single 2-D ellipse of s-sigmas over phi angle intervals
r= sqrtm(P);
a= s*r*[cos(phi); sin(phi)];
p(2,:)= [a(2,:)+x(2) NaN];
p(1,:)= [a(1,:)+x(1) NaN];
end
%
%
function p= make_laser_lines (rb,xv)
% compute set of line segments for laser range-bearing measurements
if isempty(rb), p=[]; return, end
len= size(rb,2);
lnes(1,:)= zeros(1,len)+ xv(1);
lnes(2,:)= zeros(1,len)+ xv(2);
lnes(3:4,:)= TransformToGlobal([rb(1,:).*cos(rb(2,:)); rb(1,:).*sin(rb(2,:))], xv);
p= line_plot_conversion (lnes);
end
function [wallx,wally] = getWallXY(wall)
num = size(wall,1)/2;
wallx = zeros(1,3*num);
wally = zeros(1,3*num);
for i = 1:num
wallx(i*3-2:i*3) = [wall(2*i-1,1) wall(2*i,1) nan];
wally(i*3-2:i*3) = [wall(2*i-1,2) wall(2*i,2) nan];
end
end