close all;
clear all;
n = 1:100;
stdw = sqrt(10);
ngrid = 50;
npar = 50;
N = length(n);
%N=500
% Generate the state process and observations
x0 = randn(1,1);
c0 = 1;
xpath = zeros(1,N);
xpath(1) = x0/2 + 25*x0/(1+x0^2) +8*cos(1.2) + stdw*randn(1,1);
for i=2:N
xpath(i) = xpath(i-1)/2 + 25*xpath(i-1)/(1+xpath(i-1)^2) +8*cos(1.2*i) + stdw*randn(1,1);
end
zpath = 1/20*(xpath.^2) + randn(1,N);
% Particle filter with resampling
w = ones(npar,1)/npar;
xprev = randn(npar, 1);
SParMat = zeros(npar, N);
SXParMat = zeros(npar, N);
sxparpath = zeros(1,N);
for i=1:N
xnext = drawpar(xprev, stdw, i);
xs = (xnext.^2)/20;
w = w.*(1/sqrt(2*pi)*exp(-((zpath(i)-xs).^2)/2));
w = w/sum(w);
SParMat(:,i) = w;
SXParMat(:,i) = xnext;
sxparpath(i) = w'*xnext;
[xprev, w] = impResample(xnext, w);
end

ErrorSIR = sqrt(1/N*(sxparpath-xpath)*(sxparpath-xpath)');


figure;
plot(n, xpath);
hold on;
plot(n, sxparpath, 'r');
title('SIR Particle filter');
legend('Actual', 'Estimate');