def sample(mean, cov , dimensions):
r = matrix.transpose( matrix.Cholesky(cov) )
randoms = matrix.zero(dimensions, 1)
for i in range(len(randoms)):
randoms[i][0] = random.gauss(0,0.025)
return matrix.plus( mean , matrix.mult(r, randoms))
def sample(mean, cov , dimensions):
r = matrix.transpose( matrix.Cholesky(cov) )
randoms = matrix.zero(dimensions, 1)
for i in range(len(randoms)):
randoms[i][0] = random.gauss(0,0.05)
return matrix.plus( mean , matrix.mult(r, randoms))
import matrix import math #matrix initializatioin xCord = matrix.zero(100, 100) yCord = matrix.zero(100, 100) elev = matrix.zero(100, 100) area = 0.0 cvol = 0.0 fvol = 0.0 rvol = 0.0 vol = 0.0 tempvol = 0.0 ##we can assign the value into the matrix like this... """xCord[0][0]=2 xCord[0][1]=5 xCord[1][0]=3 xCord[1][1]=6 yCord[0][0]=4 yCord[0][1]=6 yCord[1][0]=8 yCord[1][1]=9 elev[0][0]=6 elev[0][1]=7 elev[1][0]=3 elev[1][1]=6"""
import matrix import math #matrix initializatioin xCord=matrix.zero(100,100) yCord=matrix.zero(100,100) elev=matrix.zero(100,100) area=0.0 cvol=0.0 fvol=0.0 rvol=0.0 vol=0.0 tempvol=0.0 ##we can assign the value into the matrix like this... """xCord[0][0]=2 xCord[0][1]=5 xCord[1][0]=3 xCord[1][1]=6 yCord[0][0]=4 yCord[0][1]=6 yCord[1][0]=8 yCord[1][1]=9 elev[0][0]=6 elev[0][1]=7 elev[1][0]=3 elev[1][1]=6"""
class KalmanBall():
# R = noise for covariance matrix prediction step
R = [[0.01,0],[0,0.01]]
# Q = noise for covariance matrix correction step
Q = [[0.01,0],[0,0.01]]
mu = [[0],[0]]
Sigma = [[0.1,0],[0,0.1]]
u = matrix.zero( 2,1 )
z = matrix.zero( 2,1 )
I = [[1,0],[0,1]]
firstCall = bool
def init(self, measurement = (1,0)):
self.firstCall = True
self.mu[0][0] = measurement[0]
self.mu[1][0] = measurement[1]
def setFirstCall(self, arg = True, initial = (1,0)):
self.firstCall = arg
self.mu = [[ initial[0] ],[ initial[1] ]]
self.Sigma = [[0.1,0],[0,0.1]]
self.timeStamp = time.time()
def iterate(self, measurement, control):
now = time.time()
# timestamps matter. IMPORTANT: Do not use if first iteration has not been set.
if self.firstCall:
self.firstCall = False
timeTaken = time.time() - self.timeStamp
# major screwup if this happens
if timeTaken > 1.0:
print 'Interval was way too long: ', timeTaken, 'seconds.'
timeTaken = 0.0
self.timeStamp = time.time()
#velocity = motProxy.getRobotVelocity()
velocity = control
# known are speeds, convert to absolute movements
nao_movement_x = velocity[0] * timeTaken
nao_movement_y = velocity[1] * timeTaken
nao_movement_t = velocity[2] * timeTaken
#print timeTaken
# step forward using control vector u
self.u[0][0] = nao_movement_x
self.u[1][0] = nao_movement_y
print 'Increment control ', self.u
muBelief = self.mu
# ________ PREDICTION ________
# rotate using nao_movements theta
t = nao_movement_t
rotationmatrix = [[ math.cos( t ), -math.sin(t) ],[ math.sin(t), math.cos(t) ]]
# Predict new measurement based on nao movement.
# Nao moves x,y towards the ball
muBelief = matrix.subtract( muBelief , self.u )
#print muBelief
# Nao rotates theta
muBelief = matrix.mult( rotationmatrix, muBelief )
# add noise to motion
muBelief = sample( muBelief, self.Sigma, 2)
# covariance matrix update
SigmaBelief = matrix.plus( self.Sigma , self.R)
# ________ CORRECTION _________
if measurement:
self.z[0][0] = measurement[0]
self.z[1][0] = measurement[1]
# Since C = [1,0;0,1], drop it
s = matrix.inverse2D( matrix.plus( SigmaBelief, self.Q) )
K = matrix.mult(SigmaBelief, s )
self.mu = matrix.plus( muBelief, matrix.mult(K, matrix.subtract(self.z , muBelief)) )
self.Sigma = matrix.mult(matrix.subtract( self.I, K ), SigmaBelief)
else:
# if no ball is found, use the predicted state!
self.mu = muBelief
self.Sigma = SigmaBelief
#print 'Mu:',self.mu
#print 'Sigma: '
#matrix.show(self.Sigma)
#print ''
return (self.mu[0][0], self.mu[1][0])
def __init__(self, master):
self.master = master
frame = Frame(master)
frame.grid()
file = open("storagefile.txt", "r")
line = file.readline()
entries = []
while line:
entryrow = line[:-2].split()
for i in range(len(entryrow)):
entryrow[i] = float(entryrow[i])
entries.append(entryrow)
line = file.readline()
new = matrix.Matrix(entries)
reduced = new.rref()
Arows = [[reduced.rows[i][j] for j in range(len(reduced.rows[i]) - 1)]
for i in range(len(reduced.rows))]
A = matrix.Matrix(Arows)
consistent = True
for i in range(len(
reduced.rows)): #Check if the system is consistent...
if reduced.rows[i][-1] != 0:
consistent = False
for j in range(len(reduced.cols) - 1):
if reduced.rows[i][j] != 0:
consistent = True
break
if not consistent: #...if it's not:
label = Label(
master,
text="This system of linear equations\n is not consistent.")
label.grid(row=0, column=0)
else: #... if it is:
all_vectors = False
if reduced == matrix.zero(len(reduced.rows), len(reduced.cols)):
all_vectors = True
coefficients = []
for i in range(len(A.rows)):
leading = False # tells if there's a leading 1 in the row
coefficients_row = []
for j in range(len(A.cols)):
if A.rows[i][j] == 1 and not leading:
leading = True
elif A.rows[i][j] != 0 and leading:
coefficients_row.append((-A.rows[i][j], j + 1))
coefficients.append((leading, coefficients_row))
for i in range(len(coefficients)):
if all_vectors:
label = Label(master,
text="x" + str(i + 1) +
" can be any real number.")
label.grid(row=i, column=0)
if not coefficients[i][0]:
continue
elif coefficients[i][1] == []:
label = Label(master,
text="x" + str(i + 1) + " = " +
str(reduced.rows[i][-1]))
label.grid(row=i, column=0)
else:
stringer = "x" + str(i + 1) + " = "
for coefficient in coefficients[i][1]:
stringer += str(coefficient[0]) + "*x" + str(
coefficient[1]) + " + "
stringer += str(reduced.rows[i][-1])
label = Label(master, text=stringer)
label.grid(row=i, column=0)
