makerobot.m

SLAM Gridsim use with sparer sensor

function makerobot(res,range,sigmar,max,beamwidth,sigmab)

figure
axis equal
a=0.0; da=2*pi/res;
for i=1:res
%    line([0,range*cos(a)],[0,range*sin(a)],'Color',[0.7 0.2 0.2],'LineWidth',2)
    makebeam(a,beamwidth,sigmab,range,sigmar)
    a=a+da;
end
line(0,0,'Marker','.','MarkerSize',40,'Color','k')
axis([-max max -max max])
%axis off
set(gca,'xtick',[])
set(gca,'ytick',[])
%axes('Visible',false)
%print('-depsc',['robot-rho-' int2str(res) '-rmax-' int2str(range) '.eps']);
%close;


function makebeam(a,beamwidth,sb,range,sr)
makewedge(a,beamwidth+sb,range+sr,[0.4 0.4 0.8]); % C_F
makewedge(a,beamwidth+sb,range-sr,[0.8 0.4 0.4]); % C_E^\Delta
makewedge(a,beamwidth,range-sr,[0.4 0.8 0.4]);    % C_E^*
% makewedge(a,beamwidth+sb,range+sr,[0.8 0.4 0.4]); % bearing uncertainty
% makewedge(a,beamwidth,range+sr,[0.4 0.4 0.8]); % range uncertainty
% makewedge(a,beamwidth,range-sr,[0.4 0.8 0.4]); % ideal


function makewedge(a,arclen,range,col)
if(arclen > 6.28) x=[]; y=[]; else x=[0]; y=[0]; end % circle if big enough
for b=a-arclen:0.01:a+arclen
    x=[x;range*cos(b)];
    y=[y;range*sin(b)];
end
% uncomment these two lines for a range-only sensor
%x=range*cos(0:0.01:2*pi);
%y=range*sin(0:0.01:2*pi);
fill(x,y,col), hold on