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)