一个简单的粒子滤波器应用实例
机器人在一个二维平面运动,在这个二维平面中随机采样 1000 个粒子(即创建 1000 个机器人)。机器人每移动一步,粒子做完全相同的移动,接着计算粒子权重,并根据粒子权重进行粒子重采样。
- 重采样方法一:选取权重高的粒子,复制这些被选中的粒子,使总粒子数为 1000,例如:选取权重排名前 100 的粒子,对这 100 个粒子复制 10 倍,最后粒子总和即为 1000。
- 重采样方法二:详解视频 车轮法选取粒子
详细代码如下:
from math import *
import random
# 机器人四个参照物
landmarks = [[20.0, 20.0],[80.0, 80.0],[20.0, 80.0],[80.0, 20.0]]
# 地图大小
world_size = 100.0
class robot:
def __init__(self):
# 给机器人初始化一个坐标和方向
self.x = random.random() * world_size
self.y = random.random() * world_size
self.oriention = random.random() * 2.0 * pi
# 初始化噪声
self.forward_noise = 0.0
self.turn_noise = 0.0
self.sense_noise = 0.0
def set(self, new_x, new_y, new_orientation):
# 设定机器人的坐标、方向
if new_x < 0 or new_x >= world_size:
raise ValueError('X coordinate out of bound')
if new_y < 0 or new_y >= world_size:
raise ValueError('Y coordinate out of bound')
if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError('Oriention must be in [0, 2pi]')
self.x = float(new_x)
self.y = float(new_y)
self.oriention = float(new_orientation)
def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
# 设定机器人的噪声
self.forward_noise = float(new_f_noise)
self.turn_noise = float(new_t_noise)
self.sense_noise = float(new_s_noise)
def sense(self):
# 测量机器人到四个参照物的距离,添加高斯噪声
Z = []
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0])**2 + (self.y - landmarks[i][1])**2)
dist += random.gauss(0.0, self.sense_noise) # random.gauss(mu, sigma)
Z.append(dist)
return Z
def move(self, turn, forward):
# 机器人转向、前进,并返回更新后的机器人新的坐标和噪声大小
if forward < 0:
raise ValueError('Robot can\'t move backwards')
# turn, and add randomness to the turning command
orentation = self.oriention + float(turn) + random.gauss(0.0, self.turn_noise)
orentation %= 2 * pi
# move, and addd randomness to the motion command
dist = float(forward) + random.gauss(0.0, self.forward_noise)
x = self.x + (cos(orentation) * dist)
y = self.y + (sin(orentation) * dist)
x %= world_size
y %= world_size
def Gaussian(self, mu, sigma, x):
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
def measurement_prob(self, measurement):
# calculates how likely a measurement should be
# 计算出的距离相对于正确距离的概率, 计算值距离测量值越近,则概率越大
prob = 1.0
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0])**2 + (self.y - landmarks[i][1])**2)
# 与参考物的距离为期望,感知噪声为方差
prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
return prob
def prn_obj(obj):
print ('\n'.join(['%s:%s' % item for item in obj.__dict__.items()]))
'''
### test the robot class
# 初始化一个机器人
myrobot = robot()
# 打印机器人的所有参数
# prn_obj(myrobot)
# 设定初始位置
myrobot.set(30, 50, 0.5)
# 设定噪声
myrobot.set_noise(5.0, 0.1, 5.0)
# 打印机器人
# prn_obj(myrobot)
# 打印与参考物的距离
Z = myrobot.sense()
# print(Z)
# 机器人移动
myrobot.move(pi/2, 10.0)
# prn_obj(myrobot)
Z = myrobot.sense()
# print(Z)
'''
## 在运动初期给机器人初始化1000个位置粒子,这些粒子随机分布在整个地图中
myrobot = robot()
myrobot.move(0.1, 5.0) # turn: 0.1; forward: 5.0
Z = myrobot.sense()
N = 1000
# 初始化1000 个粒子
p = []
for i in range(N):
x = robot()
x.set_noise(0.05, 0.05, 5.0)
x.move(0.1, 5.0)
p.append(x)
# 计算各个粒子的权重
# 粒子权重为粒子位置与机器人位置的误差,并将 [index, w] 对存入字典
w = []
dic = {}
for i in range(N):
prob = p[i].measurement_prob(Z)
w.append(prob)
dic.update({i:prob})
# print(w)
# print(dic)
'''
## 粒子重采样:车轮法选取粒子
p_resemple = []
index = int(random.randint(0, N))
beta = 0.0
w_max = max(w)
for i in range(N):
beta += random.random() * 2.0 * w_max
while beta > w[index]:
beta -= w[index]
index = (index+1) % N
p_resemple.append(p[index])
print(len(p_resemple))
prn_obj(p_resemple[0])
'''
## 粒子重采样:挑选权重大的前100个粒子,每个粒子复制10次
# index = []
w.sort()
p_resemple = []
for i in range(100):
index = list(dic.keys())[list(dic.values()).index(w[i])]
p_resemple.extend([p[index]] * 10)
# index.extend([index_chosen] * 10)
prn_obj(p_resemple[0])