课程8 Particle Filters

版权声明: https://blog.csdn.net/buxinchun/article/details/80561577

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

没有更多推荐了,返回首页