Thetamap

thetamap.m

clear ; close all; clc
sonarmap = importdata('sonarmap.mat');

A=[];b=[];

xarr = [];yarr = [];rarr = [];tarr = [];

for i=1:size(sonarmap,1),
    x = sonarmap(i,1) - 196;
    y = 479 - sonarmap(i,2);
    r = sonarmap(i,3);
    t = sonarmap(i,4);
    
    xarr = [xarr x];
    yarr = [yarr y];
    rarr = [rarr r];
    tarr = [tarr t];
end;

plot3(xarr,yarr,tarr);

X = [xarr',yarr',tarr'];
y = zeros([size(X,1), 1]);
angleModel = @(b,x) b(1).*(1./(1+exp(-1./(b(2).*(x(:,2)+b(3))).*x(:,1)))-0.5) - (x(:,3)); 
beta0 = [150 2 30];
mdl = fitnlm(X,y,angleModel,beta0);
data = mdl.coefCI;
data = data(:,1)';

% Compute Error
rejectcount = 0;
errdata = angleModel(data, X);

figure(1);
plot3(xarr,yarr,tarr)
figure(2);
plot(errdata)

% Error occurs towards the sides close to sonar, which are impossible
% detection zones anyway