1. 粒子滤波算法

2. 1 粒子滤波算法过程解析

粒子滤波定位算法的思想也是贝叶斯规则：

$$
\frac{\text { Likelihood * Prior }}{\text { Marginal }}
$$
以下的解析是结合Matlab代码所作的说明

初始化
初始化一堆粒子，在MatLab中的代码显示NP=50，也就是总共有50个采样粒子。Estimated State [x y yaw]，可见每个状态有3个数据。px=repmat(xEst,1,NP);，
计算初始权重pw=zeros(1,NP)+1/NP; 可见初始权重是均匀的
初始化路标，landMarks=[10 0; 10 10; 0 15; -5 20]; 可见一共有4个路标
预测：根据motion model与物体的控制信息u预测下个时刻粒子群中粒子的位置
doControl()函数，输入参数time，得到控制指令u
doMotion()函数，输入初始状态x和控制指令u，得到下一时刻的x状态
doObservation()函数，输入参数xGnd（没有噪声的里程计位置估计）, xOdom（有噪声的里程计位置估计）, u（控制指令）, landMarks, MAX_RANGE，输出参数是z,xGnd,xOdom,u
更新：根据物体的观测值z与地图值zl计算出每个粒子的权重ww。更新粒子权重的依据是粒子的观测值与地图标志物相似度的高低，越高的话该粒子的权重越大
对每个粒子循环操作
doMotion()函数，对每个采样粒子输入初始状态x和控制指令u，得到下一时刻的x状态，并加入干扰
计算权重，用各个路标距离的高斯概率相乘得到总概率
重采样：根据粒子的权重w重新采样粒子

3. 2 重要代码（Tasks）

3.1. 2.1 观测模型

% do Observation model
function [z, xGnd, xOdom, u] = doObservation(xGnd, xOdom, u, landMarks, MAX_RANGE)
global Qsigma;
global Rsigma;

% Gnd Truth and Odometry
xGnd=doMotion(xGnd, u);% Ground Truth 理想状态
u=u+sqrt(Qsigma)*randn(2,1); % add noise randomly
xOdom=doMotion(xOdom, u); % odometry only

%Simulate Observation
z=[];
for iz=1:length(landMarks(:,1))
dx = xGnd(1)-landMarks(iz,1);
dy = xGnd(2)-landMarks(iz,2);
d=sqrt(dx^2+dy^2);
if d<MAX_RANGE
z=[z;[d+sqrt(Rsigma)*randn(1,1) landMarks(iz,:)]];   % add observation noise randomly
end
end
end

3.2. 2.2 运动模型

% do Motion Model
function [ x ] = doMotion( x, u)
global dt;
Delta = [ [dt*cos(x(3)),0];
[dt*sin(x(3)),0];
[0,dt]];

x = x+Delta*u;
end

3.3. 2.3 高斯函数

% Gauss function
function g = Gaussian(x,u,sigma)
g=exp(-((u-x)^2)/(sigma^2)/2.0)/sqrt(2.0*pi*(sigma^2));
end

3.4. 2.4 粒子归一化

% Normalization
function pw=Normalization(pw,NP)
pw=pw/sum(pw);

end

3.5. 2.5 重采样

function [px,pw]=ResamplingStep(px,pw,NTh,NP)
ww=pw(1);
for iw=2:NP
ww=[ww,ww(end)+pw(iw)];
end
pw1=[]
pp=[];
for i=1:NP
r=rand();
for j=1:NP
if ww(j)>r
pp=[pp,px(:,j)];
pw1=[pw1,pw(:,j)]
break
end
end
end
px=pp;
pw=pw1;
end