% PURPOSE : We address here a nonlinear non-Gaussian problem using
% the standard particle filtering algorithm.

clear;
echo off;

% INITIALISATION AND PARAMETERS:
% ==============================
sigmaT =  10;               % Variance of the Gaussian transition prior.
sigmaM =  1;                % Variance of the Gaussian likelihood.
N = 500;                    % Number of particles.
T = 100;                    % Number of time steps.

% LOAD THE DATA:
% =============

load trackingData;

% PLOT THE DATA:
% =============
figure(1)
clf;
plot(1:T,x,'r',1:T,y,'b');
ylabel('Data','fontsize',15);
xlabel('Time','fontsize',15);
legend('unknown state x','known observation y');

fprintf('\n')
fprintf('Press a key to continue')  
pause;
fprintf('\n')
fprintf('\n')
fprintf('Training has started')
fprintf('\n')

%%%%%%%%%%%%%%%    PERFORM PARTICLE FILTERING    %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%  ==============================  %%%%%%%%%%%%%%%%%%%%%

% INITIALISATION:
% ==============
xparticle = sqrt(sigmaT)*randn(T,N);  % These are the particles for the estimate
                                      % of x. Note that there's no need to store
                                      % them for all t. We're only doing this to
                                      % show you all the nice plots at the end.
xparticlePred = ones(T,N);            % One-step-ahead predicted values of the states.
yPred = ones(T,N);                    % One-step-ahead predicted values of y.
w = ones(T,N);                        % Importance weights.

for t=2:T,    
  fprintf('Time steps: t = %i / %i  \r',t,T);
  fprintf('\n')
  
  % PREDICTION STEP:
  % ================ 
  % We use the transition prior as proposal.
  for i=1:N,
    noise = sqrt(sigmaT)*randn(1,1);   
    xparticlePred(t,i) = 0.5*xparticle(t-1,i) + 25*xparticle(t-1,i)/(1+xparticle(t-1,i)^(2))+ 8*cos(1.2*t) + noise;   
  end;

  % EVALUATE IMPORTANCE WEIGHTS:
  % ============================
  % For our choice of proposal, the importance weights are give by:  
  yPred(t,:) = (xparticlePred(t,:).^(2))./20;
  for i=1:N,
    w(t,i) = exp(-0.5*inv(sigmaM)*(y(t)-yPred(t,i))^(2));
  end;  
  w(t,:) = w(t,:)./sum(w(t,:));                % Normalise the weights.
  
  % SELECTION STEP:
  % ===============
  outIndex = deterministicR(1:N,w(t,:)');      
  xparticle(t,:) = xparticlePred(t,outIndex); % Keep particles with
                                              % resampled indices.
end; 


% PLOT SOME OF THE RESULTS:
% ========================
figure(1)
clf;
ymean = mean(yPred');
plot(1:T,x,'b--',1:T,mean(xparticle(:,:)'),'r','lineWidth',2);
legend('x','Estimated x (posterior mean)');
xlabel('Time','fontsize',15)


% Plot distribution of x:
figure(3)
clf;
domain = zeros(T,1);
range = zeros(T,1);
support=[-25:1:25];
hold on;
ylabel('Time (t)','fontsize',15)
xlabel('x_t','fontsize',15)
zlabel('p(x_t|y_{1:t})','fontsize',15)
for t=8:8:T,
  [range,domain]=hist(xparticle(t,:),support);
  waterfall(domain,t,range/sum(range))
end;
view(17,60);
rotate3d on;
get(gca);
set(gca,'ygrid','off');














