%data
%parameter=1;    %enkel US
%parameter=2;    %enkel RF   
parameter=3;    %US en RF
%parameter=inputdlg('Parameter','Parameter');

 resample=1;   %select_with_replacement;
% resample=2; %linear_time_resampling;
% resample=3; %liu_resampling;

measurement_noise=0; % 0 or 1
%process_noise=1;

memory=1; %0 or 1   
% memory=0 => W(j+1)=weight(j+1)
% memory=1 => W(j+1)=W(j)*weight(j+1)

% initial particles
beta=0.8;
n=5000;

% particle1=[0;0;0;0.6];
% particle2=[1;1;1;0.4];
% S=[particle1,particle2];
% S=[];
% for i=1:n
%     S=[S,particle1];
% end

% one place particles
range_pos=50;
S=[0*ones(1,n);0*ones(1,n);0*ones(1,n);1*ones(1,n)];
S(4,:)=S(4,:)/sum(S(4,:));

% %random particles
% range_pos=50;
% range_ang=360*pi/180;
% S=[range_pos*rand(1,n)-range_pos/2;
%    range_pos*rand(1,n)-range_pos/2;
%    range_ang*rand(1,n)-range_ang/2;
%    1*ones(1,n)];
% S(4,:)=S(4,:)/sum(S(4,:));

M_rot=0;
M_trs=0;
M_drft=0;
sigma_rot=0/360;
sigma_trs=0.5; %0.5
sigma_drft=5*pi/180;%5*pi/180;

%number of steps
steps=25;

% no rotation
% delta_theta=0*pi/180*ones(steps,1);
delta_theta=[0*pi/180*ones(steps-14,1);pi/2;0*pi/180*ones(8,1);pi/2;0*pi/180*ones(5,1)];
%delta_theta=[0*pi/180*ones(steps-19,1);pi/2;0*pi/180*ones(18,1)];
% translation
rho=1*ones(steps,1);
%dx=delta_rho*cos(theta).*ones(steps,1);
%dy=0*ones(steps,1);
%rho=sqrt(dx.^2+dy.^2);
%theta=atan2(dy,dx);

% real position
xm(1)=0;
ym(1)=0;
thetam(1)=0;
for i=1:steps  
    thetam(i+1)=thetam(i)+delta_theta(i);    
    dx(i)=rho(i)*cos(thetam(i+1));
    dy(i)=rho(i)*sin(thetam(i+1));
    xm(i+1)=xm(i)+dx(i);
    ym(i+1)=ym(i)+dy(i);
end