%% initial setup
N = 200;         % number of data
om = 10;         % omega = k/m in physics
dt = 0.01;       % smapling time \Delta t

Ac = [0 1; -om^2 0];  % continuous time matrix
A= expm(Ac*dt);       % discrete time matrix

%% data generation
X = zeros(2,N);
for n=1:N
    x = A2*x;
    X(:,n) = x;
%     if n==100;  % step change in the middle
%         x(1)=x(1)+20;
%     end
end
y = X(1,:) + 1*randn(1,N); % add random noise to observations

p0=1;            % parameter for particle spread

%%
NP=100;          % number of particels
for j =1:NP
    XX{j} = p0*randn(2,1); % simulate from prior
    w(j) = 1/NP; % initial weights
    XXX{j}=zeros(2,N);
end
for n=1:N
    for j = 1:NP
        XX{j} = A2*XX{j}+0.1*randn(2,1);         % predict new state
        w(j) = exp(-1/2*(y(n)-XX{j}(1))^2)*w(j); % evaluate weight
%         [XX{j}(1) w(j)]
        XXX{j}(:,n)=XX{j};
    end
    w = w/sum(w);    % normalize weights
    W(:,n)=w;       
    
    %         n
    ind=resample_deterministic(1:NP,w'); % apply resampling
    for j=1:NP                           % copy particles to new locations
        XXn{j}=XX{ind(j)};
    end
    XX=XXn;
    w(:)=1/NP;                           % after resampling, the weights are uniform
    
end

%% prepare results 
Xgl=zeros(NP,N);
for j=1:NP
    Xgl(j,:)=XXX{j}(1,:);
end

%
fig=figure(1);
hold off
plot(X(1,:)')
hold on
%plot(y,'x')
plot(Xgl','.')
%title(['Q=[' num2str(KF.Q(1,1))  ','  num2str(KF.Q(2,2)) '], R=1, P_0=' num2str(p0) 'I, x_0=[' num2str(x0(1)) ',' num2str(x0(2)) ']' ])
title(['R=1, P_0=' num2str(p0) 'I, x_0=[' num2str(x0(1)) ',' num2str(x0(2)) ']' ])
legend('true x', 'estimated x')

% %%
% fig.PaperUnits = 'inches';
% fig.PaperPosition = [0 0. 4 3];
% fig.PaperSize = [4 3];
% print(fig,['sine_pf_ADP0' [num2str(p0)] '_q1' num2str(KF.Q(1,1)) '_q2' num2str(KF.Q(2,2)) ],'-dpdf')