from math import * import random landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0],[100.0, 100.0]] world_size = 100.0 max_steering_angle = pi/4 class robot: def __init__(self, length = 100.0): self.x = random.random() * world_size self.y = random.random() * world_size self.orientation = random.random() * 2.0 * pi self.length = length self.bearing_noise = 0.0 self.steering_noise = 0.0 self.distance_noise = 0.0 def __repr__(self): return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation)) 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) def set_noise(self, new_b_noise, new_s_noise, new_d_noise): self.bearing_noise = float(new_b_noise) self.steering_noise = float(new_s_noise) self.distance_noise = float(new_d_noise) def move(self, motion, tolerance = 0.001): steering = motion[0] distance = motion[1] if abs(steering) > max_steering_angle: raise ValueError, 'Exceeding max steering angle' if distance < 0.0: raise ValueError, 'Moving backwards is not valid' return result
Category: Artificial Intelligence
Resampling
p1, w1 = 0.6
p2, w2 = 1.2
p3, w3 = 2.4
p4, w4 = 0.6
p5, w5 = 1.2
P(p1)
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
myrobot = robot() p = [] for i in range(N): r = robot() r.set_noise(0.05, 0.05, 5.0) p.append(r) for t in range(T): myrobot = myrobot.move(0.1, 5.0) Z = myrobot.sense()
length = 20. 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
Particle Filter
– easiest to prgram
– most flexible
continuous, multi-modal, approximate
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_noize = 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) def set_noise(self, new_f_noise, new_t_noise, new_s_noise): 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' orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise) orientation %= 2 * pi 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 y %= world_size 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): return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2)) def measurement_prob(self, measurement): 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)): 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(10.0, 10.0, 0.0) print myrobot myrobot - myrobot.move(0.0, 10.0) print myrobot myrobot = myrobot.move(pi/2, 10.0) print myrobot print myrobot.sense()
N = 1000 p = [] for i in range(N): x = robot() p.append(x) print len(p)
Kalman Matrices
from math import * class matrix: def __init__(self, value): self.value = value self.dimx = len(value) self.dimy = len(value[0]) if value == [[]]: self.dimx = 0 def zero(self, dimx, dimy): if dimx < 1 or dimy < 1: raise ValueError, "Invalid size of matrix" else: self.dimx = dimx self.dimy = dimy self.value = [[0 for row in range(dimy)] for col in range(dimx)] def identity(self, dim): if dim < 1: raise ValueError, "Invalid size of matrix" else: self.dimx = dim self.dimy = dim self.value = [[0 for row in range(dim)] for col in range(dim)] for i in range(dim): self.value[i][i] = 1 def show(self): for i in range(self.dimx): print self.value[i] print '' def __add__(self, other): if self.dimx != other.dimx or self.dimy != other.dimy: raise ValueError, "Matrices must be of equal dimensions to add" else: res = matrix([[]]) res.zero(self.dimx, self.dimy) for i in range(self.dimx): for j in range(self.dimy): res.value[i][j] = self.value[i][j] + other.value[i][j] return res def __sub__(self, other): if self.dimx != otehr.dimx or self.dimy != other.dimy: raise ValueError, "Matrices must be of equal dimensions to subtract" else: res = matrix([[]]) res.zero(self.dimx, self.dimy) for i in range(self.dimx): for j in range(self.dimy): res.value[i][j] = self.value[i][j] - other.value[i][j] return res def __mul__(self, other): if self.dimy != other.dimx: raise ValueError, "Matrices must be m*n and n*p to multiply" else: res = matrix([[]]) res.zero(self.dimx, other.dimy) for i in range(self.dimx): for j in range(other.dimy): for k in range(self.dimy): res.value[i][j] += self.value[i][k] * other.value[k][j] return res def transpose(self): res = matrix([[]]) res.zero(self.dimy, self.dimx) for i in range(self.dimx): for j in range(self.dimy): res.value[j][i] = self.value[i][j] return res def Cholesky(self, ztol=1.0e-5): res = matrix([[]]) res.zero(self.dimx, self.dimx) for i in range(self.dimx): S = sum([(res.value[i][i])** 2 for k in range(i)]) d = self.value[i][i] - S if abs(d) < ztol: res.value[i][i] = 0.0 else: if d < 0.0: raise ValueError, "Matrix not positive-definite" res.value[i][i] = sqrt(d) for j in range(i+1, self.dimx): S sum([res.value[k][i] * res.value[k][j] for k in rande(self.dimx)]) if abs(S) < ztol: S = 0.0 res.value[i][j] = (self.value[i][j] - S)/res.value[i][i] return res def CholeskyInverse(self): res = matrix([[]]) res.zero(self.dimx, self.dimx) for j in reversed(range(self.dimx)): tjj = self.value[j][j] S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)]) res.value[j][j] = 1.0/tjj**2 - S/tjj for i in reversed(range(j)): res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i] return res def inverse(self): aux = self.Cholesky() res = aux.CholeskyInverse() return res def __repr__(self): return repr(self.value) def kalman_filter(x, P): for n in range(len(measurements)): return x, P
Kalman Filters
KALMAN FILTER : continuous uni-modal
MONTE CARLO Localization : discrete
f(x) = 1/√2πΩ2 exp -1/2(x-μ2)/Θ2
Gaussians smallest is preferred.
maximize gaussian
from math import * def f(mu, sigma2, x): return 1/sqrt(2.*pi*sigma2) * exp(-.5 * (x-mu)**2 / sigma2) print f(10., 4., 8.)
new mean
def update(mean1, var1, mean2, var2): new_mean = (var2 * mean1 + var1 * mean2)/(var1 + var2) new_var = 1 / (1/var1 + 1 / var2) return [new_mean, new_var] print update(10.,8.,13.,2.)
prediction
def update(mean1, var1, mean2, var2): new_mean = (var2 * mean1 + var1 * mean2)/(var1 + var2) new_var = 1 / (1/var1 + 1 / var2) return [new_mean, new_var] def predict(mean1, var1, mean2, var2): new_mean = mean1 + mean2 new_var = var1 + var2 return [new_mean, new_var] print update(10.,8.,13.,2.) print predict(10., 4., 12., 4.)
def update(mean1, var1, mean2, var2): new_mean = (var2 * mean1 + var1 * mean2)/(var1 + var2) new_var = 1 / (1/var1 + 1 / var2) return [new_mean, new_var] def predict(mean1, var1, mean2, var2): new_mean = mean1 + mean2 new_var = var1 + var2 return [new_mean, new_var] measurements = [5., 6., 7., 9., 10.] motion = [1., 1., 2., 1., 1.] measurement_sig = 4. motion_sig = 2. mu = 0. sig = 10000. for n in range(len(measurements)): [mu, sig] = update(mu, sig, measurements[n], measurement_sig) print 'update: ', [mu, sig] [mu, sig] = predict(mu, sig, motion[n], motion_sig) print 'predict: ', [mu, sig] print update(10.,8.,13.,2.) print predict(10., 4., 12., 4.)
Limit Distribution
0.8p(x2)+ 0.1p(x1)+ 0.1p(x3) = p(x4)
Gain information, loses information
Entropy -Σp(xi)log p(x)
BELiEF = PROBABility
sense = product followed by normalization
move = convolution(=addition)
formal definition
0 <= p(x) <= 1
p(x1) = 0.2
p(x2) = 0.8
bayes rule
x = grid cell, z = measurement
p(x|z) = p(z|x)p(x)/ p(z)
motion - total probability
p(xit) = Σj P(xg t-1)・p(xi|xj)
p(x)=0.2 p(¬x)=0.8
p(x)=0.2 p(y)=0.2 p(x,y) = 0.04
p(x)=0.2 p(y|x)=0.6 p(y|¬x)=0.6 p(y)=0.6
[python]
colors = [['green', 'green', 'green'],
['green', 'red', 'green'],
['green', 'green', 'green']]
measurements = ['red', 'red']
motions = [[0, 0], [0, 1]]
sensor_right = 1.0
p_move = 1.0
[/python]
def localize(colors, measurements, motions, sensor_right, p_move):
print = 1.0 / float(len(colors)) / float(len(colors[0]))
p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))]
return p
sensor_wrong = 1.0 – sensor_right
p_stay = 1.0 – p_move
def show(p):
rows = [‘[‘ + ‘,’.join(map(lambda x: ‘{0:.5f’.format(x), r))+’]’ for r in p]
print ‘[‘ + ‘,\n ‘.join(rows) + ‘]’
colors = [[‘R’,’G’,’G’,’R’,’R’],
[‘R’,’R’,’G’,’R’,’R’],
[‘R’,’R’,’G’,’G’,’R’],
[‘R’,’R’,’R’,’R’,’R’]]
measurements = [‘G’,’G’,’G’,’G’,’G’]
motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]
p = localize(colors,measurements,motions,sensor_right = 0.7, p_move = 0.8)
show(p)
Sense Function
p=[0.2,0.2,0.2,0.2] world=['green','red','red','green','green'] pHit = 0.6 pMiss = 0.2 def sense(p, Z) q=[] for i in range(len(p)): hit = (Z == world[i]) q.append(p[i] * (hit * pHit + (1-hit) * pMiss)) return q print sense(p, Z)
p=[0.2,0.2,0.2,0.2] world=['green','red','red','green','green'] pHit = 0.6 pMiss = 0.2 def sense(p, Z) q=[] for i in range(len(p)): hit = (Z == world[i]) q.append(p[i] * (hit * pHit + (1-hit) * pMiss)) sum i in range(len(p)): q[i] = q[i] / s return q print sense(p, Z)
p=[0.2,0.2,0.2,0.2] world=['green','red','red','green','green'] measurements = ['red', 'green'] pHit = 0.6 pMiss = 0.2 def sense(p, Z) q=[] for i in range(len(p)): hit = (Z == world[i]) q.append(p[i] * (hit * pHit + (1-hit) * pMiss)) sum i in range(len(p)): q[i] = q[i] / s return q for k in range(len(measurements)): p = sense(p, measurements[k]) print sense(p, Z)
def move(p, U): q = [] for i in range(len(p)): s = pExact * p[(i-U) % len(p)] s = s + pOvershoot * p[(i-U-1)%len(p)] s = s + pUndershoot * p[(i-U+1)%len(p)] q.append(s) return q
Artificial Intelligence for Robotics
Localization
Kalman Filters
Particle Filters
Search
PID Control
SLAM(simultaneous localization and mapping)
Localization
satellite pass signals with GPS(global position system)
localization has a lot of Math
easy solution of 5 vectors
p=[0.2, 0.2, 0.2, 0.2, 0.2] print p
p=[] n = 5 for i in range(n): p.append(1./n) print p
Segment
code here
def splits(characters, longest=12): "All ways to split characters into a first word and remainder." return [(characters[:i], characters[i:]) for i in range(1, 1+min(longest, len(characters)))] def Pwords(words): "probability of a sequence of words." return product(words, key=Pw) @memo def segment(text): "Best segmentation of text into words, by probability." if text == "": return [] candidates = [[first]+segment(rest) for first,rest in splits(text)] return max(candidates, key=Pwords)
spelling correction
c* = argmaxc P(c|w)
= argmaxc P(w|c) p(c)
Natural Language processing
Language Model
-probabilistic
-word-based
-Learned
P(word1, word2…)
L = {s2, s2, …}
Logical trees hand-coded
P(w1, w2, w3…Wn) = p(w1:n) = πi P(wi|w1:i-1)
Markov assumption
P(wi|w1:i-1) = P(wi|wi-k:i-1)
stationarity assumption p(wi|wi-1) = p(wj|wj-1)
smoothing
classification, clustering, input correction, sentiment analysis, information retrieval, question answering, machine translation, speech recognition, driving a car autonomously
P(the), p(der), p(rba)
Naive Bayes
k-Nearest Neighbor
Support vector machines
logistic regression
sort command
gzip command
(echo ‘cat new EN|gzip|wc -c’ EN;\
echo ‘cat new DE|gzip|wc -c’ DE; \
echo ‘cat new AZ|gzip|wc -c’ AZ) \
|sort -n|head -1
S* = maxP(w|:n) = max√li p(wi|w1:i-1)
S* = maxP(wi)