蒙特卡洛粒子滤波定位算法_粒子滤波和蒙特卡洛定位

1 function [] =PF ()2 %Just call PF (without any arguments) to run the animation3 %

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 inthe8 % 'propagation-update'of the weights9 %andreas.svensson@it.uu.se10 % 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 islicensed under a16 % Creative Commons Attribution-ShareAlike 3.0Unported 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 times24 s = RandStream('mt19937ar','Seed',1);25 RandStream.setGlobalStream(s);26

27 %Some unceratinty parameters28 measurementNoiseStdev = 0.1; speedStdev = 1;29

30 %Speed of the aircraft31 speed = 1;32 %Set starting position of aircraft33 planePosX = -25; planePosY = 4;34

35 % Some parameters forplotting the particles36 m = 1000; k = 0.0001;37

38 %Number of particles39 N = 200;40

41 % Some variables forplotting42 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 ground47 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

50 %Plot the environment51 area(plotVectorMountains,ground(plotVectorMountains),-1,'FaceColor',[0 0.6 0])52 set(gca,'XTick',[]); set(gca,'YTick',[]); hold on53 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 particles64 particles = rand(N,1)*80-40;65

66 %Plot particles67 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 weights73 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 forthe normal distribution80 w = w.*(1/(sqrt(2*pi)*measurementNoiseStdev)*exp(-((planePosY-ground(particles))-planeMeasDist).^2/(2*measurementNoiseStdev^2)));81

82 %Normalize particle weigths83 w = w/sum(w);84

85 ifFirstRun86

87 % Sort out some particles to evaluate them "in public"the first88 % run (as inthe 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 particles103 delete(particleHandle)104 particleHandle = scatter(particles,plotHeight(ones(size(particles))),m*(w+k),'k','filled');105 pause(1)106 end107

108 %Resample the particles109 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 propagation118 speedNoise = speedStdev*randn(size(particles));119 particles = particles + speed +speedNoise;120

121 %Update weights122 % w = w, since the update in the previous step is done usingour motion model, so the123 % information is already contained inthat update.124

125 %Move and plot moved aircraft126 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 ifFirstRun132 %Plot updated particles133 delete(particleHandle)134 particleHandle = scatter(particles,plotHeight(ones(size(particles))),m*(w+k),'k','filled');135 pause(1)136 end137

138 FirstRun = 0;139 end140 end141

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