粒子滤波定位算法的**也是贝叶斯规则：
以下的解析是结合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重新采样粒子
% 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% 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% Gauss function
function g = Gaussian(x,u,sigma)
g=exp(-((u-x)^2)/(sigma^2)/2.0)/sqrt(2.0*pi*(sigma^2));
end% Normalization
function pw=Normalization(pw,NP)
pw=pw/sum(pw);
endfunction [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| NP | 效果 | 运行时间 |
|---|---|---|
| 3 |  |
0.220 |
| 5 | ! |
0.286 |
| 10 |  |
0.387 |
| 25 |  |
0.640 |
| 50 |  |
1.131 |
| 100 |  |
2.159 |
可见运行时间与NP数成正比关系
可见误差趋向于0.5，这主要时由高斯噪声造成的
粒子滤波算法是基于概率的定位算法，主要有以下优点：
- 理解简单，一句话就是越相似，存活概率越大
- 计算量不大（计算量与粒子数线性相关）
但也存在以下问题：
- 严重依赖于对初始状态的估计，选择不当可能发散
- 需要有固定的路标
https://en.wikipedia.org/wiki/Particle_filter
["Probabilistic Robotics"][Sebastian Thrun]
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent