先简单总结下之前的方法:Histogram 和Kalman Filters 都是对位置的近似估计(approximately),前者是离散的容易理解,后者是线性的,对线性系统是准确的,而真实情况中,很多是非线性的。前者的内存需求是指数式增长,后者为多项式式增长,因为均值和协方差矩阵是多项式级别。modal就不说了,前uni后multi。Particle Filters是连续的,multimodal,approximately的,对于efficiency,并不确定,取决于空间维度的个数,低维度时,非常适合用来跟踪,高维度时(比如,超过四维)就不宜使用了。它的优点是,容易编程实现,解决复杂的问题!
1) Particle Filters
用sonar sensors测量最近的障碍物,使用particles,每一个粒子都结构化的由x,y坐标和heading direction(弧度radian与x轴的夹角)组成。根据传感器的测量结果筛选符合条件的粒子,不断地迭代信息,筛选,最后得到最可能的粒子cloud。
2) Robot Class,Robot World
这里的robot就是一个particle。
# Make a robot called myrobot that starts at
# coordinates 30, 50 heading north (pi/2).
# Have your robot turn clockwise by pi/2, move
# 15 m, and sense. Then have it turn clockwise
# by pi/2 again, move 10 m, and sense again.
#
# Your program should print out the result of
# your two sense measurements.
#
# Don't modify the code below. Please enter
# your code at the bottom.
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.orientation = random.random() * 2.0 * pi
self.forward_noise = 0.0;
self.turn_noise = 0.0;
self.sense_noise = 0.0;
# random.random()用于生成一个0到1的随机符点数: 0 <= n < 1.0
# random.uniform(a, b),用于生成一个指定范围内的随机符点数,两个参数其中一个是上限,一个是下限。
# 如果a > b,则生成的随机数n: a <= n <= b。如果 a <b, 则 b <= n <= a
# 欲见更多函数,移步这里:Python random模块
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, 'Orientation must be in [0.,2pi]'
self.x = float(new_x)
self.y = float(new_y)
self.orientation = 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);
# sense 就是计算 robot 到所有landmarks(obstacle) 的距离,得到一个以距离为元素的 list
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)
Z.append(dist)
return Z
# 当粒子群中的每个粒子都移动一步,形成新的粒子群分布,即对robot的位置预估计,即 motion update!
def move(self, turn, forward):
if forward < 0:
raise ValueError, 'Robot cant move backwards'
# turn, and add randomness to the turning command
orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
orientation %= 2 * pi
# move, and add randomness to the motion command
dist = float(forward) + random.gauss(0.0, self.forward_noise)
x = self.x + (cos(orientation) * dist)
y = self.y + (sin(orientation) * dist)
x %= world_size # cyclic truncate
y %= world_size
# set particle
res = robot()
res.set(x, y, orientation) # 新的位置和朝向
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise) # 关键。把旧 robot的信息传给新状态的robot
return res
def Gaussian(self, mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
# 计算particle与当前measure到的距离list的"相关性":距离越相近,概率越高,即权重越高
# 粒子群里所有的粒子都得到各自的权重,即 bayes rule的分子,标准化后,即后验分布,按比例(resampling)形成新的粒子群!
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 __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
# 计算robot和当前估计粒子群的平均误差
def eval(r, p):
sum = 0.0;
for i in range(len(p)): # calculate mean error
dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0) # 使得到的距离始终小于world_size范围的一半,取小的那一段距离(类似于取锐角)
dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
3) Robot Particles
# myrobot = robot()
# # enter code here
# myrobot.set(30.0, 50.0, pi/2)
# Now add noise to your robot as follows:
# forward_noise = 5.0, turn_noise = 0.1,
# sense_noise = 5.0.
# myrobot.set_noise(5.0,0.1,5.0)
# myrobot = myrobot.move(-pi/2, 15.0)
# print myrobot.sense()
# myrobot = myrobot.move(-pi/2, 10.0)
# print myrobot.sense()
myrobot = robot()
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
# Now we want to create particles,
# p[i] = robot(). In this assignment, write
# code that will assign 1000 such particles
# to a list.
N = 1000
p = []
#enter code here
for i in rang