%MainPF: It runs the PF

% APRENDIZAJE AUTOMÁTICO (G. Pajares y J. M. de la Cruz, Eds.)
% Editorial: RA-MA
% Capítulo 8: Monte Carlo II
% Autores: Eva Besada-Portas, Sergey M. Plis


clear;close all;

load SimulaData %Load the PF
resample=1; %0 --> SIS, 1 --> SIR, 2 --> Auxiliar PF
NP=20000;  %Number of particles
doplot=1:size(uc,3); %In which iterations we want to see results (if empty, we wont see them)
[lxm,lxM,lym,lyM]=MapLimits; %Limits of the current map
x0range=[lxm,lxM;lym,lyM;xreal(1,3,1)-0.5*pi/180,xreal(1,3,1)+0.5*pi/180];  %Initial particle range distribution
sen=[2]; %sensors used to calculate the likelihood (1 --> height, 2 --> lights)
filename=sprintf('PFStepsData%d_%d',resample,sum(sen)); %Filename used to save the intermediate positions

[xPF,w,preset]=ParticleFilter(NP,x0range,uc,dt,covrobot,covceil,light,resample,yreal,sen,filename,doplot,xreal);
x1=ObtainMeanTrajectory(xPF,w);

%Show the history information
figure
plot(x1(1,:),x1(2,:),'g','LineWidth',2);
hold on;
x=squeeze(xreal);
plot(x(1,:),x(2,:),'b','LineWidth',2);
h=legend('Valor medio','Real');
set(h,'FontSize',12);
MapLight(1);
Height(1);
xlabel('X (m)','FontSize',12);ylabel('Y (m)','FontSize',12);
axis square
