粒子滤波和蒙特卡洛定位

算法主要过程如下：

1.根据观测更新粒子权重

2.根据权重resample，也就是根据权重更新所有粒子位置，并使得所有粒子权重恢复到一样。

3.利用一个模型让所有粒子随着robot的移动而移动。

无公式解释粒子滤波（需要FQ） ： https://www.youtube.com/watch?v=aUkBa1zMKv4

蒙特卡洛定位（继续FQ）：https://www.youtube.com/watch?v=eAqAFSrTGGY

其中无公式解释粒子滤波的MATLAB开源代码如下：

function [] = PF ()
% Just call PF (without any arguments) to run the animation
% 
% This is the matlab code behind the movie "Particle Filter Explained
% without Equations", which can be found at http://youtu.be/aUkBa1zMKv4
% Written by Andreas Svensson, October 2013
% Updated by Andreas Svensson, February 2013, fixing a coding error in the
% 'propagation-update' of the weights
% andreas.svensson@it.uu.se
% http://www.it.uu.se/katalog/andsv164
% 
% The code is provided as is, and I take no responsibility for what this
% code may do to you, your computer or someone else.
%
% This code is licensed under a
% Creative Commons Attribution-ShareAlike 3.0 Unported License.
% http://creativecommons.org/licenses/by-sa/3.0/

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Setup and initialization %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Setting the random seed, so the same example can be run several times
s = RandStream('mt19937ar','Seed',1);
RandStream.setGlobalStream(s);

% Some unceratinty parameters
measurementNoiseStdev = 0.1; speedStdev = 1;

% Speed of the aircraft
speed = 1;
% Set starting position of aircraft
planePosX = -25; planePosY = 4;

% Some parameters for plotting the particles
m = 1000; k = 0.0001;

% Number of particles
N = 200;

% Some variables for plotting
plotVectorSea = -10:0.01:10;
plotVectorMountains = [-40:0.01:-10.01, 10.01:0.01:40];
plotHeight = 5;

% The function describing the ground
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))+...
    (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));

% Plot the environment
area(plotVectorMountains,ground(plotVectorMountains),-1,'FaceColor',[0 0.6 0])
set(gca,'XTick',[]); set(gca,'YTick',[]); hold on
area(plotVectorSea,ground(plotVectorSea),-1,'FaceColor',[0 0 0.8]); axis([-40 40 -1 10])
plane = plotPlane(planePosX,planePosY,1);
measurementLine = line([planePosX planePosX],[ground(planePosX),planePosY],'Color',[1 0 0],'LineStyle',':');
pause(1)


%%%%%%%%%%%%%%%%%%%%%%%
%%% Begin filtering %%%
%%%%%%%%%%%%%%%%%%%%%%%

% Generate particles
particles = rand(N,1)*80-40;

% Plot particles
particleHandle = scatter(particles,plotHeight(ones(size(particles))),m*(1/N*ones(N,1)+k),'k','filled');
pause(1)

FirstRun = 1;

% Initialize particle weights
w = 1/N*ones(N,1);

for t = 1:60
    % Generate height measurements (with gaussian measurement noise)
    planeMeasDist = planePosY - ground(planePosX) + randn*measurementNoiseStdev;
    
    % Evaluate measurements (i.e., create weights) using the pdf for the normal distribution
    w = w.*(1/(sqrt(2*pi)*measurementNoiseStdev)*exp(-((planePosY-ground(particles))-planeMeasDist).^2/(2*measurementNoiseStdev^2)));
    
    % Normalize particle weigths
    w = w/sum(w);

    if FirstRun
        
        % Sort out some particles to evaluate them "in public" the first
        % run (as in the movie)
        [~, order] = sort(w,'descend');
        pmax = order(1);
        pmaxi = setdiff(1:N,pmax);
        delete(particleHandle)
        particleHandle = scatter([particles(pmaxi);particles(pmax)],plotHeight(ones(size(particles))),m*([ones(N-1,1)/N;w(pmax)]+k),'k','filled');
        pause(1)
        
        pmax2 = order(2);
        pmaxi2 = setdiff(pmaxi,pmax2);
        delete(particleHandle)
        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');
        pause(1)
        
        % Plot all weighted particles    
        delete(particleHandle)
        particleHandle = scatter(particles,plotHeight(ones(size(particles))),m*(w+k),'k','filled');
        pause(1)
    end

    % Resample the particles
    u = rand(N,1); wc = cumsum(w);
    [~,ind1] = sort([u;wc]); ind=find(ind1<=N)-(0:N-1)';
    particles=particles(ind,:); w=ones(N,1)./N;

    delete(particleHandle);
    particleHandle = scatter(particles,plotHeight(ones(size(particles))),m*(w+k),'k','filled');
    pause(1)

    % Time propagation
    speedNoise = speedStdev*randn(size(particles));
    particles = particles + speed + speedNoise;
    
    % Update weights
    % w = w, since the update in the previous step is done using our motion model, so the
    % information is already contained in that update.
    
    % Move and plot moved aircraft
    planePosX = planePosX + speed;
    delete(plane); delete(measurementLine)
    plane = plotPlane(planePosX,planePosY,1);
    measurementLine = line([planePosX planePosX],[ground(planePosX),planePosY],'Color',[1 0 0],'LineStyle',':');
    
    if FirstRun
        % Plot updated particles
        delete(particleHandle)
        particleHandle = scatter(particles,plotHeight(ones(size(particles))),m*(w+k),'k','filled');
        pause(1)
    end

    FirstRun = 0;
end
end

function [ h ] = plotPlane( xpos,ypos,fignr )
figure(fignr)

X = xpos - 0.6 + [-1,     -0.1,   -0.09,    0.3,  0.7, 0.8, 0.7, 0.3, -0.09,  -0.1, -1];
Y = ypos + [-0.05, -0.05, -0.4, -0.05, -0.05,0 0.05, 0.05, 0.4, 0.05, 0.05];
h = fill(X,Y,'k');
end