ソフトウェアエンジニアの技術ブログ：Software engineer tech blog

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 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.)

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)

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

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