课程8 Particle Filters

课程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