def main(): P = [[40], [40]] V0 = [[30], [30]] V1 = [[40], [70]] V2 = [[50], [30]] points = [V0, V1, V2] aspect_correct = [[1, 0], [0, 4 / 7]] home = "\N{escape}[H" wakeup_time = time.time() print(home + "\N{escape}[J\N{escape}[?25l") t = 0 try: while True: kanvas = canvas.Canvas() rot = matrix.rotate_2D(t * 0.5) r_points = [] for point in points: C = matrix.sub(point, P) C = matrix.mult(rot, C) C = matrix.add(P, C) C = matrix.mult(aspect_correct, C) r_points.append(C) t += 1 kanvas.triangle(round(r_points[0][0][0]), round(r_points[0][1][0]), round(r_points[1][0][0]), round(r_points[1][1][0]), round(r_points[2][0][0]), round(r_points[2][1][0])) image = home + kanvas.get_image() print(image) wakeup_time += 0.05 time.sleep(max(0, wakeup_time - time.time())) finally: print(home + "\N{escape}[J \N{escape}[?25h")
def main(): P = [[40], [40], [0]] Axis = [[10], [5], [3]] # V0 = [[30], # [30], # [0]] # V1 = [[40], # [70], # [0]] # V2 = [[50], # [30], # [0]] # points = [V0, V1, V2] points = [] for i in range(15): points.append([[random.randint(20, 60)], [random.randint(30, 60)], [random.randint(-20, 20)]]) aspect_correct = [[1, 0, 0], [0, 4/7, 0], [0, 0, 1]] home = "\N{escape}[H" wakeup_time = time.time() print(home + "\N{escape}[J\N{escape}[?25l") t = 0 try: while True: kanvas = canvas.Canvas() rot = matrix.rotate_3D(t * 0.1, Axis) r_points = [] for point in points: C = matrix.sub(point, P) C = matrix.mult(rot, C) C = matrix.add(P, C) C = matrix.mult(aspect_correct, C) r_points.append(C) t += 1 for r_point in r_points: kanvas.dab(round(r_point[0][0]), round(r_point[1][0])) # kanvas.triangle(round(r_points[0][0][0]), # round(r_points[0][1][0]), # round(r_points[1][0][0]), # round(r_points[1][1][0]), # round(r_points[2][0][0]), # round(r_points[2][1][0])) image = home + kanvas.get_image() print(image) wakeup_time += 0.05 time.sleep(max(0, wakeup_time - time.time())) finally: print(home + "\N{escape}[J \N{escape}[?25h")
def iterate(self, measurement, control ): if measurement: self.lost = False velocity = control # known are speeds, convert to absolute movements nao_movement_x = velocity[0] nao_movement_y = velocity[1] nao_movement_t = velocity[2] # step forward using control vector u self.u[0][0] = nao_movement_x self.u[1][0] = nao_movement_y 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 self.ballPos = self.mu[0][0], self.mu[1][0]
def solve(m, bs, p): """ Solve a triangularized linear system, writing the solutions to bs, p being the number of variables. Returns False if there are no solutions. """ for i in range(len(m) - 1, -1, -1): z = first_nonzero(m[i]) if z > p: continue elif z == p: return False bs[z][0] = m[i][p] for j in range(z+1, p): matrix.add(bs, z, -m[i][j], j) matrix.mult(bs, z, 1.0/m[i][z]) return True
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 poly(x,y): if NUMPY: c = [str(x) for x in polyfit(x,y,len(x)-1)] else: a = [[a**b for b in range(len(x)-1,-1,-1)] for a in x] b = [[t] for t in y] c = [str(round(A,14)) for B in matrix.mult(matrix.inv(a),b) for A in B] return ' + '.join([X for X in (c[A]+'x'+''.join([['⁰','¹','²','³','⁴','⁵','⁶','⁷','⁸','⁹'][int(y)] for y in str(len(c)-int(A)-1)]) if float(c[A])!=0 else '' for A in range(len(c))) if X])
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))
def poly(x, y): if NUMPY: c = [str(x) for x in polyfit(x, y, len(x) - 1)] else: a = [[a**b for b in range(len(x) - 1, -1, -1)] for a in x] b = [[t] for t in y] c = [ str(round(A, 14)) for B in matrix.mult(matrix.inv(a), b) for A in B ] return ' + '.join([ X for X in ( c[A] + 'x' + ''.join([['⁰', '¹', '²', '³', '⁴', '⁵', '⁶', '⁷', '⁸', '⁹'][int(y)] for y in str(len(c) - int(A) - 1)]) if float(c[A]) != 0 else '' for A in range(len(c))) if X ])
import matrix a = matrix.rand_matrix(4, 4) b = matrix.rand_matrix(4, 4) c = matrix.create_matrix(4, 4) matrix.mult(c, a, b) print c a = matrix.rand_matrix(4, 2) b = matrix.rand_matrix(2, 4) c = matrix.create_matrix(4, 4) c = matrix.mult(c, a, b) print c r = matrix.create_matrix(2, 2) print matrix.mult(r, [[1, 0], [0, 1]], [[1, 0], [0, 1]]) print matrix.mult(r, [[2, 0], [0, 2]], [[0, 2], [2, 0]])
for i in range(len(mat)): add = [0] * len(mat) add[i] = 1 mat[i] += add latex_line(mat) # Eliminating for i in range(len(mat)): if is_zero(mat[i][i]): print("Can't invert the matrix.") latex.output("\\end{array}") latex.enq() latex.output("\\\\Can't invert the matrix.\n") latex.end("Inverting the matrix") exit() matrix.mult(mat, i, 1/mat[i][i]) for j in range(len(mat)): if j == i: continue matrix.add(mat, j, -mat[j][i], i) latex_line(mat) # Output invert = [] for i in range(len(mat)): invert += [[0] * size] for j in range(size): invert[i][j] = mat[i][j + size] matrix.output(invert) # LaTeX output
with open(path) as f: data = json.load(f) # Словарь функций dictionary = { '1': matrix.add(data['first_matrix_dimensions'], data['second_matrix_dimensions'], data['first_matrix'], data['second_matrix']), '2': matrix.sub(data['first_matrix_dimensions'], data['second_matrix_dimensions'], data['first_matrix'], data['second_matrix']), '3': matrix.mult(data['first_matrix_dimensions'], data['second_matrix_dimensions'], data['first_matrix'], data['second_matrix']), '4': matrix.det(data['number'], data['first_matrix']) } # Вызов функции из словоря по списку входных данных for param in calls: if isinstance(dictionary[param], list) and not (isinstance(dictionary[param][0], int) or isinstance(dictionary[param][0], float)): for i in range(len(dictionary[param])): for j in range(len(dictionary[param][i])): print(dictionary[param][i][j], '', end='') print() elif isinstance(dictionary[param],
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])
import matrix import numpy as np mat1 = np.random.rand(8, 8) mat2 = np.random.rand(8) mat3 = np.random.rand(8, 8) r1, r2 = matrix.gauss(mat1, mat2, 8) result = matrix.mult(mat3, mat1, 8, 8) print(result)