sirdemo1.m

particle filter using SIR

% PURPOSE : To estimate the states of the following nonlinear, 
%           nonstationary state space model:
%           x(t+1) = 0.5x(t) + [25x(t)]/[(1+x(t))^(2)] 
%                    + 8cos(1.2t) + process noise
%           y(t) =  x(t)^(2) / 20  + measurement noise
%           using the sequential importance-samping resampling
%           algorithm.                           
             
% AUTHOR  : Nando de Freitas - Thanks for the acknowledgement :-)
% DATE    : 08-09-98

clear;
echo off;

% INITIALISATION AND PARAMETERS:
% =============================

N =50;                 % Number of time steps.
t = 1:1:N;             % Time.  
x0 = 0.1;              % Initial state.
x = zeros(N,1);        % Hidden states.
y = zeros(N,1);        % Observations.
x(1,1) = x0;           % Initial state. 
R = 1;                 % Measurement noise variance.
Q = 10;                % Process noise variance.      
initVar = 5;           % Initial variance of the states.  
numSamples=500;        % Number of Monte Carlo samples per time step. 

% GENERATE PROCESS AND MEASUREMENT NOISE:
% ======================================

v = randn(N,1);% p(v) 
w = sqrt(10)*randn(N,1); %p(w)
figure(1)
clf;
subplot(221)
plot(v);
ylabel('Measurement Noise','fontsize',15);
xlabel('Time');
subplot(222)
plot(w);
ylabel('Process Noise','fontsize',15);
xlabel('Time');

% GENERATE STATE AND MEASUREMENTS:
% ===============================

y(1,1) = (x(1,1)^(2))./20 + v(1,1); 
for t=2:N,
  x(t,1) = 0.5*x(t-1,1) + 25*x(t-1,1)/(1+x(t-1,1)^(2)) + 8*cos(1.2*(t-1)) + w(t,1);
  y(t,1) = (x(t,1).^(2))./20 + v(t,1); 
end;

figure(1)
subplot(223)
plot(x)
ylabel('State x','fontsize',15);
xlabel('Time','fontsize',15);
subplot(224)
plot(y)
ylabel('Observation y','fontsize',15);
xlabel('Time','fontsize',15);
fprintf('Press a key to continue')
pause;
fprintf('\n')
fprintf('Simulation in progress')
fprintf('\n')

% PERFORM SEQUENTIAL MONTE CARLO FILTERING:
% ========================================

[samples,q] = bootstrap(x,y,R,Q,initVar,numSamples);

% COMPUTE CENTROID, MAP AND VARIANCE ESTIMATES:
% ============================================

% Posterior mean estimate
prediction = mean(samples);

% Posterior peak estimate
bins = 20;
xmap=zeros(N,1);
for t=1:N
  [p,pos]=hist(samples(:,t,1),bins);
  map=find(p==max(p));
  xmap(t,1)=pos(map(1));
end;

% Posterior standard deviation estimate
xstd=std(samples);

% PLOT RESULTS:
% ============

figure(1)
clf;
subplot(221)
plot(1:length(x),x,'g-*',1:length(x),prediction,'r-+',1:length(x),xmap,'m-*')
legend('True value','Posterior mean estimate','MAP estimate');
ylabel('State estimate','fontsize',15)
xlabel('Time','fontsize',15)
subplot(222)
plot(prediction,x,'+')
ylabel('True state','fontsize',15)
xlabel('Posterior mean estimate','fontsize',15)
hold on
c=-25:1:25;
plot(c,c,'r');
axis([-25 25 -25 25]);
hold off
subplot(223)
plot(xmap,x,'+')
ylabel('True state','fontsize',15)
xlabel('MAP estimate','fontsize',15)
hold on
c=-25:1:25;
plot(c,c,'r');
axis([-25 25 -25 25]);
hold off
% Plot histogram:
[rows,c]=size(q);
domain = zeros(rows,1);
range = zeros(rows,1);
parameter = 1;
bins = 10;
support=[-20:1:20];
figure(1)
subplot(224)
hold on
ylabel('Time','fontsize',15)
xlabel('Sample space','fontsize',15)
zlabel('Posterior density','fontsize',15)
v=[0 1];
caxis(v);
for t=1:2:N,
  [range,domain]=hist(samples(:,t,parameter),support);
  waterfall((domain),t,range)
end;