课程8 Particle Filters
1. Field Trip
2. 练习: State Space
3. 练习:Belief Modality
4. 练习:Efficiency
5. 练习:Exact or Approximate
前面几个练习,讲的内容如下:
过滤器名称 | 状态空间 | 概率分布 | 维度增加时的效率 | 对后验结果的准确性 |
直方图过滤器(Histogram Filters) | 离散(Discrete) | 多峰(Multimodal) | 指数级 | 近似 |
卡尔曼过滤器(Kalman Filters) | 连续(Continues) | 单峰(Unimodal) | 二次 | 近似,对线性系统是准确的 |
粒子过滤器(Particle Filters) | 连续 | 多峰 | 指数 | 近似 |
在任何超过4维的空间用粒子过滤器表示,都是一个错误的选择,它的优点是易于编程。它用一个粒子集合来近似表示后验结果。
6. Particle Filters
7. Using Robot Class
8. Robot Class Details
9. 练习:Moving Robot
10. 练习:Add Noice
前面几个练习,主要讲如何创建一个简单的二维空间的机器人,如何移动,并且加入了噪声因子,在指定的地图(world)里移动,如果出边界,就从另外一边进来。地图里面,有几个landmarks,称路标。
sense() 是测量函数,返回机器人跟每个路标的欧几里得距离的列表。measurement_prob() 返回当前机器人跟测量结果之间的相似性。这里是根据机器人跟每个路标的欧几里得距离,算出在每个距离上出现测量结果的对应距离的出现概率值(高斯函数值),并返回了这几个概率的乘积。过滤粒子时用的。
具体代码如下:
# -*- coding: utf-8 -*-
# Make a robot called myrobot that startsat
# coordinates 30, 50 heading north (pi/2).
# Have your robot turn clockwise by pi/2,move
# 15 m, and sense. Then have it turnclockwise
# by pi/2 again, move 10 m, and senseagain.
#
# Your program should print out the resultof
# your two sense measurements.
#
# Don't modify the code below. Pleaseenter
# 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;
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);
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
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 # cyclictruncate
y %= world_size
# set particle
res = robot()
res.set(x, y, orientation)
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
return res
def Gaussian(self, mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu andvar. sigma
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 __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y),str(self.orientation))
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)
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))
#### DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW ####
myrobot = robot()
# enter code below
myrobot.set(30., 50., pi/2)
myrobot.set_noise(5.0, 0.1, 5.0)
myrobot = myrobot.move(-pi/2, 15.)
measure1 = myrobot.sense()
print(measure1)
myrobot = myrobot.move(-pi/2, 10.)
measure2 = myrobot.sense()
print(measure2)
11. Robot World
12. 练习:Creating Particles
13. 练习:Robot Particles
创建一定数量的粒子,这些粒子其实也是机器人。刚开始时,它们被给定一些初始值,你可以考虑均匀分布在地图上,或者随机分布在地图上,也可以让它们分布在实际的机器人附近。
然后,它们会跟着那个原来的机器人做类似的运动。
14. 练习:Importance Weight
实际测量值和预测测量值之间的不匹配,会产生重要性权重。权重越大,重要性越高。权重越大,在过滤时的存活概率越高。
重采样:指从旧粒子中随机抽取N个粒子,同时根据重要性权重,进行替换。
重采样后,权重较低的粒子会被淘汰,权重较高的会存活。
取得权重的一个代码片段:
def measurement_prob(self,measurement):
# calculates howlikely a measurement should be
prob = 1.0;
for i inrange(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
….
#### DON'T MODIFY ANYTHINGABOVE HERE! ENTER CODE BELOW ####
myrobot = robot()
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
N = 1000
p = []
for i in range(N):
x = robot()
x.set_noise(0.05, 0.05,5.0)
p.append(x)
p2 = []
for i in range(N):
p2.append(p[i].move(0.1,5.0))
p = p2
w = []
#insert code here!
for i in range(N):
w.append(p[i].measurement_prob(Z))
print w
15. 练习:Resampling
获取所有粒子的权重值后,可以进行权重归一化操作。
16. 练习:Never Sampled 1
重要性低的粒子在重采样时,不大可能会被选中。
17. 练习:Never Sampled 2
重要性较高的粒子,在重采样时,也有可能不会被选中。(代码中每次重采样都是放回抽样的)
18. 练习:Never Sampled 3
19. 练习:New Particle
20. 练习:Resampling Wheel
讨论了一个重采样轮盘,里面使用的beta值,范围是在(0,2*max(w))上随机取得,感觉有点不大对,估计主要因为课程的代码中没有对w做归一化操作。所以我实现的代码 的beta值是从(0,1)中随机取得,w先做了归一化处理。
robot类下面的代码:
#### DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODEBELOW ####
myrobot = robot()
myrobot =myrobot.move(0.1, 5.0)
Z =myrobot.sense()
N = 1000
p = []
for i inrange(N):
x = robot()
x.set_noise(0.05, 0.05, 5.0)
p.append(x)
p2 = []
for i inrange(N):
p2.append(p[i].move(0.1, 5.0))
p = p2
w = []
for i inrange(N):
w.append(p[i].measurement_prob(Z))
p3 = []
# Normalized w
sumW = sum(w)
for i inrange(N):
w[i] = w[i] / sumW
# Choose a listwith N elements
index =int(random.random() * N)
beta = 0.0
mw = max(w)
for i inrange(N):
#beta += random.random() * 2.0 * mw
beta = random.random()
while w[index] < beta:
beta = beta - w[index]
index = (index + 1) % N
p3.append(p[index])
p = p3
print(p) #pleaseleave this print statement here for grading!
由于地标是独立于方向的,而且判断测量值相关性时,也没有考虑方向因素,因此会看到重采样后的粒子的方向可能多种多样。
21. 练习:Orientation 1
不同的粒子,方向不同的话,在预测下一步的位置时,就会体现出方向的作用了。
22. 练习:Orientation 2
23. 练习:Error
介绍了个新函数, eval(),以一定的方法来计算N个粒子与测量值的平均偏差,称 residual error(残余误差)
主要代码是:
# -*- coding: utf-8 -*-
# Please only modify the indicated areabelow!
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;
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:
raiseValueError('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);
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
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 # cyclictruncate
y %= world_size
# set particle
res = robot()
res.set(x, y, orientation)
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
return res
def Gaussian(self, mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu andvar. sigma
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 __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
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)
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))
#myrobot = robot()
#myrobot.set_noise(5.0, 0.1, 5.0)
#myrobot.set(30.0, 50.0, pi/2)
#myrobot = myrobot.move(-pi/2, 15.0)
#print myrobot.sense()
#myrobot = myrobot.move(-pi/2, 10.0)
#print myrobot.sense()
#### DON'T MODIFY ANYTHING ABOVE HERE! ENTER/MODIFY CODE BELOW ####
myrobot = robot()
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
N = 1000
T = 10 #Leave this as 10 for gradingpurposes.
p = []
for i in range(N):
r = robot()
r.set_noise(0.05, 0.05, 5.0)
p.append(r)
#print(eval(myrobot, p))
for t in range(T):
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
p2 = []
for i in range(N):
p2.append(p[i].move(0.1, 5.0))
p = p2
w = []
for i in range(N):
w.append(p[i].measurement_prob(Z))
#-----add begin
sm = sum(w)
for i in range(N):
w[i] = w[i]/sm
#-----add end
p3 = []
index = int(random.random() * N)
beta = 0.0
mw = max(w)
for i in range(N):
#beta += random.random() * 2.0 * mw
beta = random.random()
while beta > w[index]:
beta -= w[index]
index = (index + 1) % N
p3.append(p[index])
p = p3
#enter code here, make sure that you output 10 print statements.
print(eval(myrobot,p))
多次运行时,偶尔会出现偏差比较大的情况。
24. You and Sebastian
25. 练习:Filters
26. 2012
这个视频是录制于2011还是2012年的,这时候开始做谷歌汽车。
1. 谷歌汽车对于汽车的建模实际上是一个“自行车模型”,两个固定轮,两个方向轮。
2. 传感器数据不同,谷歌汽车不使用地标,而是非常精美的道路地图。然后抓取快照,把快照和地图相匹配,匹配度越高,分数越高。
3. 加上其他的一些传感器。
27. Preview