粒子滤波代码详解-particle filter 与 AMCL 代码
粒子滤波代码详解
构建粒子和权重
创建NP个粒子和粒子的权重并初始化
px = np.matrix(np.zeros((4, NP))) # Particle store
pw = np.matrix(np.zeros((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 第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
'''
base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
resampleid = base + np.random.rand(base.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.matrix(np.zeros((1, NP))) + 1.0 / NP # init weight
return px, pw
AMCL具体代码对比如下
//求累积和步骤
// Build up cumulative probability table for resampling.得到一个set_a samples 的权重
// TODO: Replace this with a more efficient procedure
// (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html)
c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));
//将权重累积求和
c[0] = 0.0;
for(i=0;isample_count;i++)
c[i+1] = c[i]+set_a->samples[i].weight;
//选取粒子步骤
// Naive discrete event sampler
double r;
//随机生成0到1之间的一个数字 随机采样
r = drand48();
for(i=0;isample_count;i++)
{
//c[i] 为 sample_a->weight 的weight
if((c[i] <= r) && (r < c[i+1]))
break;
}
assert(isample_count);
//随机选择出来该粒子
sample_a = set_a->samples + i;
assert(sample_a->weight > 0);
// Add sample to list 将sample_a里面的pose 更新给sample_b
sample_b->pose = sample_a->pose;
#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 observation 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 observation(xTrue, xd, u, RFID):
xTrue = motion_model(xTrue, u)
# add noise to gps x-y
z = np.matrix(np.zeros((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 noise
zi = np.matrix([dn, RFID[i, 0], RFID[i, 1]])
z = np.vstack((z, zi))
# add noise 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 * math.cos(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) * \
math.exp(-x ** 2 / (2 * sigma ** 2))
return p
def calc_covariance(xEst, px, pw):
cov = np.matrix(np.zeros((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)
#这里得到一个基准该概率
base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
print(base)
#base.shape[1] 数组的大小 np.random.rand 随机分布 基准概率再加上后验高斯分布概率
resampleid = base + np.random.rand(base.shape[1]) / NP
print(resampleid)
inds = []
ind = 0
#采样NP个粒子 旋转托盘采样 用累加和来选择大权重的粒子
#举个例子 目前 基准base 0.1 0.2 0.3 0.4 0.5 0.6 wcum 0.1，0.5 ，0.55,0.6,0.8，那么抽样更多的则是第二个粒子将被选中
for ip in range(NP):
while resampleid[0, ip] > wcum[0, ind]:
print(resampleid[0, ip])
#如果该采样累计概率大于权重累计概率 则接着寻找
ind += 1
#如果该采样累计概率小于权重累计概率 则选择该粒子
inds.append(ind)
#选择出来新的粒子
px = px[:, inds]
print(px)
input()
pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP # init weight
return px, pw
def plot_covariance_ellipse(xEst, PEst):
Pxy = PEst[0:2, 0:2]
eigval, eigvec = np.linalg.eig(Pxy)
if eigval[0] >= eigval[1]:
bigind = 0
smallind = 1
else:
bigind = 1
smallind = 0
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
#eigval[bigind] or eiqval[smallind] were occassionally negative numbers extremely
#close to 0 (~10^-20), catch these cases and set the respective variable to 0
try: a = math.sqrt(eigval[bigind])
except ValueError: a = 0
try: b = math.sqrt(eigval[smallind])
except ValueError: b = 0
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
R = np.matrix([[math.cos(angle), math.sin(angle)],
[-math.sin(angle), math.cos(angle)]])
fx = R * np.matrix([x, y])
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
py = np.array(fx[1, :] + xEst[1, 0]).flatten()
plt.plot(px, py, "--r")
def main():
print(__file__ + " start!!")
time = 0.0
# RFID positions [x, y]
RFID = np.array([[10.0, 0.0],
[10.0, 10.0],
[0.0, 15.0],
[-5.0, 20.0]])
# State Vector [x y yaw v]'
xEst = np.matrix(np.zeros((4, 1)))
xTrue = np.matrix(np.zeros((4, 1)))
PEst = np.eye(4)
px = np.matrix(np.zeros((4, NP))) # Particle store
pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP # Particle weight
print(px)
print(pw)
xDR = np.matrix(np.zeros((4, 1))) # Dead reckoning
# history
hxEst = xEst
hxTrue = xTrue
hxDR = xTrue
while SIM_TIME >= time:
time += DT
u = calc_input()
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
xEst, PEst, px, pw = pf_localization(px, pw, xEst, PEst, z, ud)
# store data history
hxEst = np.hstack((hxEst, xEst))
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
if show_animation:
plt.cla()
for i in range(len(z[:, 0])):
plt.plot([xTrue[0, 0], z[i, 1]], [xTrue[1, 0], z[i, 2]], "-k")
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
plt.plot(px[0, :], px[1, :], ".r")
plt.plot(np.array(hxTrue[0, :]).flatten(),
np.array(hxTrue[1, :]).flatten(), "-b")
plt.plot(np.array(hxDR[0, :]).flatten(),
np.array(hxDR[1, :]).flatten(), "-k")
plt.plot(np.array(hxEst[0, :]).flatten(),
np.array(hxEst[1, :]).flatten(), "-r")
plot_covariance_ellipse(xEst, PEst)
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
if __name__ == '__main__':
main()