clear all;close all;clc;

%% Chargement obs.
load ../obs_float/init.dat;
load ../obs_float/obs.dat

fname='25float-filtre15-inc';

%figure(1)
%clf
%plot(init(1,end)+1,init(2,end)+1,'k+');
%hold on
%plot(obs(1,end)+1,obs(2,end)+1,'*r');

%% Chargement des trajectoires
 figure(1)
    clf
for j=1:6
    ind=round(linspace(1,16,6));
    subplot(3,2,j)
    plot(init(1,end)+1,init(2,end)+1,'k+');
hold on
plot(obs(1,end)+1,obs(2,end)+1,'*r');
r_float=load(['../obs_float/r_float_optim' fname '_' int2str(ind(j)) '.dat']);
uoptim=load(['../obs_float/uoptim' fname int2str(ind(j)) '.dat']);
voptim=load(['../obs_float/voptim' fname int2str(ind(j)) '.dat']);
plot(r_float(1:2:end,end)+1,r_float(2:2:end,end)+1,'-b');
plot(r_float(3:2:end-3,end)+1,r_float(4:2:end-2,end)+1,'ob');

plot(r_float(1,end)+1,r_float(2,end)+1,'+k');
plot(r_float(end-1,end)+1,r_float(end,end)+1,'*b');
quiver(uoptim(:,end-2),uoptim(:,end-1),uoptim(:,end),voptim(:,end),'k');
axis([6 15 13 22]);
plot(obs(1,end)+1,obs(2,end)+1,'*r');
title([' extern loop number ' int2str(ind(j))]);
grid on
end
