粒⼦滤波代码详解-particlefilter与AMCL代码粒⼦滤波代码详解
构建粒⼦和权重
创建NP个粒⼦和粒⼦的权重并初始化
px = np.s((4, NP)))# Particle store
pw = np.s((1, NP)))+1.0/ NP # Particle weight
更新粒⼦的权重
def pf_localization(px, pw, xEst, PEst, z, u):
崇高近义词"""
Localization with Particle filter
"""
#z 绝对真实位置测量得出信标的位置
for ip in range(NP):
#获取该粒⼦
x = px[:, ip]
#获取该粒⼦的权重
w = pw[0, ip]
# Predict with ramdom input sampling
ud1 = u[0,0]+ np.random.randn()* Rsim[0,0]
ud2 = u[1,0]+ np.random.randn()* Rsim[1,1]
ud = np.matrix([ud1, ud2]).T
#更新该粒⼦的位置
x = motion_model(x, ud)
# Calc Inportance Weight
for i in range(len(z[:,0])):
dx = x[0,0]- z[i,1]
dy = x[1,0]- z[i,2]
prez = math.sqrt(dx**2+ dy**2)
#该粒⼦和信标之间的位置误差和真实位置得出的位置误差
dz = prez - z[i,0]
#使⽤激光的模型来计算权重计算该粒⼦的权重直接使⽤⾼斯模型来更新权重做累积求和
w = w * gauss_likelihood(dz, math.sqrt(Q[0,0]))
#更新该粒⼦在粒⼦群存储变量中的值
px[:, ip]= x
pw[0, ip]= w
#均⼀化权重
#print("srg")
pw = pw / pw.sum()# normalize
#print(px)
#print(pw.T)
xEst = px * pw.T
print(xEst)
#a=input()
PEst = calc_covariance(xEst, px, pw)和老师谈恋爱
px, pw = resampling(px, pw)
return xEst, PEst, px, pw
重采样
def resampling(px, pw):
"""
low variance re-sampling
"""
#print(pw)
#print('neff')
Neff =1.0/(pw * pw.T)[0,0]# Effective particle number
if Neff < NTh:
'''
这⾥是⾃⼰根据AMCL 源代码实现的重采样的写法下⾯屏蔽的是原来的写法原理如下
重采样的核⼼原理，⾮常简单
1，假如有5个粒⼦，各个粒⼦权重归⼀化以后
[0.05 0.05 0.4 0.4 0.1]
2，对粒⼦累积求和前⾯并补0值得到以下数组
[0.0 0.05 0.1 0.5 0.9 1.0]
3，现在在0到1之间⽣成5次随机数
0.35，0.56,0.89,0.016,0.28
4，根据这个规则
if r>wcum[0,ind] and r <wcum[0,ind+1] 就可以筛选出来所需要的粒⼦，粒⼦可能被重复筛选出来，总⽽⾔之，权重⼤的粒⼦被选出来的概率⼤ 0.35-->第2个粒⼦被选出权重 0.4
0.56-->第3个粒⼦被选出权重 0.4
0.89-->第3个粒⼦被选出权重 0.4
0.016-->第0个粒⼦被选出权重 0.05
0.35-->第2个粒⼦被选出权重 0.4
'''
#求累积权重和对应AMCL 具体代码在AMCL pf.c pf_update_resample 函数⾥⾯
wcum = np.cumsum(pw)
#前⾯补⼀个0值
zz=np.matrix(np.array([0.0]))
wcum = np.hstack((zz,wcum))
inds =[]
for ip in range(NP):
请示报告
#因为权重已经归⼀化所以随机选择0到1以内的随机数
r=np.random.rand(1)
#print(r)
ind=0
while ind < wcum.shape[1]-1:
#print (ind) 寻找满⾜该随机数的粒⼦
if r>wcum[0,ind]and r <wcum[0,ind+1]:
inds.append(ind)
ind = ind +1
'''
ba = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
resampleid = ba + np.random.rand(ba.shape[1]) / NP
inds = []
ind = 0
for ip in range(NP):
while resampleid[0, ip] > wcum[0, ind]:
ind += 1
inds.append(ind)
'''
px = px[:, inds]
pw = np.s((1, NP)))+1.0/ NP # init weight
return px, pw
AMCL具体代码对⽐如下
//求累积和步骤
// Build up cumulative probability table for resampling.得到⼀个t_a samples 的权重 // TODO: Replace this with a more efficient procedure
// (e.g., uk/docs/gslref/GeneralDiscreteDistributions.html) c = (double*)malloc(sizeof(double)*(t_a->sample_count+1));
//将权重累积求和
c[0] = 0.0;
for(i=0;i<t_a->sample_count;i++)
c[i+1] = c[i]+t_a->samples[i].weight;
//选取粒⼦步骤
// Naive discrete event sampler
double r;
//随机⽣成0到1之间的⼀个数字随机采样
r = drand48();
for(i=0;i<t_a->sample_count;i++)
{
//c[i] 为 sample_a->weight 的weight
if((c[i] <= r) && (r < c[i+1]))
break;
}
asrt(i<t_a->sample_count);
//随机选择出来该粒⼦害怕的图片
sample_a = t_a->samples + i;
asrt(sample_a->weight > 0);
// Add sample to list 将sample_a⾥⾯的po 更新给sample_b
帅的网名sample_b->po = sample_a->po;
#coding = utf8
"""
Particle Filter localization sample
author: Atsushi Sakai (@Atsushi_twi)
"""
import numpy as np
import math
import matplotlib.pyplot as plt
# Estimation parameter of PF
Q = np.diag([0.1])**2 # range error
R = np.diag([1.0, math.radians(40.0)])**2 # input error
# Simulation parameter
Qsim = np.diag([0.2])**2
Rsim = np.diag([1.0, math.radians(30.0)])**2
DT =0.1# time tick [s]
SIM_TIME =50.0# simulation time [s]
MAX_RANGE =20.0# maximum obrvation range
# Particle filter parameter
NP =100# Number of Particle 粒⼦总数
NTh = NP / 2.0# Number of particle for re-sampling
show_animation = True
def calc_input():
v=1.0# [m/s]
yawrate =0.1# [rad/s]
u = np.matrix([v, yawrate]).T
return u
def obrvation(xTrue, xd, u, RFID):
xTrue = motion_model(xTrue, u)
# add noi to gps x-y
z = np.s((0,3)))
for i in range(len(RFID[:, 0])):
dx = xTrue[0, 0] - RFID[i, 0]
dy = xTrue[1, 0] - RFID[i, 1]
d = math.sqrt(dx**2 + dy**2)
if d <= MAX_RANGE:
dn = d + np.random.randn() * Qsim[0, 0]# add noi zi = np.matrix([dn, RFID[i, 0], RFID[i, 1]])
z = np.vstack((z, zi))
# add noi to input
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
ud = np.matrix([ud1, ud2]).T
xd = motion_model(xd, ud)
return xTrue, z, xd, ud
def motion_model(x, u):
F = np.matrix([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
[0, 0, 0, 0]])
B = np.matrix([[DT * s(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT],
[1.0, 0.0]])
x = F * x + B * u
return x
def gauss_likelihood(x, sigma):
p =1.0 / math.sqrt(2.0 * math.pi * sigma ** 2) * \
return p
def calc_covariance(xEst, px, pw):
cov = np.s((3,3)))
for i in range(px.shape[1]):
dx =(px[:, i] - xEst)[0:3]
cov += pw[0, i] * dx * dx.T
return cov
def pf_localization(px, pw, xEst, PEst, z, u):
"""
Localization with Particle filter
"""
#z 绝对真实位置测量得出信标的位置
for ip in range(NP):
#获取该粒⼦
x = px[:, ip]
#获取该粒⼦的权重
w = pw[0, ip]
# Predict with ramdom input sampling
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
美国感恩节ud = np.matrix([ud1, ud2]).T
#更新该粒⼦的位置
x = motion_model(x, ud)
# Calc Inportance Weight
for i in range(len(z[:, 0])):
dx = x[0, 0] - z[i, 1]
dy = x[1, 0] - z[i, 2]
prez = math.sqrt(dx**2 + dy**2)
#该粒⼦和信标之间的位置误差和真实位置得出的位置误差
dz = prez - z[i, 0]
#使⽤激光的模型来计算权重计算该粒⼦的权重直接使⽤⾼斯模型来更新权重做累积求和 w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))
#更新该粒⼦在粒⼦群存储变量中的值
px[:, ip]= x
pw[0, ip]= w
#均⼀化权重
#print("srg")
pw = pw / pw.sum()# normalize
#print(px)
#print(pw.T)
xEst = px * pw.T
print(xEst)
#a=input()
PEst = calc_covariance(xEst, px, pw)
px, pw = resampling(px, pw)
return xEst, PEst, px, pw
def resampling(px, pw):
"""
low variance re-sampling 低⽅差重采样
"""
Neff =1.0 / (pw * pw.T)[0, 0]# Effective particle number
if Neff < NTh:
北京有什么山#这⾥得到累计概率和
wcum = np.cumsum(pw)一枝花蔡庆
print("wcum")
print(wcum)
#这⾥得到⼀个基准该概率
ba = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
print(ba)
#ba.shape[1] 数组的⼤⼩ np.random.rand 随机分布基准概率再加上后验⾼斯分布概率
resampleid = ba + np.random.rand(ba.shape[1]) / NP
print(resampleid)
