パーティクルフィルタ

2013-05-07

AI for Robotics - Lesson 3: Particle Filters (Wiki) (動画)

実装が簡単、とのこと。

いろんな状態のパーティクルで予測してみて、それぞれ実際の観測データとの誤差によって重み付けして、ありえそうなパーティクルを選択するようにしていく。

weight

各パーティクルがどのくらいセンサーからの観測データに近いかに応じて、ウェイトを計算する。ウェイトはセンサーで取得した値とそのパーティクルでの理論値との二乗誤差が規定のガウシアンででる確率を乗算したもの:

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 *= Gaussian(dist, self.sense_noise, measurement[i])
return prob

def Gaussian(mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))

resampling

世代を進めるときに、ウェイトが大きいものは重要なものとして多く残し、小さいものは重要じゃないので減らしていく。実際にはウェイトに比例してサンプリングしなおす。遺伝アルゴリズムみたいな感じか。

ウェイトに比例したリサンプリングは、単純に実装すると非効率的なものになってしまう。うまいリサンプリングの仕方:ウェイトの最大値をwmaxとしたとき、0〜2 * wmaxの乱数を生成して、周期的にピックアップしていく:

# p: パーティクルリスト
# N: パーティクルの数
p3 = []
index = int(random.random() * N)
beta = 0.0
mw = max(w)
for i in range(N):
beta += random.random() * 2.0 * mw
while beta >= w[index]:
beta -= w[index]
index = (index + 1) % N
p3.append(p[index])
p = p3

このアルゴリズムで、2*wmaxにはなにか理由があるのか、選ばれるパーティクルに偏りができてしまわないかという質問に対する回答は、わからん、とのこと。

リサンプリングによって同じパーティクルが何度も選ばれる可能性があるが、次回の移動時に正規分布の乱数によってばらつくので、リサンプルしたときにランダムでずらさなくてもよい。

プログラミング課題

ノイズのある移動、回転、センサー情報からロボットの姿勢を推測する。センサーはランドマークの角度を使う:

# --------------
# USER INSTRUCTIONS
#
# Now you will put everything together.
#
# First make sure that your sense and move functions
# work as expected for the test cases provided at the
# bottom of the previous two programming assignments.
# Once you are satisfied, copy your sense and move
# definitions into the robot class on this page, BUT
# now include noise.
#
# A good way to include noise in the sense step is to
# add Gaussian noise, centered at zero with 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 sure that your
# actual steering angle is chosen from a Gaussian
# distribution of steering angles. This distribution
# should be centered at the intended steering angle
# with variance of self.steering_noise.
#
# Feel free to use the included set_noise function.
#
# Please do not modify anything except where indicated
# below.

from math import *
import random

# --------
#
# some top level parameters
#

max_steering_angle = pi / 4.0 # You do not need 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 for localization in the x and y directions.
tolerance_orientation = 0.25 # Tolerance for orientation.


# --------
#
# the "world" has 4 landmarks.
# the robot's initial coordinates are somewhere 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 of 4 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 initializes location/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 a measurement
#

def measurement_prob(self, measurements):

# calculate the correct measurement
predicted_measurements = self.sense(0) # Our sense function took 0 as an argument 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:
#
def move(self, motion):
alpha = random.gauss(motion[0], self.steering_noise)
d = random.gauss(motion[1], self.distance_noise)

L = self.length
beta = d / L * tan(alpha)
if abs(beta) < 0.001:
new_x = self.x + cos(self.orientation) * d
new_y = self.y + sin(self.orientation) * d
new_orientation = (self.orientation + beta) % (2 * pi)
else:
R = d / beta
theta = self.orientation
cx = self.x - sin(theta) * R
cy = self.y + cos(theta) * R
new_x = cx + sin(theta + beta) * R
new_y = cy - cos(theta + beta) * R
new_orientation = (theta + beta) % (2 * pi)

result = robot(self.length)
result.set(new_x, new_y, new_orientation)
result.bearing_noise = self.bearing_noise
result.steering_noise = self.steering_noise
result.distance_noise = self.distance_noise

return result

# --------
# sense:
#
def sense(self, add_noise = True):
Z = []
for i in range(len(landmarks)):
dx = landmarks[i][1] - self.x
dy = landmarks[i][0] - self.y
bearing = atan2(dy, dx) - self.orientation
if add_noise:
bearing += random.gauss(0.0, self.bearing_noise)
Z.append(bearing % (2.0 * pi))
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 the measurements 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)

Z = []
T = len(motions)

for t in range(T):
myrobot = myrobot.move(motions[t])
Z.append(myrobot.sense())
#print 'Robot: ', myrobot
return [myrobot, Z]

# --------
#
# The following code prints the measurements 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 your particle filter
# localizes the robot to within the desired tolerances
# of the true position. The tolerances are defined 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()
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]))

# resampling
p3 = []
index = int(random.random() * N)
beta = 0.0
mw = max(w)
for i in range(N):
beta += random.random() * 2.0 * mw
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 test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out.
##
## You can test whether your particle filter works using the
## function check_output (see test case 2). We will be using a similar
## function. Note: Even for a well-implemented particle filter this
## function occasionally returns False. This is because a particle
## filter is a randomized algorithm. We will be testing your code
## multiple times. Make sure check_output returns True at least 80%
## of the time.



## --------
## TEST CASES:
##
##1) Calling the particle_filter function with 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 in range(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 cases by 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 in range(number_of_iterations)]
##
##x = generate_ground_truth(motions)
##final_robot = x[0]
##measurements = x[1]
##estimated_position = particle_filter(motions, measurements)
##print_measurements(measurements)
##print 'Ground truth: ', final_robot
##print 'Particle filter: ', estimated_position
##print 'Code check: ', check_output(final_robot, estimated_position)
  • measurement_prob()での誤差の計算は、モデルごとに変える必要がある。ここでは、ランドマークの方向との差がセンサーのノイズのガウシアンでどのくらい起こりえるか、を乗算していったもの。
  • measurement_prob()で、ノイズを加えないセンサーからのデータを使う理由がよくわからない…。実際にはセンサーからのデータからノイズを完全に除去できるわけじゃないと思うので。

QA

パーティクルフィルタは