% this is a demo for the bootstrap particle filter (bpf)

rng(10)   % use rng('shuffle') to initialize the seed to a different value each time you run this script

% load exactly the same data produced with the NimbleSMC R package
% those were produced with a = 1 and b=1.
yobs = load('yobs.dat');


% Set up some parameters
N = 1000;          % Number of particles 
T = length(yobs);            % Length of data record, assumes integer observational times
x0 = 0; % assumed deterministic initial condition (you can change this and make it random)

% run a bootstrap particle filter with N particles, parameters a and b and
% initial condition x0

% lets' try some parameter values
% start with the true ones a=1,b=1
a=1; b=1;
loglik = bpf(yobs, a, b, x0, N)

% of course loglik is a random variable since it is estimated via Monte
% Carlo (particle filters), so if we rerun it we obtain a different value
loglik = bpf(yobs, a, b, x0, N)

% try some values further away from the truth
a=1.1; b=0.7;
loglik = bpf(yobs, a, b, x0, N) 

% fix b to truth and lets make a vary on a grid of values
b=1; a_grid = [0.2:.1:2];

numattempts = 10;  % you can set it to 1 for a single run
for run=1:numattempts
loglik_grid = zeros(1,length(a_grid)); % store here all loglikelihoods
   for ii=1:length(a_grid)
      loglik_grid(ii) = bpf(yobs, a_grid(ii), b, x0, N); 
   end
   plot(a_grid,loglik_grid)
   hold on
end
hold off

% let's look again at one run, but this time we focused on the x from the
% filtering distribution
a=1; b=1;
N = 20;  % take N=20 just to make things readable in the plots
[loglik,x] = bpf(yobs, a, b, x0, N); % notice here I also return x
figure
plot(yobs,'o')
hold on
for(ii=1:N)
   plot([1:length(yobs)],x(ii,:),'Color',[.7 .7 .7])
end
hold off
   
function [loglik,x] = bpf(y, a, b, x0, N)
% Bootstrap particle filter
% Input:
%   y - measurements
%   a, and b - model parameters
%   x0 - initial condition of the latent state
%   N - number of particles
% Output:
%   loglik - the estimated loglikelihood value at input parameters
%   x      - states from an approximation to the filtering distribution

T = length(y);
x = zeros(N, T); % Particles (preallocated to zero)
w = zeros(N, T); % Weights (preallocated to zero)
loglik = 0;  % initialize loglikelihood

for(t = 1:T)
    if t==1 
       x(:,1) = a*x0 + randn(N,1);  % will add x0 to N draws from the standard Gaussian
    else
        ind = resampling(w(:,t-1)); % multinomial resampling of particle indeces
        % propagate forward ONLY selected particles having indeces inside ind 
        x(:,t) = a*x(ind,t-1) + randn(N,1);
    end

    % Compute weights (log-scale for numerical stability) using the
    % (log)density of the observations y ~ N(b*x,0.3^2)
    logweights = -1/2*log(2*pi*0.3^2) - 1/(2*0.3^2)*(y(t) - b*x(:,t)).^2;
    
   % NOTICE: the three commented lines below show how things shuld be PROPERLY done, for numerical stability. But are less readable
   % const = max(logweights); % Subtract below the maximum value for numerical stability
   % weights = exp(logweights-const); % this way it is "less likely" to obtain a numerical underflow/overflow
   % loglik = loglik + const + log(sum(weights)) - log(N);  
    weights = exp(logweights); % unnormalised weights
    loglik = loglik + log(sum(weights)) - log(N); 
    w(:,t) = weights/sum(weights); % Save the normalized weights
end

end