课程9：Problem Set 3
1. 练习：Empty Cell
粒子过滤器初始化时如果采用的是随机均匀分布的策略，那么当N足够大时，粒子散步得会越广泛。空白区域的机率会变小。
2. 练习：Motion Question
一个有限的空间内，初始时，不同区域分布着不同权重，经过无穷多次移动后，在此空间中的权重的概率会区域均衡。
3. 练习：Single Particle
粒子过滤器中的粒子总数为1，粒子会忽略测量值，很可能是失败的。
4. 练习：Circular Motion
汽车转向运动，在谷歌汽车模型（单车模型，两个前轮是方向轮，2个后轮是固定轮）中，根据当前汽车（机器人）的x,y,车身偏转弧度值θ, 和方向轮与车身的夹角弧度值α，以及车身的长度L，转向距离d。定位x,y的传感器在车尾2个固定轮之间。
上图给出了计算公式。β是转向时经过的圆心角弧度值，R是转向时的曲率半径。θ是车身相对座标系x轴的偏转弧度值。cx,cy是转向时的圆心位置。这个图中的部分，在课程10中会再解释一遍。。下面的是我自己的理解。
车子在转向时，边走边转。其实有两个分运动在进行，一个是圆周运动，一个是直线运动。
上面的计算过程如下：
1. 计算转向角β，β = d * tan(α) / L
当|β| < 0.001时，认为完成转向或不需要转向，看成直线行驶，否则进入2-4步骤。直线行驶模式时，x,y更新方程变为：
x’ = x + d * cos θ
y’ = y + d * sin θ
θ’ = (θ + β) mod 2π
2. 计算转向半径R, R = d/β
3. 计算转向的圆心点座标（cx,cy）:
cx = x – sin(θ)*R
cy = y + cos(θ)*R
4. 更新车的座标和方向角
x’ = cx + sin(θ+β)* R
y’ = cy – cos(θ+β)*R
θ’ =（θ+β）mod 2π
代码如下：
# -*- coding: utf-8 -*-
# -----------------
# USER INSTRUCTIONS
#
# Write a function in the class robotcalled move()
#
# that takes self and a motion vector(this
# motion vector contains a steering* angleand a
# distance) as input and returns aninstance of the class
# robot with the appropriate x, y, andorientation
# for the given motion.
#
# *steering is defined in the video
# which accompanies this problem.
#
# For now, please do NOT add noise to yourmove function.
#
# Please do not modify anything exceptwhere indicated
# below.
#
# There are test cases which you are freeto use at the
# bottom. If you uncomment them fortesting, make sure you
# re-comment them before you submit.
from math import *
import random
# --------
#
# the "world" has 4 landmarks.
# the robot's initial coordinates aresomewhere in the square
# represented by the landmarks.
#
# NOTE: Landmark coordinates are given in(y, x) form and NOT
# in the traditional (x, y) format!
landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of4 landmarks
world_size = 100.0 # world is NOT cyclic.Robot is allowed to travel "out of bounds"
max_steering_angle = pi/4 # You don't needto use this value, but it is good to keep in mind the limitations of a realcar.
#------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializeslocation/orientation
#
def __init__(self, length = 10.0):
self.x = random.random() * world_size # initial x position
self.y = random.random() * world_size # initial y position
self.orientation = random.random() * 2.0 * pi # initial orientation
self.length = length # length of robot
self.bearing_noise = 0.0 #initialize bearing noise to zero
self.steering_noise = 0.0 # initialize steering noise to zero
self.distance_noise = 0.0 # initialize distance noise to zero
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y),str(self.orientation))
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
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)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_b_noise, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.bearing_noise =float(new_b_noise)
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
############# ONLY ADD/MODIFY CODE BELOW HERE ###################
# --------
# move:
# move along a section of acircular path according to motion
#
def move(self, motion, tolerance = 0.001): # Do not change the name ofthis function
# ADD CODE HERE
L = self.length
aerfa = motion[0]
distance = motion[1]
if abs(aerfa) > max_steering_angle:
raise ValueError("The steering angle is too large, invalid!!")
#!!!
if distance < 0.0:
raise ValueError("Moving backwards is not valid!!")
# apply noise
steering2 = random.gauss(aerfa, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)
# make a new copy
result = robot(L)
result.set(self.x, self.y, self.orientation)
result.set_noise(self.bearing_noise, self.steering_noise,self.distance_noise)
####
beta = distance2 * tan(steering2) / L
#if (abs(beta) < 0.001):
if (abs(beta) < tolerance):
result.x = self.x + distance2 * cos(self.orientation)
result.y = self.y + distance2 * sin(self.orientation)
result.orientation = (self.orientation + beta) % (2 * pi)
return result
radius = distance2/beta
cx = self.x - sin(self.orientation) * radius
cy = self.y + cos(self.orientation) * radius
result.x = cx + sin(self.orientation + beta) * radius
result.y = cy - cos(self.orientation + beta) * radius
result.orientation = (self.orientation + beta) % (2 * pi)
return result # make sure your move function returns an instance
# of the robot class withthe correct coordinates.
############## ONLY ADD/MODIFY CODE ABOVE HERE ####################
## IMPORTANT: You may uncomment the testcases below to test your code.
## But when you submit this code, yourtest cases MUST be commented
## out. Our testing program provides itsown code for testing your
## move function with randomized motiondata.
## --------
## TEST CASE:
##
## 1) The following code should print:
## Robot: [x=0.0 y=0.0orient=0.0]
## Robot: [x=10.0 y=0.0orient=0.0]
## Robot: [x=19.861 y=1.4333orient=0.2886]
## Robot: [x=39.034 y=7.1270orient=0.2886]
##
##
##length = 20.
##bearing_noise = 0.0
##steering_noise = 0.0
##distance_noise = 0.0
##
##myrobot = robot(length)
##myrobot.set(0.0, 0.0, 0.0)
##myrobot.set_noise(bearing_noise,steering_noise, distance_noise)
##
##motions = [[0.0, 10.0], [pi / 6.0, 10],[0.0, 20.0]]
##
##T = len(motions)
##
##print('Robot: ', myrobot)
##for t in range(T):
## myrobot = myrobot.move(motions[t])
## print('Robot: ', myrobot)
##
##
## IMPORTANT: You may uncomment the testcases below to test your code.
## But when you submit this code, yourtest cases MUST be commented
## out. Our testing program provides itsown code for testing your
## move function with randomized motiondata.
## 2) The following code should print:
## Robot: [x=0.0 y=0.0orient=0.0]
## Robot: [x=9.9828 y=0.5063orient=0.1013]
## Robot: [x=19.863 y=2.0201orient=0.2027]
## Robot: [x=29.539 y=4.5259orient=0.3040]
## Robot: [x=38.913 y=7.9979orient=0.4054]
## Robot: [x=47.887 y=12.400orient=0.5067]
## Robot: [x=56.369 y=17.688orient=0.6081]
## Robot: [x=64.273 y=23.807orient=0.7094]
## Robot: [x=71.517 y=30.695orient=0.8108]
## Robot: [x=78.027 y=38.280orient=0.9121]
## Robot: [x=83.736 y=46.485orient=1.0135]
##
##
length = 20.
bearing_noise = 0.0
steering_noise = 0.0
distance_noise = 0.0
##
myrobot = robot(length)
myrobot.set(0.0, 0.0, 0.0)
myrobot.set_noise(bearing_noise,steering_noise, distance_noise)
##
motions = [[0.2, 10.] for row inrange(10)]
##
T = len(motions)
##
print('Robot: ', myrobot)
for t in range(T):
myrobot = myrobot.move(motions[t])
print('Robot: ', myrobot)
## IMPORTANT: You may uncomment the testcases below to test your code.
## But when you submit this code, yourtest cases MUST be commented
## out. Our testing program provides itsown code for testing your
## move function with randomized motiondata.5. 练习：Sensing
上一节是写出自行车模型的move(),这一节练习如何写出sense()，假设有4个landmarks，每个landmarks 是[y,x]的形式，sense()要返回机器人车身方向与机器人定位点到各个landmarks点之间连线，经过的弧度值。
这里，视频提到了 detaY/detaX, 然后用反正切函数atan2()获取这个弧度值，这里一定要注意函数atan(x)与atan2(y,x)的区别，用atan(x)做起来比较麻烦，感觉不适合座标系内进行角度计算。在这里用的话，要多加好几个判断。而atan2(y,x)就容易编程些。
1、atan（x）表示求的是x的反正切，其返回值为[-pi/2,+pi/2]之间的一个数。
2、atan2（y，x）求的是y/x的反正切，其返回值为[-pi,+pi]之间的一个数。
atan2返回值解释：
在三角函数中，两个参数的函数atan2是正切函数的一个变种。对于任意不同时等于0的实参数x和y，atan2(y,x)所表达的意思是座标原点为起点，指向(y,x)的射线在座标平面上与x轴正方向之间的角的角度度。当y>0时，射线与x轴正方向的所得的角的角度指的是x轴正方向绕逆时针方向到达射线旋转的角的角度；而当y<0时，射线与x轴正方向所得的角的角度指的是x轴正方向绕顺时针方向达到射线旋转的角的角度。
上述截取自：http://blog.sina.com.cn/s/blog_7155fb1a01014l5h.html
另外要注意相减的顺序，A-B,B(robot位置)作为的原点。方便关联车身本身的方向orientation。
而且这里的原座标用的座标系就是普通的平面直角座标系，不是有的显示系统中屏幕左上角为（0,0），然后右下角为（Xmax,Ymax）的座标系。
sense()代码如下：
# --------
# sense:
# obtains bearings from positions
#
def sense(self, add_noise = True): #do notchange the name of this function
Z = []
# ENTER CODE HERE
for i in range(len(landmarks)):
# 这里计算，要注意的是在计算 atan2 时，传入的是 detaY和detaX,
# 返回的结果是以被减去的点 self(x,y) 这里为圆心，平行于目前的座标系的新座标系
# 中，从新 x 轴正方向所在的射线，到点1landmark[i] 与圆心连线的线段之间经过的角度。
# 新座标系的x轴,y轴是平行于原座标系的，所以，车身方向与新x轴的夹角是已知的 orientation
# 要减去它，才得到
detaX = landmarks[i][1] - self.x
detaY = landmarks[i][0] - self.y
arctan = atan2(detaY, detaX)
alpha = arctan - self.orientation
# let alpha from[-2*pi, 2*pi] to[0, 2*pi]
alpha %= 2.0 * pi
if (alpha < 0) :
alpha = 2.0 * pi + alpha
if add_noise:
alpha += random.gauss(0.0,self.bearing_noise)
Z.append(alpha)
# HINT: You will probably need to usethe function atan2()
return Z #Leave this line here. Returnvector Z of 4 bearings.上一个课程中，测量方法sense()返回的是一个列表，表示机器人当前位置跟各个landmarks的大致距离。这回sense()返回的列表中装的是机器人当前车身方向到各个landmarks与车子连线之间的角度。可以知道，这次课程比前一次课程的测量结果中，除了隐含车子的x,y位置以外，融入了车身当前方向的因素。
6. 练习：Final Quiz
改了一点代码，加了部分测试代码，结果差不多。这里，老师已经加了一些函数，包括measurement_prob()，用来获取当前粒子的采样权重的。实现方式是：对当前粒子的测量结果，与收到的实际测量结果(作为均值)，结合噪声特性(作为方差)，获取它们的高斯概率分布的值，并累积测量维度的概率，得到当前粒子的权重。
# -*- coding: utf-8 -*-
# --------------
# USER INSTRUCTIONS
#
# Now you will put everything together.
#
# First make sure that your sense and movefunctions
# work as expected for the test casesprovided at the
# bottom of the previous two programmingassignments.
# Once you are satisfied, copy your senseand move
# definitions into the robot class on thispage, BUT
# now include noise.
#
# A good way to include noise in the sensestep is to
# add Gaussian noise, centered at zerowith variance
# of self.bearing_noise to each bearing.You can do this
# with the command random.gauss(0,self.bearing_noise)
#
# In the move step, you should make surethat your
# actual steering angle is chosen from aGaussian
# distribution of steering angles. This distribution
# should be centered at the intendedsteering angle
# with variance of self.steering_noise.
#
# Feel free to use the included set_noisefunction.
#
# Please do not modify anything exceptwhere indicated
# below.
from math import *
import random
# --------
#
# some top level parameters
#
max_steering_angle = pi / 4.0 # You do notneed to use this value, but keep in mind the limitations of a real car.
bearing_noise = 0.1 # Noise parameter:should be included in sense function.
steering_noise = 0.1 # Noise parameter:should be included in move function.
distance_noise = 5.0 # Noise parameter:should be included in move function.
tolerance_xy = 15.0 # Tolerance forlocalization in the x and y directions.
tolerance_orientation = 0.25 # Tolerancefor orientation.
# --------
#
# the "world" has 4 landmarks.
# the robot's initial coordinates aresomewhere in the square
# represented by the landmarks.
#
# NOTE: Landmark coordinates are given in(y, x) form and NOT
# in the traditional (x, y) format!
landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of4 landmarks in (y, x) format.
world_size = 100.0 # world is NOT cyclic.Robot is allowed to travel "out of bounds"
#------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializeslocation/orientation
#
def __init__(self, length = 20.0):
self.x = random.random() * world_size # initial x position
self.y = random.random() * world_size # initial y position
self.orientation = random.random() * 2.0 * pi # initial orientation
self.length = length # length of robot
self.bearing_noise = 0.0 #initialize bearing noise to zero
self.steering_noise = 0.0 # initialize steering noise to zero
self.distance_noise = 0.0 # initialize distance noise to zero
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
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)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_b_noise, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.bearing_noise =float(new_b_noise)
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
# --------
# measurement_prob
# computes the probability of ameasurement
#
def measurement_prob(self, measurements):
# calculate the correct measurement
predicted_measurements = self.sense(0) # Our sense function took 0 as anargument to switch off noise.
# compute errors
error = 1.0
for i in range(len(measurements)):
error_bearing = abs(measurements[i] - predicted_measurements[i])
error_bearing = (error_bearing + pi) % (2.0 * pi) - pi # truncate
# update Gaussian
error *= (exp(- (error_bearing ** 2) / (self.bearing_noise ** 2) / 2.0)/
sqrt(2.0 * pi *(self.bearing_noise ** 2)))
return error
def __repr__(self): #allows us to print robot attributes.
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y),
str(self.orientation))
############# ONLY ADD/MODIFY CODE BELOW HERE ###################
# --------
# move:
#
# copy your code from the previous exercise
# and modify it so that it simulates motion noise
# according to the noise parameters
# self.steering_noise
# self.distance_noise
def move(self, motion, tolerance = 0.001):
# ADD CODE HERE
L = self.length
aerfa = motion[0]
distance = motion[1]
if abs(aerfa) > max_steering_angle:
raise ValueError("The steering angle is too large, invalid!!")
#!!!
if distance < 0.0:
raise ValueError("Moving backwards is not valid!!")
# apply noise
steering2 = random.gauss(aerfa, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)
# make a new copy
result = robot(L)
result.set(self.x, self.y, self.orientation)
result.set_noise(self.bearing_noise, self.steering_noise,self.distance_noise)
####
beta = distance2 * tan(steering2) / L
#if (abs(beta) < 0.001):
if (abs(beta) < tolerance):
result.x = self.x + distance2 * cos(self.orientation)
result.y = self.y + distance2 * sin(self.orientation)
result.orientation = (self.orientation + beta) % (2 * pi)
return result
radius = distance2 / beta
cx = self.x - sin(self.orientation) * radius
cy = self.y + cos(self.orientation) * radius
result.x = cx + sin(self.orientation + beta) * radius
result.y = cy - cos(self.orientation + beta) * radius
result.orientation = (self.orientation + beta) % (2 * pi)
return result
# --------
# sense:
#
# copy your code from the previous exercise
# and modify it so that it simulates bearing noise
#according to
# self.bearing_noise
def sense(self, add_noise = True): #do not change the name of thisfunction
Z = []
for i in range(len(landmarks)):
# 这里计算，要注意的是在计算atan2 时，传入的是 detaY和detaX,
# 返回的结果是以被减去的点self(x,y) 这里为圆心，平行于目前的座标系的新座标系
# 中，从新 x 轴正方向所在的射线，到点1 landmark[i] 与圆心连线的线段之间经过的角度。
# 新座标系的x轴,y轴是平行于原座标系的，所以，车身方向与新x轴的夹角是已知的orientation
# 要减去它，才得到
detaX = landmarks[i][1] - self.x
detaY = landmarks[i][0] - self.y
arctan = atan2(detaY, detaX)
alpha = arctan - self.orientation
# let alpha from[-2*pi, 2*pi] to [0, 2*pi]
alpha %= 2.0 * pi
if (alpha < 0) :
alpha = 2.0 * pi + alpha
if add_noise:
alpha += random.gauss(0.0,self.bearing_noise)
Z.append(alpha)
return Z
############## ONLY ADD/MODIFY CODE ABOVE HERE ####################
# --------
#
# extract position from a particle set
#
def get_position(p):
x = 0.0
y = 0.0
orientation = 0.0
for i in range(len(p)):
x += p[i].x
y += p[i].y
# orientation is tricky because it is cyclic. By normalizing
# around the first particle we are somewhat more robust to
# the 0=2pi problem
orientation += (((p[i].orientation - p[0].orientation + pi) % (2.0 *pi))
+ p[0].orientation- pi)
return [x / len(p), y / len(p), orientation / len(p)]
# --------
#
# The following code generates themeasurements vector
# You can use it to develop your solution.
#
def generate_ground_truth(motions):
myrobot= robot()
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
# added for test
initRobot = myrobot
Z = []
T = len(motions)
for t in range(T):
myrobot = myrobot.move(motions[t])
Z.append(myrobot.sense())
#print 'Robot: ', myrobot
#return [myrobot, Z]
return [myrobot, Z, initRobot]
# --------
#
# The following code prints themeasurements associated
# with generate_ground_truth
#
def print_measurements(Z):
T = len(Z)
print('measurements = [[%.8s, %.8s, %.8s, %.8s],' % \
(str(Z[0][0]), str(Z[0][1]), str(Z[0][2]), str(Z[0][3])))
for t in range(1,T-1):
print(' [%.8s,%.8s, %.8s, %.8s],' % \
(str(Z[t][0]), str(Z[t][1]), str(Z[t][2]), str(Z[t][3])))
print(' [%.8s,%.8s, %.8s, %.8s]]' % \
(str(Z[T-1][0]), str(Z[T-1][1]), str(Z[T-1][2]), str(Z[T-1][3])))
# --------
#
# The following code checks to see if yourparticle filter
# localizes the robot to within thedesired tolerances
# of the true position. The tolerances aredefined at the top.
#
def check_output(final_robot,estimated_position):
error_x = abs(final_robot.x - estimated_position[0])
error_y = abs(final_robot.y - estimated_position[1])
error_orientation = abs(final_robot.orientation - estimated_position[2])
error_orientation = (error_orientation + pi) % (2.0 * pi) - pi
correct = error_x < tolerance_xy and error_y < tolerance_xy \
and error_orientation < tolerance_orientation
return correct
def particle_filter(motions, measurements,N=500): # I know it's tempting, but don't change N!
# --------
#
# Make particles
#
p = []
for i in range(N):
r = robot()
# added begin 自己加的一段代码，假如被测试的粒子的sensor数据可信度比较高的话，
# 初始化粒子时，默认初始化到机器人附近，sigmaA较大时，还是能发现有 False的现象出现
sigmaA = 10 # 主要是假设的测量误差？
r.x = random.gauss(init_robot.x, sigmaA)
r.y = random.gauss(init_robot.y, sigmaA)
# added end.
r.set_noise(bearing_noise, steering_noise, distance_noise)
p.append(r)
# --------
#
# Update particles
#
for t in range(len(motions)):
# motion update (prediction)
p2 = []
for i in range(N):
p2.append(p[i].move(motions[t]))
p = p2
# measurement update
w = []
for i in range(N):
w.append(p[i].measurement_prob(measurements[t]))
#-----add begin
sm = sum(w)
for i in range(N):
w[i] = w[i]/sm
#-----add end
# resampling
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() # added by myself
while beta > w[index]:
beta -= w[index]
index = (index + 1) % N
p3.append(p[index])
p = p3
return get_position(p)
## IMPORTANT: You may uncomment the testcases below to test your code.
## But when you submit this code, yourtest cases MUST be commented
## out.
##
## You can test whether your particlefilter works using the
## function check_output (see test case2). We will be using a similar
## function. Note: Even for awell-implemented particle filter this
## function occasionally returns False.This is because a particle
## filter is a randomized algorithm. Wewill be testing your code
## multiple times. Make sure check_outputreturns True at least 80%
## of the time.
## --------
## TEST CASES:
##
##1) Calling the particle_filter functionwith the following
## motions and measurements should return a [x,y,orientation]
## vector near [x=93.476 y=75.186 orient=5.2664], that is, the
## robot's true location.
##
#motions = [[2. * pi / 10, 20.] for row inrange(8)]
#measurements = [[4.746936, 3.859782,3.045217, 2.045506],
## [3.510067, 2.916300, 2.146394,1.598332],
## [2.972469, 2.407489,1.588474, 1.611094],
## [1.906178, 1.193329, 0.619356,0.807930],
## [1.352825, 0.662233, 0.144927,0.799090],
## [0.856150, 0.214590, 5.651497,1.062401],
## [0.194460, 5.660382, 4.761072,2.471682],
## [5.717342, 4.736780, 3.909599,2.342536]]
##
#print(particle_filter(motions,measurements))
## 2) You can generate your own test casesby generating
## measurements using the generate_ground_truth function.
## It will print the robot's last location when calling it.
##
##
number_of_iterations = 6
motions = [[2. * pi / 20, 12.] for row inrange(number_of_iterations)]
##
x = generate_ground_truth(motions)
final_robot = x[0]
measurements = x[1]
init_robot = x[2] # added for test
estimated_position =particle_filter(motions, measurements)
print_measurements(measurements)
print("init robot may be: ",init_robot)
print('Ground truth: ', final_robot)
print('Particle filter: ', estimated_position)
print('Code check: ', check_output(final_robot,estimated_position))