load rvpig507.raw.mat

k = size(d,1);
m = 16; n = 16;

dist = zeros( k*m, n );
t = 0:k-1;

for i = 1:m,
   for j = 1:n,
       dist( i + m*t, j ) = d( : , n*(i-1)+j );
   end;
end;

clear d

% save rvpig507 dist
% load rvpig507

dTeta;


i = 15; j = 12;

x = dExtract(dist, i,j); xt = dExtract(teta, i,j);
t = 651:1050;

figure(1); 

subplot(211); 
plot(t/100, x(t)); 
xlabel('time [sec]');
ylabel('distance');


subplot(212); 
H = plot(t/100, xt(t));
set(H,'LineWidth', 2);
axis([ get(gca,'XLim')  0 1.2] );
xlabel('time [sec]');
ylabel('confidence');

subplot(211); 
[xc, yc] = ginput(2);
rx = abs(diff(xc))/2;  ry = abs(diff(yc))/2; 
cx = mean(xc);	       cy = mean(yc);
ellipse = [(cx + rx*cos(0:0.01:2*pi)') (cy + ry*sin(0:0.01:2*pi)')];

hold on;
plot(ellipse(:,1), ellipse(:,2));
hold off;