1 function [] = PF ()
  2 % Just call PF (without any arguments) to run the animation
  3 %
  4 % This is the matlab code behind the movie "Particle Filter Explained
  5 % without Equations", which can be found at http://youtu.be/aUkBa1zMKv4
  6 % Written by Andreas Svensson, October 2013
  7 % Updated by Andreas Svensson, February 2013, fixing a coding error in the
  8 % 'propagation-update' of the weights
  9 % andreas.svensson@it.uu.se
 10 % http://www.it.uu.se/katalog/andsv164
 11 %
 12 % The code is provided as is, and I take no responsibility for what this
 13 % code may do to you, your computer or someone else.
 14 %
 15 % This code is licensed under a
 16 % Creative Commons Attribution-ShareAlike 3.0 Unported License.
 17 % http://creativecommons.org/licenses/by-sa/3.0/
 18
 19 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 20 %%% Setup and initialization %%%
 21 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 22
 23 % Setting the random seed, so the same example can be run several times
 24 s = RandStream('mt19937ar','Seed',1);
 25 RandStream.setGlobalStream(s);
 26
 27 % Some unceratinty parameters
 28 measurementNoiseStdev = 0.1; speedStdev = 1;
 29
 30 % Speed of the aircraft
 31 speed = 1;
 32 % Set starting position of aircraft
 33 planePosX = -25; planePosY = 4;
 34
 35 % Some parameters for plotting the particles
 36 m = 1000; k = 0.0001;
 37
 38 % Number of particles
 39 N = 200;
 40
 41 % Some variables for plotting
 42 plotVectorSea = -10:0.01:10;
 43 plotVectorMountains = [-40:0.01:-10.01, 10.01:0.01:40];
 44 plotHeight = 5;
 45
 46 % The function describing the ground
 47 ground = @(x) (x>=10).*((1-(x-10)/30).*sin(x-10)+((x-10)/30).*sin(1.5*(x-10))+0.2.*(x-10).*(x<=20)+2*(x>20))+...
 48     (x<=-10).*((1-(-x-10)/30).*sin(-x-10)+((-x-10)/30).*sin(1.5*(-x-10))+0.2.*(-x-10).*(x>=-20)+2*(x<-20));
 49
 50 % Plot the environment
 51 area(plotVectorMountains,ground(plotVectorMountains),-1,'FaceColor',[0 0.6 0])
 52 set(gca,'XTick',[]); set(gca,'YTick',[]); hold on
 53 area(plotVectorSea,ground(plotVectorSea),-1,'FaceColor',[0 0 0.8]); axis([-40 40 -1 10])
 54 plane = plotPlane(planePosX,planePosY,1);
 55 measurementLine = line([planePosX planePosX],[ground(planePosX),planePosY],'Color',[1 0 0],'LineStyle',':');
 56 pause(1)
 57
 58
 59 %%%%%%%%%%%%%%%%%%%%%%%
 60 %%% Begin filtering %%%
 61 %%%%%%%%%%%%%%%%%%%%%%%
 62
 63 % Generate particles
 64 particles = rand(N,1)*80-40;
 65
 66 % Plot particles
 67 particleHandle = scatter(particles,plotHeight(ones(size(particles))),m*(1/N*ones(N,1)+k),'k','filled');
 68 pause(1)
 69
 70 FirstRun = 1;
 71
 72 % Initialize particle weights
 73 w = 1/N*ones(N,1);
 74
 75 for t = 1:60
 76     % Generate height measurements (with gaussian measurement noise)
 77     planeMeasDist = planePosY - ground(planePosX) + randn*measurementNoiseStdev;
 78
 79     % Evaluate measurements (i.e., create weights) using the pdf for the normal distribution
 80     w = w.*(1/(sqrt(2*pi)*measurementNoiseStdev)*exp(-((planePosY-ground(particles))-planeMeasDist).^2/(2*measurementNoiseStdev^2)));
 81
 82     % Normalize particle weigths
 83     w = w/sum(w);
 84
 85     if FirstRun
 86
 87         % Sort out some particles to evaluate them "in public" the first
 88         % run (as in the movie)
 89         [~, order] = sort(w,'descend');
 90         pmax = order(1);
 91         pmaxi = setdiff(1:N,pmax);
 92         delete(particleHandle)
 93         particleHandle = scatter([particles(pmaxi);particles(pmax)],plotHeight(ones(size(particles))),m*([ones(N-1,1)/N;w(pmax)]+k),'k','filled');
 94         pause(1)
 95
 96         pmax2 = order(2);
 97         pmaxi2 = setdiff(pmaxi,pmax2);
 98         delete(particleHandle)
 99         particleHandle = scatter([particles(pmaxi2);particles(pmax);particles(pmax2)],plotHeight(ones(size(particles))),m*([ones(N-2,1)/N;w(pmax);w(pmax2)]+k),'k','filled');
100         pause(1)
101
102         % Plot all weighted particles
103         delete(particleHandle)
104         particleHandle = scatter(particles,plotHeight(ones(size(particles))),m*(w+k),'k','filled');
105         pause(1)
106     end
107
108     % Resample the particles
109     u = rand(N,1); wc = cumsum(w);
110     [~,ind1] = sort([u;wc]); ind=find(ind1<=N)-(0:N-1)';
111     particles=particles(ind,:); w=ones(N,1)./N;
112
113     delete(particleHandle);
114     particleHandle = scatter(particles,plotHeight(ones(size(particles))),m*(w+k),'k','filled');
115     pause(1)
116
117     % Time propagation
118     speedNoise = speedStdev*randn(size(particles));
119     particles = particles + speed + speedNoise;
120
121     % Update weights
122     % w = w, since the update in the previous step is done using our motion model, so the
123     % information is already contained in that update.
124
125     % Move and plot moved aircraft
126     planePosX = planePosX + speed;
127     delete(plane); delete(measurementLine)
128     plane = plotPlane(planePosX,planePosY,1);
129     measurementLine = line([planePosX planePosX],[ground(planePosX),planePosY],'Color',[1 0 0],'LineStyle',':');
130
131     if FirstRun
132         % Plot updated particles
133         delete(particleHandle)
134         particleHandle = scatter(particles,plotHeight(ones(size(particles))),m*(w+k),'k','filled');
135         pause(1)
136     end
137
138     FirstRun = 0;
139 end
140 end
141
142 function [ h ] = plotPlane( xpos,ypos,fignr )
143 figure(fignr)
144
145 X = xpos - 0.6 + [-1,     -0.1,   -0.09,    0.3,  0.7, 0.8, 0.7, 0.3, -0.09,  -0.1, -1];
146 Y = ypos + [-0.05, -0.05, -0.4, -0.05, -0.05,0 0.05, 0.05, 0.4, 0.05, 0.05];
147 h = fill(X,Y,'k');
148 end