%% Odhadovani pozice z GPS

N = 5;       % pocet satelitu
p = [0;0];   % pozice pozorovatele

x = [4;-4];  % pocatecni odhad pozice pozorovatele
%x = [-4;4];

%% vygeneruj pozice satelitu a vzdalenosti od k pozorovatele
NOISE = 0;   % nahodna odchylka mereni vzdalenosti k satelitu
fname = 'gps_data.mat';
if exist(fname)
    fprintf('Nahravam data z %s\n', fname);
    load(fname,'A','y');
else    
    A = zeros(2,N);
    y = zeros(N,1);
    for i=1:N
        t = rand(1);
        y(i) = 4*(1-t)+3*t;
        phi = rand(1)*2*pi;
    
        A(:,i) = p + y(i)*[cos(phi);sin(phi)];
    end
    y = y + NOISE*randn(size(y));    
    save(fname,'A','y','N');
end


%% zobraz vstupni body
figure(1);
axis([-6 6 -6 6]);
axis manual;
hold on;
plot(A(1,:),A(2,:),'xk','markersize',12);
plot(p(1),p(2),'sm','markersize',14,'linewidth',2);
h1 = plot(x(1),x(2),'or','markersize',12,'markerfacecolor','r');
xlabel('x_1');
ylabel('x_2');


%% zobraz kriterialni funkci a pocatecni reseni
figure(2);
axis([-6 6 -6 6]);
M=50; x1=linspace(-6,6,M)'*ones(1,M); x2=x1';
F = zeros(size(x1));
for i=1:size(A,2)
   F= F + (sqrt((x1-A(1,i)).^2 + (x2-A(2,i)).^2) - y(i)).^2;
end
mesh(x1,x2,F,'facecolor','w','edgecolor','k');
hold on; 
xlabel('x_1');
ylabel('x_2');
zlabel('f');

fVal = sum((sqrt(sum((repmat(x,1,N)-A).^2))' - y).^2);
h2 = plot3(x(1),x(2),fVal,'or','markersize',12,'markerfacecolor','r');

pause;


%% hlavni cyklus
for i=1:100
    
   dif = repmat(x,1,N)-A;
   normDif = sqrt(sum(dif.^2));
   
   g = normDif(:) - y(:);
   fVal = g'*g;
   gGrad = dif'./repmat(normDif(:),1,2);

   fprintf('%d:  fval=%f\n', i, fVal);
    
      
   switch 1
       case 1 % Gradientni metoda s fixnim krokem
            
            alpha = 1e-1;
            x = x - alpha * 2*gGrad'*g;
            
       case 2  % Gaussova metoda

       
       case 3  % Levemberg-Markvuardt
           
   end
           
   
   
   %% zobraz reseni
   figure(1);
   delete(h1);
   h1 = plot(x(1),x(2),'or','markersize',12,'markerfacecolor','r');
   
   figure(2);
   delete(h2);
   fVal = sum((sqrt(sum((repmat(x,1,N)-A).^2))' - y).^2);
   h2 = plot3(x(1),x(2),fVal,'or','markersize',12,'markerfacecolor','r');
   
   pause;
   
end
