real_sensors.m

SLAM Gridsim use with sparer sensor

function real_sensors

warning off all

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% sensor models
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% sick lms 200
laser.F    = 20;
laser.rho  = 360;
laser.rmax = 10;
laser.db   = deg2rad(0.6);
laser.sb   = deg2rad(0.06);
laser.sr   = 0.015;
laser.ee   = 0.02;
laser.ef   = 0.001;

% polaroid 6500 (array of 16)
sonar16.F    = 12;
sonar16.rho  = 16;
sonar16.rmax = 10;
sonar16.db   = deg2rad(15);
sonar16.sb   = deg2rad(0.06);
sonar16.sr   = 0.1;
sonar16.ee   = 0.08;
sonar16.ef   = 0.01;

% polaroid 6500 (array of 24)
sonar24 = sonar16;
sonar24.rho = 24;

% sharp gp2d12 (array of 5)
ir5.F    = 25;
ir5.rho  = 5;
ir5.rmax = 0.8;
ir5.db   = deg2rad(1.8);
ir5.sb   = deg2rad(0.06);
ir5.sr   = 0.015;
ir5.ee   = 0.02;
ir5.ef   = 0.001;

% sharp gp2d12 (array of 16)
ir16 = ir5;
ir16.rho = 16;

sensors=[laser; sonar16; sonar24; ir5; ir16];
names=char('laser','sonar-16','sonar-24','ir-5','ir-16');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% environment models
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

small_sparse.delta = 0.1;
small_sparse.M     = 200;
small_sparse.d     = 0.01;
small_sparse.maxnu = 3e4;
small_sparse.maxT  = 2.5e3;

large_sparse.delta = 0.1;
large_sparse.M     = 1000;
large_sparse.d     = 0.01;
large_sparse.maxnu = 10e5;
large_sparse.maxT  = 10e4;

small_dense        = small_sparse;
small_dense.d      = 0.1;

large_dense        = large_sparse;
large_dense.d      = 0.1;

envs = [small_sparse; large_sparse; small_dense; large_dense];
envnames = char('small-sparse','large-sparse','small-dense','large-dense');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% model error assuming the sensors have the same running time
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

T=1:1:1800;
for e=1:length(envs)
%    plot_all_error_vs_time(sensors, names, envs(e), envnames(e,:), T);
end

fprintf(1, 'Expected error for different time limits\n');
for j=1:length(envs)
    fprintf(1, 'delta=%f, M=%d, d=%f\n', envs(j).delta, envs(j).M, envs(j).d);
    T=[100;1000;10000];
    for t=1:length(T)
        fprintf(1, '  T=%f\n', T(t));
        [o,lasernu]=model_error3(sensors(1), envs(j), T(t));
        for i=1:length(sensors)
            [o,nu]=model_error3(sensors(i), envs(j), T(t));
            fprintf(1, '    %s: nu=%f (%f x laser)\n', names(i,:), nu, nu/lasernu);
        end
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute running time to obtain equal expected error
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nu=0:1:5000;
for e=1:length(envs)
%    plot_all_time_vs_error(sensors, names, envs(e), envnames(e,:), nu);
end

fprintf(1, 'Time required to obtain different expected error bounds\n');
for j=1:length(envs)
    fprintf(1, 'delta=%f, M=%d, d=%f\n', envs(j).delta, envs(j).M, envs(j).d);
    nuperc=[0.001; 0.01; 0.1; 0.3];
    for n=1:length(nuperc)
        fprintf(1, '  Percent of incorrect cells (nu/M^2) = %f\n', nuperc(n));
        laserT=model_time(sensors(1), envs(j), nuperc(n)*(envs(j).M)^2);
        for i=1:length(sensors)
            T=model_time(sensors(i), envs(j), nuperc(n)*(envs(j).M)^2);
            fprintf(1, '    %s: T=%f (%f x laser)\n', names(i,:), T, T/laserT);
        end
    end
end    

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% "normalized wrt laser" running times plotted vs. env. density
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

d=0:0.005:1;
normenv = small_sparse;
plot_all_time_vs_density(sensors, names, normenv, d);




function [r] = deg2rad(d)
r = d*pi/180;

function [o,nu] = error_vs_time(sensor, env, T)
o=[]; nu=[];
for i=1:length(T)
    [o(i),nu(i)]=model_error3(sensor, env, T(i));
end

function [T] = time_vs_error(sensor, env, nu)
T=[];
for i=1:length(nu)
    T(i)=model_time(sensor, env, nu(i));
end

function [T] = time_vs_density(sensor, env, d)
T=[];
for i=1:length(d)
    env.d = d(i);
    T(i)=model_time(sensor, env, 1);
end

function plot_all_error_vs_time(sensors, names, env, envname, T)
figure
linespecs=['k','r','g','b','m', 'y', 'c'];
for i=1:length(sensors)
    [o,nu]=error_vs_time(sensors(i), env, T);
%    plot(T, nu, linespecs(i), 'LineWidth', 4), hold on
    niceplot(nu, [1 length(nu)], [0 env.maxnu], 6);
    set(gca,'Box','off');
    print('-depsc',['results/err-vs-time-' deblank(envname) '-' deblank(names(i,:)) '.eps']);
    close
end
%axis([-Inf Inf 0 Inf]);
%xlabel('Running time (T)','FontSize',16);
%ylabel('Expected ML map error (E[\nu])','FontSize',16);
%legend(names);
%title(['\delta=' num2str(env.delta) ', M=' num2str(env.M) ', d=' num2str(env.d)]);
%print('-depsc',['results/err-vs-time-' deblank(envname) '.eps']);
%close

function plot_all_time_vs_error(sensors, names, env, envname, nu)
figure
linespecs=['k','r','g','b','m', 'y', 'c'];
for i=1:length(sensors)
    T=time_vs_error(sensors(i), env, nu);
%    plot(nu, T, linespecs(i), 'LineWidth', 2), hold on
    niceplot(T, [1 length(nu)], [0 env.maxT], 6);
    set(gca,'Box','off');
    print('-depsc',['results/time-vs-err-' deblank(envname) '-' deblank(names(i,:)) '.eps']);
    close
end
%axis([-Inf Inf 0 Inf]);
%xlabel('Expected ML map error (E[\nu])','FontSize',16);
%ylabel('Running time (T) to obtain E[\nu]','FontSize',16);
%legend(names);
%title(['\delta=' num2str(env.delta) ', M=' num2str(env.M) ', d=' num2str(env.d)]);
%print('-depsc',['results/time-vs-err-' deblank(envname) '.eps']);
%close

function plot_all_time_vs_density(sensors, names, env, d)
figure
linespecs=['k','r','g','b','m', 'y', 'c'];
Tlaser=time_vs_density(sensors(1), env, d);
for i=1:length(sensors)
    T=time_vs_density(sensors(i), env, d);
    T=T./Tlaser;
%    plot(d, T, linespecs(i), 'LineWidth', 2), hold on
    niceplot2(T, [1 length(T)], [1 900], 4)
    print('-depsc',['results/normalized-time-vs-d-' deblank(names(i,:)) '.eps']);
    close
end
%%axis([-Inf Inf 0 Inf]);
%axis([-Inf Inf 0 900]);
%xlabel('Environment density (d)','FontSize',16);
%ylabel('Laser-normalized running time','FontSize',16);
%legend(names);
%%title('');
%pbaspect([3 1 1]);
%%print('-depsc',['results/normalized-time-vs-d.eps']);
%%close