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 cam_transform(self, camera): """camera = VCam object returns Position object representing the camera's view""" # translation inversion t = self.translate(-camera.pos[0], -camera.pos[1], -camera.pos[2]) cam_pos_col = [[camera.pos[0]], [camera.pos[1]], [camera.pos[2]]] cam_upvec_col = [[camera.upvec[0]], [camera.upvec[1]], [camera.upvec[2]]] # rotation inversion # ***maybe try quaternions here instead? # 3 perpendicular unit vecs with Z pointing towards the camera Z = matrix.normalize(matrix.subtract(t.vec[0:3], cam_pos_col)) X = matrix.normalize(matrix.crossprod(cam_upvec_col, Z)) Y = matrix.crossprod(Z, X) # camera rotation inversion matrix for the object C = [ [X[0][0], X[1][0], X[2][0], 0], # [Xaxis.x, Xaxis.y, Xaxis.z, 0] [Y[0][0], Y[1][0], Y[2][0], 0], # [Yaxis.x, Yaxis.y, Yaxis.z, 0] [Z[0][0], Z[1][0], Z[2][0], 0], # [Zaxis.x, Zaxis.y, Zaxis.z, 0] [0, 0, 0, 1] ] # the camera transformation can be done with a single matrix mult vec = matrix.multiply(C, t.vec) return Position(vec[0][0], vec[1][0], vec[2][0])
def train(self, input_array, target_array): # Generating the hidden outputs inputs = matrix.fromArray(input_array) hidden = matrix.multiply(self.weights_ih, inputs) hidden.add(self.bias_h) # activation function hidden.map(sigmoid) # Generating the output layer's output outputs = matrix.multiply(self.weights_ho, hidden) outputs.map(sigmoid) targets = matrix.fromArray(target_array) output_errors = matrix.subtract(targets, outputs) # gradient = outputs * (1 - outputs) # Calculate gradient gradients = matrix.map(outputs, dsigmoid) # get hadamard product gradients.multiply(output_errors) # perform scalar multiplication gradients.multiply(self.learning_rate) # Calculate deltas hidden_t = matrix.transpose(hidden) weight_ho_deltas = matrix.multiply(gradients, hidden_t) # Change weights by the calculated deltas self.weights_ho.add(weight_ho_deltas) # Adjust bias by the gradient self.bias_o.add(gradients) # after output errors are calculated, they are backpropagated to hidden layers for hidden layer error calculation weights_ho_t = matrix.transpose(self.weights_ho) hidden_errors = matrix.multiply(weights_ho_t, output_errors) # Calculate hidden gradient hidden_gradient = matrix.map(hidden, dsigmoid) # hadamard product hidden_gradient.multiply(hidden_errors) hidden_gradient.multiply(self.learning_rate) # Calculate input->hidden deltas inputs_t = matrix.transpose(inputs) weight_ih_deltas = matrix.multiply(hidden_gradient, inputs_t) self.weights_ih.add(weight_ih_deltas) self.bias_h.add(hidden_gradient)
def main(x, hidden, b, learning, test, w, g, n_d): random.seed(1) training_data = x[:] noise_data = n_d[:] # Adding bias to training data training_data.append([]) noise_data.append([]) for _ in x[0]: training_data[1].append(b) noise_data[1].append(b) # Random weights for synapses synapses0 = [] synapses1 = [] for _ in range(hidden): synapses0.append([random.uniform(w, -w), random.uniform(w, -w)]) # second rand for bias synapses for j in range(hidden + 1): # +1 for bias synapses1.append([random.uniform(w, -w)]) sig_layer2 = [] global loading_message global loading_progress # learning loop (learning = iterations) for i in xrange(learning): temp = i+1 loading_progress = round((float(temp) / float(iterations)) * 100, 1) # loading_progress = (temp / learning) * 100 # # # Forward pass # # Input Layer layer1 = matrix.multiply(synapses0, training_data) # Activation level sig_layer1 = matrix.sig(layer1) # # Hidden Layer # Adding bias to layer1 b_sig_layer1 = sig_layer1[:] b_sig_layer1.append([]) for _ in b_sig_layer1[0]: b_sig_layer1[len(b_sig_layer1) - 1].append(b) layer2 = matrix.multiply(matrix.transpose(synapses1), b_sig_layer1) sig_layer2 = matrix.sig(layer2) # Calculate net error error = [matrix.subtract(test, matrix.transpose(sig_layer2))] # if i % 25000 == 0: # temp = 0 # for j in range(len(error)): # temp += temp + error[0][j] # print i, temp # Delta for neuron in output layer (1 for each training data) deriv_sig_layer2 = matrix.derivative(sig_layer2) delta_layer2 = [[]] for j in range(len(error[0])): delta_layer2[0].append(deriv_sig_layer2[0][j] * error[0][j] * g) # Delta for neurons in hidden layer deriv_sig_layer1 = matrix.derivative(sig_layer1) delta_layer1 = [] delta_weight_sum = [] for k in range(len(synapses1)): delta_weight_sum.append([]) for j in range(len(delta_layer2[0])): delta_weight_sum[k].append(synapses1[k][0] * delta_layer2[0][j]) for k in range(len(deriv_sig_layer1)): delta_layer1.append([]) for j in range(len(deriv_sig_layer1[0])): delta_layer1[k].append(deriv_sig_layer1[k][j] * delta_weight_sum[k][j] * g) delta_w_oh = matrix.multiply(delta_layer2, matrix.transpose(b_sig_layer1)) delta_w_hi = matrix.multiply(delta_layer1, matrix.transpose(training_data)) # # Update weights synapses1 = matrix.add(synapses1, matrix.transpose(delta_w_oh)) synapses0 = matrix.add(synapses0, delta_w_hi) if i > learning * 0.5: if i > learning * 0.95: loading_message = "I'm nearly done, good training." else: loading_message = "Well, I'm halfway through." # Testing net with noised data sig_noise = [] if len(n_d) > 0: # print "testing with noise data" l1 = matrix.multiply(synapses0, noise_data) sig_l1 = matrix.sig(l1) b_sig_l1 = sig_l1[:] b_sig_l1.append([]) for _ in b_sig_l1[0]: b_sig_l1[len(b_sig_l1) - 1].append(b) l2 = matrix.multiply(matrix.transpose(synapses1), b_sig_l1) sig_noise = matrix.sig(l2) # formatting net output for plot result1 = [] # training data result2 = [] # noised data for i in range(len(sig_layer2[0])): result1.append(sig_layer2[0][i] * 2 - 1) result2.append(sig_noise[0][i] * 2 - 1) # Plot # Some code lines from: https://matplotlib.org/users/legend_guide.html neuron_patch = mpatches.Patch(label='Neurons: ' + str(hidden)) bias_patch = mpatches.Patch(label='Bias: ' + str(b)) iteration_patch = mpatches.Patch(label='Iterations: ' + str(learning)) epsilon_patch = mpatches.Patch(label='Epsilon: ' + str(g)) weight_patch = mpatches.Patch(label='Weight range (0 +/-): ' + str(w)) time_patch = mpatches.Patch(label=str(round((time.time() - start_time) / 60, 2)) + " min") first_legend = plt.legend( handles=[bias_patch, time_patch, epsilon_patch, neuron_patch, iteration_patch, weight_patch], bbox_to_anchor=(0., 1.02, 1., .102), loc=3, ncol=3, mode="expand", borderaxespad=0.) line1, = plt.plot(inputData[0], result1, label="Training Data", linewidth=0.75) line2, = plt.plot(inputData[0], result2, label="Test Data", linestyle=':', linewidth=0.75) line3, = plt.plot(x_data, y_data, label="sin(x)", linestyle='--', linewidth=0.75) ax = plt.gca().add_artist(first_legend) plt.legend(handles=[line1, line2, line3]) plt.savefig('./plots/plot' + str(time.time())[2:10] + '.png') plt.clf() plt.cla() plt.close()
def main(x, hidden, b, epochs, test, w, g, n_d, m, p_m): random.seed(1) training_data = x[:] noise_data = n_d[:] # Adding bias to training data training_data.append([]) noise_data.append([]) for _ in x[0]: training_data[len(training_data)-1].append(b) noise_data[len(noise_data)-1].append(b) # Random weights for synapses synapses0 = [] synapses1 = [] for f in range(hidden): synapses0.append([]) for _ in range(len(training_data)): synapses0[f].append(random.uniform(w, -w)) # second rand for bias synapses for j in range(hidden + 1): # +1 for bias synapses1.append([random.uniform(w, -w)]) sig_layer2 = [] error_log = [] error_log2 = [] gamma_log = [] global loading_message global loading_progress # learning loop (learning = iterations) for i in xrange(epochs): loading_progress = round((float(i) / float(iterations)) * 100, 1) # # # Forward pass # # Input Layer layer1 = matrix.multiply(synapses0, training_data) # Activation level sig_layer1 = matrix.sig(layer1) # # Hidden Layer # Adding bias to layer1 b_sig_layer1 = sig_layer1[:] b_sig_layer1.append([]) for _ in b_sig_layer1[0]: b_sig_layer1[len(b_sig_layer1) - 1].append(b) layer2 = matrix.multiply(matrix.transpose(synapses1), b_sig_layer1) sig_layer2 = matrix.sig(layer2) # # # ---------------- # # Calculate net error error = [matrix.subtract(test, matrix.transpose(sig_layer2))] # error = [matrix.error(test, matrix.transpose(sig_layer2))] # if i % 5000 == 0: # print(error) temp = 0 for j in range(len(error)): temp += temp + error[0][j] error_log.append(temp/len(error)) # Test with test data sig_noise = [] l1 = matrix.multiply(synapses0, noise_data) sig_l1 = matrix.sig(l1) b_sig_l1 = sig_l1[:] b_sig_l1.append([]) for _ in b_sig_l1[0]: b_sig_l1[len(b_sig_l1) - 1].append(b) l2 = matrix.multiply(matrix.transpose(synapses1), b_sig_l1) sig_noise = matrix.sig(l2) error2 = [matrix.subtract(test, matrix.transpose(sig_noise))] temp2 = 0 for j in range(len(error2)): temp2 += temp2 + error2[0][j] error_log2.append(temp2 / len(error2)) # # # ---------------- # # Calculating weight updates # Delta for neuron in output layer (1 for each training data) deriv_sig_layer2 = matrix.derivative(sig_layer2) delta_layer2 = [[]] # temp_g = (g/(i+1)) # gamma_log.append(temp_g) for j in range(len(error[0])): delta_layer2[0].append(deriv_sig_layer2[0][j] * error[0][j] * g) # Delta for neurons in hidden layer deriv_sig_layer1 = matrix.derivative(sig_layer1) delta_layer1 = [] delta_weight_sum = [] for k in range(len(synapses1)): delta_weight_sum.append([]) for j in range(len(delta_layer2[0])): delta_weight_sum[k].append(synapses1[k][0] * delta_layer2[0][j]) for k in range(len(deriv_sig_layer1)): delta_layer1.append([]) for j in range(len(deriv_sig_layer1[0])): delta_layer1[k].append(deriv_sig_layer1[k][j] * delta_weight_sum[k][j] * g) delta_w_oh = matrix.multiply(delta_layer2, matrix.transpose(b_sig_layer1)) delta_w_hi = matrix.multiply(delta_layer1, matrix.transpose(training_data)) # # # Backwards pass # # Update weights synapses1 = matrix.add(synapses1, matrix.transpose(delta_w_oh)) synapses0 = matrix.add(synapses0, delta_w_hi) if i > epochs * 0.5: if i > epochs * 0.95: loading_message = "I'm nearly done, good training." else: loading_message = "Well, I'm halfway through." # # # End of learning # Testing net with noised/test data sig_noise = [] l1 = matrix.multiply(synapses0, noise_data) sig_l1 = matrix.sig(l1) b_sig_l1 = sig_l1[:] b_sig_l1.append([]) for _ in b_sig_l1[0]: b_sig_l1[len(b_sig_l1) - 1].append(b) l2 = matrix.multiply(matrix.transpose(synapses1), b_sig_l1) sig_noise = matrix.sig(l2) # formatting net output for plot result1 = [] # training data result2 = [] # noised data for i in range(len(sig_layer2[0])): result1.append(sig_layer2[0][i] * 2 - 1) result2.append(sig_noise[0][i] * 2 - 1) if m == "sin": # Plot # Some code lines from: https://matplotlib.org/users/legend_guide.html neuron_patch = mpatches.Patch(label='Neurons: ' + str(hidden)) bias_patch = mpatches.Patch(label='Bias: ' + str(b)) iteration_patch = mpatches.Patch(label='Iterations: ' + str(epochs)) epsilon_patch = mpatches.Patch(label='Gamma: ' + str(g)) weight_patch = mpatches.Patch(label='Weight range: +/- ' + str(w)) time_patch = mpatches.Patch(label=str(round((time.time() - start_time) / 60, 2)) + " min") first_legend = plt.legend( handles=[bias_patch, time_patch, epsilon_patch, neuron_patch, iteration_patch, weight_patch], bbox_to_anchor=(0., 1.02, 1., .102), loc=3, ncol=3, mode="expand", borderaxespad=0.) line4, = plt.plot(error_axis[0], error_log, label="Error", linewidth=0.5) line6, = plt.plot(error_axis[0], error_log2, label="Error2", linewidth=0.5) line1, = plt.plot(inputData[0], result1, label="Training Data", linewidth=0.75) line2, = plt.plot(inputData[0], result2, label="Test Data", linestyle=':', linewidth=0.75) line3, = plt.plot(x_data, y_data, label="sin(x)", linestyle='--', linewidth=0.75) line5, = plt.plot(x_axis, y_axis, label="Axis", linewidth=0.5) ax = plt.gca().add_artist(first_legend) plt.legend(handles=[line4, line1, line2, line3, line5, line6]) if p_m: plt.savefig('./plots/' + str(time.time())[2:10] + '.png') else: plt.show() plt.clf() plt.cla() plt.close() elif m == "xor": print("-----") for i in range(len(sig_noise[0])): print "Input: " + str(round(noise_data[0][i], 0)) + " & " \ + str(round(noise_data[1][i], 0)) + " = " + str(round(sig_noise[0][i], 0)) + " (" \ + str(round(sig_noise[0][i] * 100, 4)) + "% for True)"
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])