rmap

rmap.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 = cosd(sonarmap(i,4));
    
    xarr = [xarr x];
    yarr = [yarr y];
    rarr = [rarr r];
    tarr = [tarr t];
    
    A=[A;x.^2 x*y y.^2 x y 1];
    b=[b;r.^2];

end;

% Least squares
estimated=(A'*A)\(A'*b);

% Compute Error
rejectcount = 0;
recompute = sqrt(A*estimated);
errdata = [];
m = 0;
for i=1:size(sonarmap,1),
    x = sonarmap(i,1) - 196;
    y = 479 - sonarmap(i,2);
    r = sonarmap(i,3);
    
    err = recompute(m) - r;
    errdata = [errdata err]; 
end

figure(1);
plot3(xarr,yarr,rarr)
figure(2);
plot(errdata)
norm(errdata).^2