課程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))