% particle filter
% example: 2D localization

clear; close all;

T=50;  % simulation length
M=1000;
N=1000; r=M/N; 

sigmav=.2;
artQ=0.05; % variance of robot perturbation (try artQ=0 !)

L=[1  1]';  % 2nd landmark

% data generation;
v=sigmav*randn(2,T);
x(1:T,:)=ones(T,1)*[0 1];
for i=1:T,
    y(:,i)=[sqrt(x(i,1)^2+x(i,2)^2); sqrt((x(i,1)-L(1))^2+(x(i,2)-L(2))^2)]+v(:,i);
    end

% initialization
xi=4*randn(N,2)-2;

xekf=[0.1 0.1]';
P=eye(2);

for t=1:T,  
    
    % prediction
    xhati=xi+artQ*randn(N,2);  
   
    % weights
    yhati=[sqrt(xhati(:,1).^2+xhati(:,2).^2) sqrt((xhati(:,1)-L(1)).^2+(xhati(:,2)-L(2)).^2)]; %output prediction
    unq=normpdf(y(1,t)-yhati(:,1),0,sigmav).*normpdf(y(2,t)-yhati(:,2),0,sigmav); % unnormalized weights
    q=unq./sum(unq); % normalized weights
    sumq(1)=q(1); for ih=2:M, sumq(ih)=sumq(ih-1)+q(ih); end;
    
    % resampling
    for i=1:N,
        ux=rand;
        diffux=(ux > sumq);
        xi(i,:)=xhati(sum(diffux)+1,:);
    end
    
    % ekf
    Qe=artQ*eye(2);
    P=P+Qe;
    H=[xekf(1)/sqrt(xekf(1)^2+xekf(2)^2) xekf(1)/sqrt(xekf(1)^2+xekf(2)^2);
        (xekf(1)-L(1))/sqrt((xekf(1)-L(1))^2+(xekf(2)-L(2))^2)  (xekf(2)-L(2))/sqrt((xekf(1)-L(1))^2+(xekf(2)-L(2))^2)];
    K=P*H'*inv(H*P*H'+sigmav*eye(2));
    xekf=xekf+K*(y(:,t)-[sqrt(xekf(1)^2+xekf(2)^2); sqrt((xekf(1)-L(1))^2+(xekf(2)-L(2))^2)]);
    P=P-K*H*P;
    
    fy=5; %plot situation every fy iterations
    if t==1 | (t/fy)==ceil(t/fy),
         scrsz = get(0,'ScreenSize');
         figure('Position',scrsz)
         subplot(121),  plot(xi(:,1),xi(:,2),'*');
         hold on
         th=0:0.1:2*pi; plot(cos(th),sin(th),'r--');
         radi=norm(L-[0 1]'); plot(radi*cos(th)+L(1),radi*sin(th)+L(2),'r--');
         axis('equal')
         plot(xekf(1),xekf(2),'kx');
         title('particles (blue) and ekf (black cross)');
         subplot(122), plot(sort(q));
         title('sorted weights');
         pause
    end

    
end

