clear all;
close all;

load sigma
load N
load X
load del_t

NP = 100;
sigma_0 = 3.0;    % mean-reverting value in stocastic volatility
mu = 0.006;       % value of drift
c = 0.5;          % r^2/(2*lambda)
                  %  where, r = vol of stochastic volatility
                  %         lambda = rate of mean-reversion
lambda = 2;       % the rate of mean-reversion 

sigma_estimated = zeros(N,1);  % estimated volatility after p.f.
sigma_estimated(1) = 0.1;

% estimation part

t=1;
for i=1:NP,
  partic(i,t) = sigma_0 + exp(-lambda*del_t)*(sigma_estimated(1)-sigma_0) + ...
          sqrt(c*(1-exp(-2*lambda*del_t))) * randn;
  weights(i,t) = 1/NP;
end

% iterations

for t=2:N,
  for i=1:NP,
    % propagate the particles
    partic(i,t) = g(partic(i,t-1),sigma_0,lambda,c,del_t);
    % update the weight
    weights(i,t) = weights(i,t-1)*...
        exp(-(0.5/partic(i,t)^2/del_t)*((1+mu*del_t)-X(t))^2)*...
        (1/partic(i,t));
  end
  % normalisation of the weights
  weights(:,t) = weights(:,t)/sum(weights(:,t));
  sigma_estimated(t) = sum(weights(:,t).*partic(:,t));%MMSE
  
%   figure(1); plot(sigma_estimated(1:t)); hold on; plot(sigma(1:t),'r'); hold off
%   legend('Estimated Vol', 'Real Vol');
%   figure(2); hist(weights(:,t))

  % resampling done here
  % optional
  if 1/sum(weights(:,t).^2)<0.7*NP,
%     disp('resampling'); fflush(1); % for octave only
    N_offspring = resample(NP,weights(:,t));
    counter = 0;
    for i=1:NP,
      if N_offspring(i)>0,
        for j=1:N_offspring(i)
          counter = counter +1;
          intermediate_partic(counter) = partic(i,t);
        end
      end
    end
    partic(:,t) = intermediate_partic(:);
    weights(:,t) = 1/NP;
  end


end

%plot

figure(1); plot(sigma_estimated(1:t)); hold on; plot(sigma(1:t),'r'); hold off
legend('Estimated Vol', 'Real Vol');
figure(2); hist(weights(:,t))

% error

disp('RMSE')
norm(sigma_estimated-sigma)/sqrt(N)

