% this is a demo for the sequential importance sampling particle filter (SIS)

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 sequential importance sampling particle filter with N particles, parameters a and b and
% initial condition x0

% lets' try some parameter values with the SIS particle filter (pf_sis is
% a function defined further down!)
% start with the true ones a=1,b=1
a=1; b=1;
loglik = pf_sis(yobs, a, b, x0, N)  % notice pf_sis is a function defined further down

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

% try some values further away from the truth
a=1.3; b=0.7;
loglik = pf_sis(yobs, a, b, x0, N) % much smaller value

% fix "b" to truth and lets make "a" vary on a grid of values [0.2,0.3,0.4,...,2]
b=1; a_grid = [0.2:.1:2];
% the following it is just to produce plots
numattempts = 10;  % you can set numattempts to 1 for a single run
for run=1:numattempts
   loglik_grid = zeros(1,length(a_grid)); % store here all loglikelihoods for the different "a" values
   for ii=1:length(a_grid)
      loglik_grid(ii) = pf_sis(yobs, a_grid(ii), b, x0, N); 
   end
   plot(a_grid,loglik_grid)
   xlabel('a')
   title('loglik for several values of a (and b is kept fixed to 1)')
   hold on
end

%::::::: HERE FOLLOWS THE SIS PARTICLE FILTER :::::::::::::::::::::::

function loglik = pf_sis(y, a, b, x0, N)
% SIS 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

T = length(y);
x = zeros(N, T); % N x T matrix of particles (preallocated to zero)
w = zeros(N, T); % N x T matrix of normalised weights (preallocated to zero)
w_init = 1/N * ones(N,1); % N x 1 vector of starting normalised weights, a column vector [1/N,1/N,...,1/N]'
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
        x(:,t) = a*x(:,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) 
    if t==1  
        logweights = -1/2*log(2*pi*0.3^2) - 1/(2*0.3^2)*(y(t) - b*x(:,t)).^2 + log(w_init);
    else
        logweights = -1/2*log(2*pi*0.3^2) - 1/(2*0.3^2)*(y(t) - b*x(:,t)).^2 + log(w(:,t-1));
    end
   % NOTICE: the seven commented lines below show how things should 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
%    if t==1
%        loglik = loglik + log(sum(weights.*w_init));  % in Matlab .* is elementwise multiplication
%    else
%        loglik = loglik + log(sum(weights.*w(:,t-1)));   
%    end
%    w(:,t) = weights/sum(weights); % normalized weights
   
    weights = exp(logweights);  % unnormalised weights
    if t==1
        loglik = loglik + log(sum(weights.*w_init));  % in Matlab .* is elementwise multiplication
    else
        loglik = loglik + log(sum(weights.*w(:,t-1)));   
    end
    w(:,t) = weights/sum(weights); % normalised weights
end

end