def getDataArray(): # get received data-time and coordinate(возвращает собранные данные в виде 2х массивов-время и коррдинату) try: p.stdin.write(bytes('M\n', 'UTF-8')) p.stdin.flush() status = p.stdout.readline().strip().decode() meas = measurement.measurement() meas.set_Status(status) k = int(p.stdout.readline().strip().decode()) k=k-1 meas.set_Count(k) if (status == 'I' and k > 0): meas.set_Count(k) time = [] coordinate = [] while k > 0: time.append(float(p.stdout.readline().strip().decode())) coordinate.append(int(p.stdout.readline().strip().decode())) k = k - 1 meas.set_Time(time) meas.set_Coordinate(coordinate) return meas else: return meas except: raise Exception('error')
def __init__(self): #extract info from xml tree = ET.parse('config.xml') root = tree.getroot() #create control structure self.Controls = [] ListOfControls = root.findall('control') for iterator in ListOfControls: try: self.Controls.append( controlhandler.control(iterator.attrib['name'], int(iterator.attrib['setValue']), int(iterator.attrib['value']), int(iterator.attrib['minValue']), int(iterator.attrib['maxValue']), iterator.attrib['unit'], int(iterator.attrib['MsgID']), iterator.attrib['type'])) except: sys.exit("Config import crashed. Check config.xml") # create measurements structure self.Measurements = [] ListOfMeasurements = root.findall('measurement') for iterator in ListOfMeasurements: try: self.Measurements.append( meashandler.measurement(iterator.attrib['name'], int(iterator.attrib['value']), int(iterator.attrib['minValue']), int(iterator.attrib['maxValue']), iterator.attrib['unit'], int(iterator.attrib['MsgID']))) except: sys.exit("Config import crashed. Check config.xml") # misc options self.commsMethod = root.findall('communication')[0].attrib['method'] if self.commsMethod == "loopback": logging.info("Communication disabled, Loopback only") self.GUIUpdate = root.findall('GUIUpdate')[0].text #logging options root = root.findall('logging') tempLogging = {} for iterator in root: tempLogging[iterator.attrib['name']] = iterator.attrib['value'] #tempLogging['cycletime']=int(tempLogging['cycletime']) tempLogging['objList'] = [] for object in self.Controls: tempLogging['objList'].append(object) for object in self.Measurements: tempLogging['objList'].append(object) self.logging = tempLogging
def check_polar(rho0, S, m, g, CD0, b, e): max_error_ratio = 0.0 for v in range(22, 40): Ve = measurement(float(v), 0.0) sink = polar(rho0, Ve, S, m, g, CD0, b, e) error_ratio = sink.get_stdev() / sink.get_mean() if max_error_ratio < error_ratio: max_error_ratio = error_ratio return max_error_ratio
def __init__(self): super(main, self).__init__() self.setupUi(self) self.MSMT = measurement.measurement() self.thread = QThread() ################################################### self.loop1 = '' self.saving_dir = r'' self.newdirect = r"C:\Users\kaife\Desktop" self.newname = 'new file name.txt' self.newtitle = 'x y1 y2 y3 y4' self.file_list = [] self.directory = '' self.f = '' self.se_setting = [] ############################################################### self.fig_dict = {} self.header = [] self.num = 0 self.fig, self.ax, self.canvas, self.toolbar = self.initialize_fig() ############################################################################fig editor self.file_name.textChanged.connect(self.loadData) self.bbutton.clicked.connect(self.get_file) self.plot_Btn.clicked.connect(self.newPlot) self.addPlot.clicked.connect(self.add_plot) self.FileIndex.valueChanged.connect(self.scroll_file) self.PlotList.itemClicked.connect(self.change_fig) self.Xaxis.valueChanged.connect(self.update_label) self.Yaxis.valueChanged.connect(self.update_label) ######################################################################seq editor template = path + "\\template.txt" self.initialize_seq(template) self.DataSave.clicked.connect(self.get_saving_profile) self.LoadSeq.clicked.connect(self.Load_Seq) self.NewSeq.clicked.connect(self.SeqGetItem) self.ClearSeq.clicked.connect(self.clear_que) self.AbortSeq.clicked.connect(self.Abort_MS) self.StartSeq.clicked.connect(self.Start_MS) self.SaveSeq.clicked.connect(self.save_seq) self.StartSeq.clicked.connect(self.exceute) self.PauseButton.clicked.connect(self.Pause_MS) self.ResumeButton.clicked.connect(self.Resume_MS) self.QueList.itemDoubleClicked.connect(self.edit_seq)
def run_seq(self, loop1): self.Abort_MS() self.Continuous.setChecked(True) self.statusBar().showMessage('Measurement started!') fname = self.saving_dir self.file_name.setText(fname) seqlist = [ self.QueList.item(i).text() for i in range(self.QueList.count()) ] self.MSMT = measurement.measurement(seqlist, loop1, fname, self.newtitle) self.MSMT.SingleMS.connect(self.statusBar().showMessage) self.MSMT.FN_out.connect(self.get_current_data) self.MSMT.error_message.connect(self.show_message) self.MSMT.moveToThread(self.thread) self.MSMT.finished.connect(self.thread.quit) self.thread.started.connect(self.MSMT.loop_control) self.thread.start()
def click(event, x, y, flags, param): if event == cv2.EVENT_LBUTTONDOWN: refPt = [(x, y)] measure_instance = measurement() measure_instance.set_image_positions(refPt[0][0], refPt[0][1]) measure_instance.set_camera_parameters(43.603, 43.603) measure_instance.set_camera_pitch_and_height(60, 840) horizontal_angle = measure_instance.calc_horizontal_angle() vertical_angle = measure_instance.calc_vertical_angle() #print("horizontal angle : " + str(horizontal_angle)) #print("vertical angle : " + str(vertical_angle)) position_top = measure_instance.calc_3d_position( vertical_angle, horizontal_angle) debug_string = "3d position of point : " + str( position_top[0]) + ", " + str(position_top[1]) + ", " + str( position_top[2]) print(debug_string)
def run(args): #if __name__=="__main__": # initialize parameters of interest # Method: # 0: linear policy # 1: RBF policy # 2: MLP policy #method = args[0] #RBF_components = args[1] #MLP_neurons = args[2] process_index = args[3] folder_name = args[4] np.random.seed(process_index+100) #process_index = 0 #np.random.seed(process_index + 100) #vel_var = args[5] #num_targets = args[6] method = 0 RBF_components = 20 MLP_neurons = 50 vel_var = .001 num_targets = min(6,max(2,np.random.poisson(3))) num_targets = np.random.randint(2,10) #num_targets = 4 print("Starting Thread:" + str(process_index)) #Initialize all the parameters params ={0:{},1:{},2:{}} if method==0: params[0]["weight2"] = np.random.normal(0, .3, [2, num_states_layer2]) #params[0]["weight2"] = np.array([[ 3.97573312, 0.4639474 , 2.27280486, 12.9085868 , # 3.45722461, 6.36735166], #[-11.87940874, 2.59549414, -5.68556954, 2.87746786, # 7.08059984, 5.5631133 ]]) params[0]["weight"] = np.array([[7.18777985, -13.68815256, 1.69010242, -5.62483187, -4.30451483, 10.09592853], [13.33104057, 13.60537864, 3.46939294, 0.8446329, -14.79733566, -4.78599648]]) #params[0]["weight"] = np.array([[ 1.45702249, -1.17664153, -0.11593174, 1.02967173, -0.25321044, #0.09052774], #[ 0.67730786, 0.3213561 , 0.99580938, -2.39007038, -1.16340594, #-1.77515938]]) elif method==1: featurizer = sklearn.pipeline.FeatureUnion([("rbf1", RBFSampler(gamma=rbf_var, n_components=RBF_components, random_state=1))]) featurizer.fit(np.array(list_of_states)) # Use this featurizer for normalization params[1]["weight"] = np.random.normal(0, 1, [2, RBF_components]) elif method==2: params[2]["weigh1"] = np.random.normal(0, 1, [MLP_neurons, num_states]) params[2]["bias1"] = np.random.normal(0,1,[MLP_neurons,1]) params[2]["weigh2"] = np.random.normal(0, 1, [2, MLP_neurons]) params[2]["bias2"] = np.random.normal(0, 1, [2, 1]) return_saver = [] error_saver = [] episode_counter = 0 weight_saver1 = [] weight_saver2 = [] weight_saver2_1 = [] weight_saver2_2 = [] #for episode_counter in range(0,N_max): #Training parameters avg_reward = [] avg_error = [] var_reward = [] training = True result_folder = base_path+folder_name+"/" reward_file = open(result_folder+"reward_noise:"+str(vel_var)+"_"+str(process_index)+ "_linear_6states.txt","a") error_file = open(result_folder + "error_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a") error_file_median = open(result_folder + "error_median_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") var_file = open(result_folder + "var_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a") var_error_file = open(result_folder + "var_error_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a") weight_file = open(result_folder + "weight_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a") #flatten initial weight and store the values if method==0: weight = params[0]['weight'] flatted_weights = list(weight[0, :]) + list(weight[1, :]) temp = [] [temp.append(str(x)) for x in flatted_weights] weight_file.write("\t".join(temp)+"\n") elif method==1: weight = params[1]['weight'] flatted_weights = list(weight[0, :]) + list(weight[1, :]) temp = [] [temp.append(str(x)) for x in flatted_weights] weight_file.write("\t".join(temp) + "\n") elif method==2: pass #weight = np.reshape(np.array(weights[0]), [2, 6]) init_max_target = 3 num_targets = init_max_target while episode_counter<N_max: if episode_counter%1000==0 and episode_counter>0: init_max_target +=1 init_max_target = min(20,init_max_target) if episode_counter%100==0 and episode_counter>0: num_targets = np.random.randint(3,init_max_target+1) sigma = gen_learning_rate(episode_counter,sigma_max,.1,5000) sigma = sigma_max discounted_return = np.array([]) discount_vector = np.array([]) #print(episodes_counter) scen = scenario(1,1) bearing_var = 1E-2#variance of bearing measurement #Target information x = 10000*np.random.random([num_targets])-5000#initial x-location y = 10000 * np.random.random([num_targets]) - 5000#initial y-location xdot = 10*np.random.random([num_targets])-5#initial xdot-value ydot = 10 * np.random.random([num_targets]) - 5#initial ydot-value #TEMP #x = [2000,-2000] #y = [2000,2000] #xdot = [1,1] #ydot = [-1,-1] init_target_state = [] init_for_smc = [] for target_counter in range(0,num_targets): init_target_state.append([x[target_counter],y[target_counter],xdot[target_counter],ydot[target_counter]])#initialize target state init_for_smc.append([x[target_counter]+np.random.normal(0,5),y[target_counter] +np.random.normal(0,5),np.random.normal(0,5),np.random.normal(0,5)])#init state for the tracker (tracker doesn't know about the initial state) #temp_loc = np.array(init_target_state[0:2]).reshape(2,1) #init_location_estimate = temp_loc+0*np.random.normal(np.zeros([2,1]),10) #init_location_estimate = [init_location_estimate[0][0],init_location_estimate[1][0]] #init_velocity_estimate = [6*random.random()-3,6*random.random()-3] #init_velocity_estimate = [init_target_state[2],init_target_state[3]] #init_estimate = init_location_estimate+init_velocity_estimate init_covariance = np.diag([MAX_UNCERTAINTY,MAX_UNCERTAINTY,MAX_UNCERTAINTY,MAX_UNCERTAINTY])#initial covariance of state estimation t = [] for i in range(0,num_targets): t.append(target(init_target_state[i][0:2], init_target_state[i][2], init_target_state[i][3], vel_var, vel_var, "CONS_V"))#constant-velocity model for target motion A, B = t[0].constant_velocity(1E-10)#Get motion model x_var = t[0].x_var y_var = t[0].y_var tracker_object = [] for i in range(0,num_targets): tracker_object.append(EKF_tracker(init_for_smc[i], np.array(init_covariance), A,B,x_var,y_var,bearing_var))#create tracker object #smc_object = smc_tracker(A,B,x_var,y_var,bearing_var,1000,np.array(init_for_smc)) #Initialize sensor object if method==0: s = sensor("POLICY_COMM_LINEAR")#create sensor object (stochastic policy) elif method==1: s = sensor("POLICY_COMM_RBF") elif method==2: s = sensor("POLICY_COMM_MLP") measure = measurement(bearing_var)#create measurement object m = [] x_est = []; y_est = []; x_vel_est = []; y_vel_est = [] x_truth = []; y_truth = []; x_vel_truth = []; y_vel_truth = [] uncertainty = [] vel_error = [] pos_error = [] iteration = [] innovation = [] for i in range(0,num_targets): x_truth.append([]) y_truth.append([]) x_vel_truth.append([]) y_vel_truth.append([]) uncertainty.append([]) vel_error.append([]) x_est.append([]) y_est.append([]) x_vel_est.append([]) y_vel_est.append([]) pos_error.append([]) innovation.append([]) reward = [] episode_condition = True n=0 violation = 0 #store required information episode_state = [] episode_state_out_layer = [] episode_MLP_state = [] episode_actions = [] avg_uncertainty= [] max_uncertainty = [] while episode_condition: temp_m = [] input_state_temp = [] for i in range(0,num_targets): t[i].update_location() temp_m.append(measure.generate_bearing(t[i].current_location,s.current_location)) m.append(temp_m) temp_reward = [] target_actions = [] for i in range(0,num_targets): tracker_object[i].update_states(s.current_location, m[-1][i]) normalized_innovation = (tracker_object[i].innovation_list[-1])/tracker_object[i].innovation_var[-1] #print(normalized_innovation) #if (normalized_innovation<1E-4 or n<10) and n<200: #end of episode current_state = list(tracker_object[i].x_k_k.reshape(len(tracker_object[i].x_k_k))) + list(s.current_location) #print(current_state) #state normalization x_slope = 2.0/(scen.x_max-scen.x_min) y_slope = 2.0 / (scen.y_max - scen.y_min) x_slope_sensor = 2.0 / (40000) y_slope_sensor = 2.0 / (40000) vel_slope = 2.0/(scen.vel_max-scen.vel_min) #normalization current_state[0] = -1+x_slope*(current_state[0]-scen.x_min) current_state[1] = -1 + y_slope * (current_state[1] - scen.y_min) current_state[2] = -1 + vel_slope * (current_state[2] - scen.vel_min) current_state[3] = -1 + vel_slope * (current_state[3] - scen.vel_min) current_state[4] = -1 + x_slope * (current_state[4] -scen.x_min) current_state[5] = -1 + y_slope * (current_state[5] - scen.y_min) #Refactor states based on the usage if method==0 or method==2: input_state = current_state input_state_temp.append(input_state) #store input-sates elif method==1: #Generate states for the RBF input input_state = featurizer.transform(np.array(current_state).reshape(1,len(current_state))) input_state = list(input_state[0]) target_actions.append(s.generate_action(params,input_state,.01)) estimate = tracker_object[i].x_k_k episode_state.append(input_state) ####Neeed to get modified if method==2: episode_MLP_state.append(extra_information) #need to get modified truth = t[i].current_location x_est[i].append(estimate[0]) y_est[i].append(estimate[1]) x_vel_est[i].append(estimate[2]) y_vel_est[i].append(estimate[3]) x_truth[i].append(truth[0]) y_truth[i].append(truth[1]) x_vel_truth[i].append(t[i].current_velocity[0]) y_vel_truth[i].append(t[i].current_velocity[1]) vel_error[i].append(np.linalg.norm(estimate[2:4]-np.array([t[i].current_velocity[0],t[i].current_velocity[1]]).reshape(2,1))) pos_error[i].append(np.linalg.norm(estimate[0:2]-np.array(truth).reshape(2,1))) innovation[i].append(normalized_innovation[0]) unormalized_uncertainty = np.sum(tracker_object[i].p_k_k.diagonal()) #if unormalized_uncertainty>MAX_UNCERTAINTY: # normalized_uncertainty = 1 #else: # normalized_uncertainty = (1.0/MAX_UNCERTAINTY)*unormalized_uncertainty uncertainty[i].append((1.0 / MAX_UNCERTAINTY) * unormalized_uncertainty) #if len(uncertainty[i])<window_size+window_lag: # temp_reward.append(0) #else: # current_avg = np.mean(uncertainty[i][-window_size:]) # prev_avg = np.mean(uncertainty[i][-(window_size+window_lag):-window_lag]) # if current_avg<prev_avg or uncertainty[i][-1]<.1: #if current_avg < prev_avg: # temp_reward.append(1) #else: # temp_reward.append(0) this_uncertainty = [] [this_uncertainty.append(uncertainty[x][-1]) for x in range(0, num_targets)] avg_uncertainty.append(np.mean(this_uncertainty)) max_uncertainty.append(np.max(this_uncertainty)) if len(avg_uncertainty) < window_size + window_lag: reward.append(0) else: current_avg = np.mean(avg_uncertainty[-window_size:]) prev_avg = np.mean(avg_uncertainty[-(window_size + window_lag):-window_lag]) if current_avg < prev_avg or avg_uncertainty[-1] < .1: # if current_avg < prev_avg: reward.append(1) else: reward.append(0) #voting #if np.mean(temp_reward)>.5: # reward.append(np.mean(temp_reward)) #else: # reward.append(np.mean(temp_reward)) #if sum(reward)>1100 and num_targets>2: sys.exit(1) #Do something on target_actions #Create feature-vector from generated target actions normalized_state,index_matrix1,index_matrix2,slope = s.update_location_decentralized(target_actions,sigma,params) #Update the sensor location based on all individual actions #index_matrix: an n_s \times T matrix that shows the derivative of state in the output layer to the action space in the internal-layer backpropagated_to_internal_1 = index_matrix1.dot(np.array(input_state_temp))#8 by 6 backpropagated_to_internal_2 = index_matrix2.dot(np.array(input_state_temp))# 8 by 6 episode_state_out_layer.append(normalized_state) episode_state.append([backpropagated_to_internal_1,backpropagated_to_internal_2]) #each entry would be a T \times 6 matrix with T being the number of targets #reward.append(-1*uncertainty[-1]) #update return discount_vector = gamma*np.array(discount_vector) discounted_return+= (1.0*reward[-1])*discount_vector new_return = 1.0*reward[-1] list_discounted_return = list(discounted_return) list_discounted_return.append(new_return) discounted_return = np.array(list_discounted_return) list_discount_vector = list(discount_vector) list_discount_vector.append(1) discount_vector = np.array(list_discount_vector) iteration.append(n) if n>episode_length: break n+=1 #Based on the return from the episode, update parameters of the policy model #Normalize returns by the length of episode #if episode_counter%10==0 and episode_counter>0: print(weight_saver[-1]) prev_params = dict(params) condition = True for i in range(0,num_targets): if np.mean(pos_error[i])>10000: condition = False break episode_condition = False episode_counter-=1 if not condition: #print("OOPSSSS...") continue #if episode_counter%100==0 and training: #print("Starting the evaluation phase...") #training = False #episode_condition = False condition = True if episode_condition and training: normalized_discounted_return = discounted_return episode_actions = s.sensor_actions #init_weight = np.array(weight) rate = gen_learning_rate(episode_counter,learning_rate,1E-12,20000) internal_rate = gen_learning_rate(episode_counter, 3*1E-5, 1E-15, 20000) total_adjustment = np.zeros(np.shape(weight)) for e in range(0,len(episode_actions)): #calculate gradiant #state = np.array(episode_state[e]).reshape(len(episode_state[e]),1) out_state = np.array(episode_state_out_layer[e]).reshape(len(episode_state_out_layer[e]),1) backpropagated_terms = episode_state[e] #calculate gradient if method==0: deriv_with_out_state = (episode_actions[e].reshape(2, 1) - params[0]['weight2'].dot(out_state)).transpose().dot(params[0]['weight2']) #1 by n_s==> derivative of F with respect to the output state-vector internal_gradiant1 = deriv_with_out_state.dot(backpropagated_terms[0]) #1 by 6 internal_gradiant2 = deriv_with_out_state.dot(backpropagated_terms[1]) #1 by 6 internal_gradiant = np.concatenate([internal_gradiant1,internal_gradiant2]) #gradiant = ((episode_actions[e].reshape(2,1)-params[0]['weight'].dot(state)).dot(state.transpose()))/sigma**2#This is the gradiant gradiant_out_layer = ((episode_actions[e].reshape(2, 1) - params[0]['weight2'].dot(out_state)).dot( out_state.transpose())) / sigma ** 2 # This is the gradiant elif method==1: gradiant = ((episode_actions[e].reshape(2, 1) - params[1]['weight'].dot(state)).dot( state.transpose())) / sigma ** 2 # This is the gradiant elif method==2: #Gradient for MLP pass if np.max(np.abs(gradiant_out_layer))>1E2 or np.max(np.abs(internal_gradiant))>1E2: #print("OOPPSSSS...") continue #clip large gradients if method==0: adjustment_term_out_layer = gradiant_out_layer*normalized_discounted_return[e]#an unbiased sample of return adjustment_term_internal_layer = internal_gradiant*normalized_discounted_return[e] params[0]['weight2'] += rate * adjustment_term_out_layer params[0]['weight'] += internal_rate* adjustment_term_internal_layer elif method==1: adjustment_term = gradiant * normalized_discounted_return[e] # an unbiased sample of return params[1]['weight'] += rate * adjustment_term elif method==2: #Gradient for MLP pass #if not condition: # weight = prev_weight # continue episode_counter+=1 flatted_weights1 = list(params[0]['weight'][0, :]) + list(params[0]['weight'][1, :]) flatted_weights2 = list(params[0]['weight2'][0, :]) + list(params[0]['weight2'][1, :]) temp1 = [] [temp1.append(str(x)) for x in flatted_weights1] temp2 = [] [temp2.append(str(x)) for x in flatted_weights2] weight_file.write("\t".join(temp1)+"$$$"+"\t".join(temp2)+"\n") #flatted_weights = list(weight[0, :]) + list(weight[1, :]) #temp = [] #[temp.append(str(x)) for x in flatted_weights] #weight_file.write("\t".join(temp)+"\n") weight_saver1.append(params[0]['weight'][0][0]) weight_saver2.append(params[0]['weight'][1][0]) weight_saver2_1.append(params[0]['weight2'][0][0]) weight_saver2_2.append(params[0]['weight2'][1][0]) else: #print("garbage trajectory: no-update") pass #if not training: return_saver.append(sum(reward)) error_saver.append(np.mean(pos_error)) #print(len(return_saver),n) if episode_counter%100 == 0 and episode_counter>0: # if episode_counter%100==0 and episode_counter>0: print(episode_counter, np.mean(return_saver), sigma) #print(params[method]['weight']) #weight = np.reshape(np.array(weights[episode_counter]), [2, 6]) #print(weight) reward_file.write(str(np.mean(sorted(return_saver,reverse=True)[0:int(.95*len(return_saver))]))+"\n") error_file.write(str(np.mean(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n") error_file_median.write(str(np.median(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n") var_error_file.write(str(np.var(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n") var_file.write(str(np.var(sorted(return_saver,reverse=True)[0:int(.95*len(return_saver))]))+"\n") #weight_file.write(str(np.mean(return_saver)) + "\n") avg_reward.append(np.mean(sorted(return_saver)[0:int(.95*len(return_saver))])) avg_error.append(np.mean(sorted(error_saver)[0:int(.95*len(error_saver))])) var_reward.append(np.var(return_saver)) reward_file.close() var_file.close() error_file.close() error_file_median.close() var_error_file.close() weight_file.close() reward_file = open( result_folder + "reward_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") error_file = open( result_folder + "error_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") var_file = open( result_folder + "var_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") var_error_file = open( result_folder + "var_error_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") weight_file = open( result_folder + "weight_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") error_file_median = open( result_folder + "error_median_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") return_saver = [] error_saver = [] num_episodes.append(n)
initialdir=directory) if not inFiles: sys.exit('Program cancelled') path = inFiles[0] f = open(pathfile, 'w') path = inFiles[0] f.write(path) f.close() #Specify name you want to save to filename = tkFileDialog.asksaveasfilename( parent=root, title='Save file with multiple plots as:', initialdir=path) if not filename: sys.exit('Program cancelled') # Specify particle size (Optional??) size = mbox('Enter the particle diameter in um: ', entry=True) data = [] for inFile in inFiles: print(inFile) current = measurement(inFile, float(size)) if CLEANED == True: current.clean() for item in thresholds: current.findThresholdStrain(item) for item in strains: current.findResistanceAtStrain(item) data.append(current) writeToXlsx(filename, data)
def __init__(self, process_noise): self.kalman = measurement(0.0, float_inf) self.process_noise = measurement(0.0, process_noise) self.last_time = 0.0
def run(args): #if __name__=="__main__": # initialize parameters of interest # Method: # 0: linear policy # 1: RBF policy # 2: MLP policy #args = [0,20,50,0,"TEST1",.001,10] method = args[0] RBF_components = args[1] MLP_neurons = args[2] process_index = args[3] folder_name = args[4] #process_index = args[4] np.random.seed(process_index + 100) vel_var = args[5] num_targets = args[6] #method = 0 #RBF_components = 20 #MLP_neurons = 50 #vel_var = .001 #num_targets = min(6,max(2,np.random.poisson(3))) #num_targets = 2 print("Starting Thread:" + str(process_index)) #Initialize all the parameters (input && output-layers) params ={0:{},1:{},2:{}} if method==0: params[0]["weight"] = np.random.normal(0, .3, [2, output_size]) #Output-layer (maps flattened states to the actions) #params[0]["weight"] = [] #for f in range(0,filter_size): # params[0]["weight"].append(np.random.normal(0,1,[spatial_weight_size,temporal_weight_size])) #Convolution weith matrix #params[0]["weight"] = np.array([[ 1.45702249, -1.17664153, -0.11593174, 1.02967173, -0.25321044, #0.09052774], #[ 0.67730786, 0.3213561 , 0.99580938, -2.39007038, -1.16340594, #-1.77515938]]) elif method==1: featurizer = sklearn.pipeline.FeatureUnion([("rbf1", RBFSampler(gamma=rbf_var, n_components=RBF_components, random_state=1))]) featurizer.fit(np.array(list_of_states)) # Use this featurizer for normalization params[1]["weight"] = np.random.normal(0, 1, [2, RBF_components]) elif method==2: params[2]["weigh1"] = np.random.normal(0, 1, [MLP_neurons, num_states]) params[2]["bias1"] = np.random.normal(0,1,[MLP_neurons,1]) params[2]["weigh2"] = np.random.normal(0, 1, [2, MLP_neurons]) params[2]["bias2"] = np.random.normal(0, 1, [2, 1]) return_saver = [] error_saver = [] episode_counter = 0 weight_saver1 = [] weight_saver2 = [] #for episode_counter in range(0,N_max): #Training parameters avg_reward = [] avg_error = [] var_reward = [] training = True result_folder = base_path+folder_name+"/" reward_file = open(result_folder+"reward_noise:"+str(vel_var)+"_"+str(process_index)+ "_linear_6states.txt","a") error_file = open(result_folder + "error_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a") error_file_median = open(result_folder + "error_median_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") var_file = open(result_folder + "var_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a") var_error_file = open(result_folder + "var_error_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a") weight_file = open(result_folder + "weight_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a") #flatten initial weight and store the values if method==0: weight = params[0]['weight'] flatted_weights = list(weight[0, :]) + list(weight[1, :]) temp = [] [temp.append(str(x)) for x in flatted_weights] weight_file.write("\t".join(temp)+"\n") elif method==1: weight = params[1]['weight'] flatted_weights = list(weight[0, :]) + list(weight[1, :]) temp = [] [temp.append(str(x)) for x in flatted_weights] weight_file.write("\t".join(temp) + "\n") elif method==2: pass #weight = np.reshape(np.array(weights[0]), [2, 6]) init_max_target = 3 num_targets = 3 while episode_counter<N_max: if episode_counter%1000==0 and episode_counter>0: init_max_target +=1 init_max_target = min(10,init_max_target) if episode_counter%100==0 and episode_counter>0: num_targets = np.random.randint(3,init_max_target+1) num_targets = 3 sigma = gen_learning_rate(episode_counter,sigma_max,.1,5000) sigma = sigma_max discounted_return = np.array([]) discount_vector = np.array([]) #print(episodes_counter) scen = scenario(1,1) bearing_var = 1E-2#variance of bearing measurement #Target information x = 10000*np.random.random([num_targets])-5000#initial x-location y = 10000 * np.random.random([num_targets]) - 5000#initial y-location xdot = 10*np.random.random([num_targets])-5#initial xdot-value ydot = 10 * np.random.random([num_targets]) - 5#initial ydot-value init_target_state = [] init_for_smc = [] for target_counter in range(0,num_targets): init_target_state.append([x[target_counter],y[target_counter],xdot[target_counter],ydot[target_counter]])#initialize target state init_for_smc.append([x[target_counter]+np.random.normal(0,5),y[target_counter] +np.random.normal(0,5),np.random.normal(0,5),np.random.normal(0,5)])#init state for the tracker (tracker doesn't know about the initial state) #temp_loc = np.array(init_target_state[0:2]).reshape(2,1) #init_location_estimate = temp_loc+0*np.random.normal(np.zeros([2,1]),10) #init_location_estimate = [init_location_estimate[0][0],init_location_estimate[1][0]] #init_velocity_estimate = [6*random.random()-3,6*random.random()-3] #init_velocity_estimate = [init_target_state[2],init_target_state[3]] #init_estimate = init_location_estimate+init_velocity_estimate init_covariance = np.diag([MAX_UNCERTAINTY,MAX_UNCERTAINTY,MAX_UNCERTAINTY,MAX_UNCERTAINTY])#initial covariance of state estimation t = [] for i in range(0,num_targets): t.append(target(init_target_state[i][0:2], init_target_state[i][2], init_target_state[i][3], vel_var, vel_var, "CONS_V"))#constant-velocity model for target motion A, B = t[0].constant_velocity(1E-10)#Get motion model x_var = t[0].x_var y_var = t[0].y_var tracker_object = [] for i in range(0,num_targets): tracker_object.append(EKF_tracker(init_for_smc[i], np.array(init_covariance), A,B,x_var,y_var,bearing_var))#create tracker object #smc_object = smc_tracker(A,B,x_var,y_var,bearing_var,1000,np.array(init_for_smc)) #Initialize sensor object if method==0: s = sensor("POLICY_COMM_LINEAR")#create sensor object (stochastic policy) elif method==1: s = sensor("POLICY_COMM_RBF") elif method==2: s = sensor("POLICY_COMM_MLP") measure = measurement(bearing_var)#create measurement object m = [] x_est = []; y_est = []; x_vel_est = []; y_vel_est = [] x_truth = []; y_truth = []; x_vel_truth = []; y_vel_truth = [] uncertainty = [] avg_uncertainty = [] max_uncertainty = [] vel_error = [] pos_error = [] iteration = [] innovation = [] for i in range(0,num_targets): x_truth.append([]) y_truth.append([]) x_vel_truth.append([]) y_vel_truth.append([]) uncertainty.append([]) vel_error.append([]) x_est.append([]) y_est.append([]) x_vel_est.append([]) y_vel_est.append([]) pos_error.append([]) innovation.append([]) reward = [] episode_condition = True n=0 violation = 0 #store required information episode_state = [] episode_state_out_layer = [] episode_grad_with_state_w1 = [] episode_grad_with_state_w2 = [] episode_MLP_state = [] episode_actions = [] while episode_condition: temp_m = [] for i in range(0,num_targets): t[i].update_location() temp_m.append(measure.generate_bearing(t[i].current_location,s.current_location)) m.append(temp_m) temp_reward = [] target_actions = [] #create input-feature matrix input_state = np.zeros([num_states,num_targets]) #create a fixed-size matrix for input states for i in range(0,num_targets): tracker_object[i].update_states(s.current_location, m[-1][i]) normalized_innovation = (tracker_object[i].innovation_list[-1])/tracker_object[i].innovation_var[-1] #print(normalized_innovation) #if (normalized_innovation<1E-4 or n<10) and n<200: #end of episode current_state = list(tracker_object[i].x_k_k.reshape(len(tracker_object[i].x_k_k))) + list(s.current_location) #print(current_state) #state normalization x_slope = 2.0/(scen.x_max-scen.x_min) y_slope = 2.0 / (scen.y_max - scen.y_min) x_slope_sensor = 2.0 / (40000) y_slope_sensor = 2.0 / (40000) vel_slope = 2.0/(scen.vel_max-scen.vel_min) #normalization current_state[0] = -1+x_slope*(current_state[0]-scen.x_min) current_state[1] = -1 + y_slope * (current_state[1] - scen.y_min) current_state[2] = -1 + vel_slope * (current_state[2] - scen.vel_min) current_state[3] = -1 + vel_slope * (current_state[3] - scen.vel_min) current_state[4] = -1 + x_slope * (current_state[4] -scen.x_min) current_state[5] = -1 + y_slope * (current_state[5] - scen.y_min) if method==0 or method==2:input_state[:,i] = current_state #target_actions.append(s.generate_action(params,input_state,.1)) estimate = tracker_object[i].x_k_k episode_state.append(input_state) ####Neeed to get modified if method==2: episode_MLP_state.append(extra_information) #need to get modified truth = t[i].current_location x_est[i].append(estimate[0]) y_est[i].append(estimate[1]) x_vel_est[i].append(estimate[2]) y_vel_est[i].append(estimate[3]) x_truth[i].append(truth[0]) y_truth[i].append(truth[1]) x_vel_truth[i].append(t[i].current_velocity[0]) y_vel_truth[i].append(t[i].current_velocity[1]) vel_error[i].append(np.linalg.norm(estimate[2:4]-np.array([t[i].current_velocity[0],t[i].current_velocity[1]]).reshape(2,1))) pos_error[i].append(np.linalg.norm(estimate[0:2]-np.array(truth).reshape(2,1))) innovation[i].append(normalized_innovation[0]) unormalized_uncertainty = np.sum(tracker_object[i].p_k_k.diagonal()) #if unormalized_uncertainty>MAX_UNCERTAINTY: # normalized_uncertainty = 1 #else: # normalized_uncertainty = (1.0/MAX_UNCERTAINTY)*unormalized_uncertainty uncertainty[i].append((1.0 / MAX_UNCERTAINTY) * unormalized_uncertainty) #Featurize input-state using pooling input_state = list(np.max(input_state,axis=1))+list(np.min(input_state,axis=1))+\ list(np.mean(input_state, axis=1))+list(np.median(input_state,axis=1)) this_uncertainty = [] [this_uncertainty.append(uncertainty[x][-1]) for x in range(0,num_targets)] avg_uncertainty.append(np.mean(this_uncertainty)) max_uncertainty.append(np.max(this_uncertainty)) if len(avg_uncertainty) < window_size + window_lag: reward.append(0) else: current_avg = np.mean(avg_uncertainty[-window_size:]) prev_avg = np.mean(avg_uncertainty[-(window_size + window_lag):-window_lag]) if current_avg < prev_avg or avg_uncertainty[-1] < .1: # if current_avg < prev_avg: reward.append(1) else: reward.append(0) #voting #if np.mean(temp_reward)>.5: # reward.append(np.mean(temp_reward)) #else: # reward.append(np.mean(temp_reward)) #if sum(reward)>1100 and num_targets>2: sys.exit(1) #Do something on target_actions #Create feature-vector from generated target actions s.update_location_new(params,np.array(input_state).reshape([len(input_state),1]),sigma) #Output created by the CNN episode_state_out_layer.append(input_state) #reward.append(-1*uncertainty[-1]) #update return discount_vector = gamma*np.array(discount_vector) discounted_return+= (1.0*reward[-1])*discount_vector new_return = 1.0*reward[-1] list_discounted_return = list(discounted_return) list_discounted_return.append(new_return) discounted_return = np.array(list_discounted_return) list_discount_vector = list(discount_vector) list_discount_vector.append(1) discount_vector = np.array(list_discount_vector) iteration.append(n) if n>episode_length: break n+=1 #Based on the return from the episode, update parameters of the policy model #Normalize returns by the length of episode #if episode_counter%10==0 and episode_counter>0: print(weight_saver[-1]) prev_params = dict(params) condition = True for i in range(0,num_targets): if np.mean(pos_error[i])>10000: condition = False break episode_condition = False episode_counter-=1 if not condition: #print("OOPSSSS...") continue condition = True prev_params = dict(params) if episode_condition and training: normalized_discounted_return = discounted_return episode_actions = s.sensor_actions #init_weight = np.array(weight) rate = gen_learning_rate(episode_counter,learning_rate,1E-8,10000) internal_rate = gen_learning_rate(episode_counter, 5*1E-5, 1E-9, 10000) total_adjustment = np.zeros(np.shape(weight)) for e in range(0,len(episode_actions)): #calculate gradiant #state = np.array(episode_state[e]).reshape(len(episode_state[e]),1) out_state = np.array(episode_state_out_layer[e]).reshape(len(episode_state_out_layer[e]),1) #calculate gradient if method==0: predicted_action = params[0]['weight'].dot(out_state) #gradiant = ((episode_actions[e].reshape(2,1)-params[0]['weight'].dot(state)).dot(state.transpose()))/sigma**2#This is the gradiant gradiant_out_layer = ((episode_actions[e].reshape(2, 1) - predicted_action).dot( out_state.transpose())) / sigma ** 2 # This is the gradiant elif method==1: gradiant = ((episode_actions[e].reshape(2, 1) - params[1]['weight'].dot(state)).dot( state.transpose())) / sigma ** 2 # This is the gradiant elif method==2: #Gradient for MLP pass if np.max(np.abs(gradiant_out_layer))>1E2:# or np.max(np.abs(gradiant_internal[0]))>1E2: #print("OOPPSSSS...") continue #clip large gradients if method==0: adjustment_term_out_layer = gradiant_out_layer*normalized_discounted_return[e]#an unbiased sample of return params[0]['weight'] += rate * adjustment_term_out_layer elif method==1: adjustment_term = gradiant * normalized_discounted_return[e] # an unbiased sample of return params[1]['weight'] += rate * adjustment_term elif method==2: #Gradient for MLP pass #if not condition: # weight = prev_weight # continue episode_counter+=1 flatted_weights = list(params[0]['weight'][0,:]) + list(params[0]['weight'][1,:]) temp = [] [temp.append(str(x)) for x in flatted_weights] weight_file.write("\t".join(temp)+"\n") #weight_saver1.append(params[0]['weight'][0][0][0]) #weight_saver2.append(params[0]['weight'][0][1][0]) else: #print("garbage trajectory: no-update") pass #if not training: return_saver.append(sum(reward)) error_saver.append(np.mean(pos_error)) #print(len(return_saver),n) if episode_counter%100 == 0 and episode_counter>0: # if episode_counter%100==0 and episode_counter>0: print(episode_counter, np.mean(return_saver), sigma) #print(params[method]['weight']) #weight = np.reshape(np.array(weights[episode_counter]), [2, 6]) #print(weight) reward_file.write(str(np.mean(sorted(return_saver)[0:int(.95*len(return_saver))]))+"\n") error_file.write(str(np.mean(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n") error_file_median.write(str(np.median(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n") var_error_file.write(str(np.var(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n") var_file.write(str(np.var(sorted(return_saver)[0:int(.95*len(return_saver))]))+"\n") #weight_file.write(str(np.mean(return_saver)) + "\n") avg_reward.append(np.mean(sorted(return_saver)[0:int(.95*len(return_saver))])) avg_error.append(np.mean(sorted(error_saver)[0:int(.95*len(error_saver))])) var_reward.append(np.var(return_saver)) reward_file.close() var_file.close() error_file.close() error_file_median.close() var_error_file.close() weight_file.close() reward_file = open( result_folder + "reward_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") error_file = open( result_folder + "error_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") var_file = open( result_folder + "var_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") var_error_file = open( result_folder + "var_error_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") weight_file = open( result_folder + "weight_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") error_file_median = open( result_folder + "error_median_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") return_saver = [] error_saver = [] num_episodes.append(n)
def seif(): filename1 = 'z.mat' filename2 = 'aa3_dr.mat' filename3 = 'aa3_lsr2.mat' filename4 = 'c_5000.mat' include_dir = './data/' z_contents = sio.loadmat(include_dir + filename1) z = z_contents['z'] z_contents = sio.loadmat(include_dir + filename2) speed = z_contents['speed'].ravel() steering = z_contents['steering'].ravel() time = z_contents['time'].ravel() z_contents = sio.loadmat(include_dir + filename3) timeLsr = z_contents['TLsr'].ravel() L = size(timeLsr, 0) z_contents = sio.loadmat(include_dir + filename4) corresp = z_contents['corresp'] Lc = size(corresp, 0) del z_contents dt = 25e-3 G = csr_matrix((1, 1), dtype=bool) Q = diag([5.0, 0.02]) m = zeros(3) xi = zeros(3) omega = 10e4 * eye(3) m0 = zeros(0) #max active landmarks N = 20 plt.ion() fig, ax = plt.subplots(1, 1) ax.set_aspect('equal') ax.set_xlim(-100, 300) ax.set_ylim(-50, 350) ax.hold(True) plt.show(False) plt.draw() #background = fig.canvas.copy_from_bbox(ax.bbox) line1 = ax.plot(0, 0, 'b-')[0] line2 = ax.plot(-1000, 1000, 'ro', markersize=2)[0] poses = zeros([3, 5000]) stindex = 0 j = searchsorted(timeLsr, time[stindex]) timechr.sleep(3) t1 = timechr.time() for i in range(stindex, time.shape[0]): #for i in range(stindex, 10000): t3 = timechr.time() if i > stindex and i % 5000 == 0: save(include_dir + 'poses_' + str(i), poses) save(include_dir + 'landmarks_' + str(i), m) save(include_dir + 'xi_' + str(i), xi) save(include_dir + 'omega_' + str(i), omega) save(include_dir + 'm0_' + str(i), m0) save_npz(include_dir + 'G_', G) xi, omega, m, G = motion(speed[i], steering[i], dt, m0, xi, omega, m, G) m = estimate(m0, xi, omega, m) while j < L and timeLsr[j] < time[i] + dt * 1000: z1 = z[0:2, nonzero(z[0, :, j]), j].transpose((0, 2, 1))[:, :, 0] if z1.size > 0: co = correspondence(z1, m0, omega, m, G) xi, omega, m, G = measurement(z1, co, xi, omega, m, G) j += 1 if co.size > 0: n = (xi.size - 3) // 2 m0a = setdiff1d(m0, co) #returns float when m0 empty tq = hstack((m0a, unique(co))).astype(int) tq = tq[max(0, tq.size - N):] m1 = setdiff1d(m0, tq) m1 = union1d(m1, setdiff1d(co, tq)) m0 = tq if m1.size > 0: xi, omega, G = sparsification(m0, m1, xi, omega, m, G) print("\x1b[2J\x1b[H") t2 = timechr.time() print('iter: ' + str(i)) print('iter time: {0:.5f}'.format(t2 - t3)) print('avg: {0:.5f}'.format((t2 - t1) / (i + 1))) poses[:, i % 5000] = m[0:3] if i % 5000 == 4999: line2.set_data(m[3::2], m[4::2]) line1.set_xdata(hstack((line1.get_xdata(), poses[0, :]))) line1.set_ydata(hstack((line1.get_ydata(), poses[1, :]))) #fig.canvas.restore_region(background) #ax.draw_artist(line1) #ax.draw_artist(line2) fig.canvas.blit(ax.bbox) fig.canvas.draw()
from __future__ import division import sys import time from PyQt5.QtCore import QThread from PyQt5 import QtCore, QtGui, QtWidgets, QtTest, uic import numpy as np import sip sip.setapi('QString', 2) import worker #############################################################self made modules import measurement import QueEditor as AddQue import SecondExcite as SE_control MSMT = measurement.measurement() path = sys.path[0] vtiTempControlGUI = path + r'\measurement.ui' Ui_MainWindow, QtBaseClass = uic.loadUiType(vtiTempControlGUI) class main(QtWidgets.QMainWindow, Ui_MainWindow, QtWidgets.QFileDialog, QtWidgets.QMessageBox, QtWidgets.QInputDialog): def __init__(self): super(main, self).__init__() self.thread = QThread() self.obj = worker.Worker() self.obj.moveToThread(self.thread)
int(x) for x in classes[0].tolist() ], int(num[0]) def close(self): self.sess.close() self.default_graph.close() if __name__ == "__main__": model_path = 'D:/Workspace/PedestrianDetection/pedestrian_detection/frozen_inference_graph.pb' odapi = DetectorAPI(path_to_ckpt=model_path) threshold = 0.9 trackerType = "MEDIANFLOW" print('Starting application ...') msr = measurement() msr.test_call_method(10, 15) ## Counter data holder people_count = 0 ## Select boxes bboxes = [] colors = [] persons = [] cv2.namedWindow("image") cv2.setMouseCallback("image", click) cap = cv2.VideoCapture(0)
def run(args): # initialize parameters of interest # Method: # 0: linear policy # 1: RBF policy # 2: MLP policy vel_var = args[0] heading_rate = args[1] experiment_folder_name = args[2] file = open( base_path + "/" + experiment_folder_name + "/best_data_" + str(heading_rate), "w") # initialize actor parameters MAX_UNCERTAINTY = 1E9 num_states = 6 weight = np.random.normal(0, 1, [2, num_states]) sigma_max = 1 num_episodes = [] gamma = .99 episode_length = 1500 learning_rate = 1E-3 min_learning_rate = 1E-6 N_max = 200 window_size = 50 window_lag = 10 return_saver = [] weight_saver1 = [] weight_saver2 = [] total_error = {} total_error_variance = {} total_reward = {} #for episode_counter in range(0,N_max): #Training parameters print("heading-rate=" + str(heading_rate)) for xdot_sensor in np.arange(-15, 16, 1): for ydot_sensor in np.arange(-15, 16, 1): episode_counter = 0 avg_reward = [] var_reward = [] error_saver = [] while episode_counter < N_max: sigma = gen_learning_rate(episode_counter, sigma_max, .1, 5000) sigma = sigma_max discounted_return = np.array([]) discount_vector = np.array([]) #print(episodes_counter) scen = scenario(1, 1) bearing_var = 1E-2 #variance of bearing measurement #Target information x = 10000 * random.random() - 5000 #initial x-location y = 10000 * random.random() - 5000 #initial y-location xdot = 10 * random.random() - 5 #initial xdot-value ydot = 10 * random.random() - 5 #initial ydot-value #x = 250; y = 50; xdot = 7; ydot = -5 init_target_state = [x, y, xdot, ydot] #initialize target state init_for_smc = [ x + np.random.normal(0, 5), y + np.random.normal(0, 5), np.random.normal(0, 5), np.random.normal(0, 5) ] #init state for the tracker (tracker doesn't know about the initial state) #init_for_smc = [x, y, xdot, ydot] init_sensor_state = [ 10000 * random.random() - 5000, 10000 * random.random() - 5000, 3, -2 ] #initial sensor-state temp_loc = np.array(init_target_state[0:2]).reshape(2, 1) init_location_estimate = temp_loc + 0 * np.random.normal( np.zeros([2, 1]), 10) init_location_estimate = [ init_location_estimate[0][0], init_location_estimate[1][0] ] init_velocity_estimate = [ 6 * random.random() - 3, 6 * random.random() - 3 ] init_velocity_estimate = [ init_target_state[2], init_target_state[3] ] init_estimate = init_location_estimate + init_velocity_estimate init_covariance = np.diag([ MAX_UNCERTAINTY, MAX_UNCERTAINTY, MAX_UNCERTAINTY, MAX_UNCERTAINTY ]) #initial covariance of state estimation t = target( init_target_state[0:2], init_target_state[2], init_target_state[3], vel_var, vel_var, "CONS_V") #constant-velocity model for target motion A, B = t.constant_velocity(1E-10) #Get motion model x_var = t.x_var y_var = t.y_var tracker_object = EKF_tracker( init_for_smc, init_covariance, A, B, x_var, y_var, bearing_var) #create tracker object #smc_object = smc_tracker(A,B,x_var,y_var,bearing_var,1000,np.array(init_for_smc)) s = sensor( "CONS_V", [0, 0], [xdot_sensor, ydot_sensor], heading_rate) #create sensor object (stochastic policy) #s = sensor("CONS_V") measure = measurement(bearing_var) #create measurement object m = [] x_est = [] y_est = [] x_vel_est = [] y_vel_est = [] x_truth = [] y_truth = [] x_vel_truth = [] y_vel_truth = [] uncertainty = [] vel_error = [] pos_error = [] iteration = [] innovation = [] reward = [] episode_condition = True n = 0 violation = 0 #store required information episode_state = [] episode_actions = [] while episode_condition: #if n>50: episode_condition=False #update location of target and sensor + generate new measurement #Also, run tracker object t.update_location() m.append( measure.generate_bearing(t.current_location, s.current_location)) tracker_object.update_states(s.current_location, m[-1]) #if len(tracker_object.meas_vec)>20: # tmp = np.zeros([2,2]) # for n in range(0,10): # vector = tracker_object.meas_vec[-1-n] # cov = (vector.transpose().dot(vector))/bearing_var # sliced_cov = np.array([[cov[0,0],cov[0,1]],[cov[1,0],cov[1,1]]]) # tmp+= sliced_cov #Fisher_matrix = tmp/10.0 #crlb = np.linalg.inv(Fisher_matrix) #print(crlb.diagonal()) #create state-vector normalized_innovation = ( tracker_object.innovation_list[-1] ) / tracker_object.innovation_var[-1] #print(normalized_innovation) #if (normalized_innovation<1E-4 or n<10) and n<200: #end of episode current_state = list( tracker_object.x_k_k.reshape(len( tracker_object.x_k_k))) + list(s.current_location) #print(current_state) #state normalization x_slope = 2.0 / (scen.x_max - scen.x_min) y_slope = 2.0 / (scen.y_max - scen.y_min) vel_slope = 2.0 / (scen.vel_max - scen.vel_min) #normalization current_state[0] = -1 + x_slope * (current_state[0] - scen.x_min) current_state[1] = -1 + y_slope * (current_state[1] - scen.y_min) current_state[2] = -1 + vel_slope * (current_state[2] - scen.vel_min) current_state[3] = -1 + vel_slope * (current_state[3] - scen.vel_min) current_state[4] = -1 + x_slope * (current_state[4] - scen.x_min) current_state[5] = -1 + y_slope * (current_state[5] - scen.y_min) s.update_location(weight, sigma, np.array(current_state)) estimate = tracker_object.x_k_k episode_state.append(current_state) truth = t.current_location x_est.append(estimate[0]) y_est.append(estimate[1]) x_vel_est.append(estimate[2]) y_vel_est.append(estimate[3]) x_truth.append(truth[0]) y_truth.append(truth[1]) x_vel_truth.append(t.current_velocity[0]) y_vel_truth.append(t.current_velocity[1]) #print(estimate[-1]) #print(np.linalg.norm(estimate[2:4]-np.array([t.current_velocity[0],t.current_velocity[1]]))) vel_error.append( np.linalg.norm(estimate[2:4] - np.array([ t.current_velocity[0], t.current_velocity[1] ]).reshape(2, 1))) pos_error.append( np.linalg.norm(estimate[0:2] - np.array(truth).reshape(2, 1))) innovation.append(normalized_innovation[0]) unormalized_uncertainty = np.sum( tracker_object.p_k_k.diagonal()) #if unormalized_uncertainty>MAX_UNCERTAINTY: # normalized_uncertainty = 1 #else: # normalized_uncertainty = (1.0/MAX_UNCERTAINTY)*unormalized_uncertainty uncertainty.append( (1.0 / MAX_UNCERTAINTY) * unormalized_uncertainty) if len(uncertainty) < window_size + window_lag: reward.append(0) else: current_avg = np.mean(uncertainty[-window_size:]) prev_avg = np.mean( uncertainty[-(window_size + window_lag):-window_lag]) if current_avg < prev_avg or uncertainty[-1] < .1: #if current_avg < prev_avg: reward.append(1) else: reward.append(0) #reward.append(-1*uncertainty[-1]) #update return discount_vector = gamma * np.array(discount_vector) #discount_vector = list(discount_vector) #discount_vector.append(1) discounted_return += (1.0 * reward[-1]) * discount_vector new_return = 1.0 * reward[-1] list_discounted_return = list(discounted_return) list_discounted_return.append(new_return) discounted_return = np.array(list_discounted_return) list_discount_vector = list(discount_vector) list_discount_vector.append(1) discount_vector = np.array(list_discount_vector) iteration.append(n) if n > episode_length: break n += 1 num_episodes.append(n) error_saver.append(np.mean(pos_error)) return_saver.append(sum(reward)) episode_counter += 1 total_error[str(xdot_sensor) + "|" + str(ydot_sensor)] = np.mean( sorted(error_saver)[0:int(.95 * N_max)]) total_reward[str(xdot_sensor) + "|" + str(ydot_sensor)] = np.mean( sorted(return_saver, reverse=True)[0:int(.95 * N_max)]) total_error_variance[str(xdot_sensor) + "|" + str(ydot_sensor)] = np.var( sorted(error_saver)[0:int(.95 * N_max)]) sorted_error = sorted(total_error.items(), key=operator.itemgetter(1)) key = sorted_error[0][0] file.write("Min Error=" + str(sorted_error[0][1]) + "\n") file.write("Best params=" + str(key) + "\n") file.close()
tracker_object = [] for i in range(0, num_targets): tracker_object.append( EKF_tracker(init_for_smc[i], np.array(init_covariance), A, B, x_var, y_var, bearing_var)) #create tracker object #smc_object = smc_tracker(A,B,x_var,y_var,bearing_var,1000,np.array(init_for_smc)) #create JPDAF-tracker too jpdaf_object = JPDAF_agent(tracker_object, .004, pd, 1E-5) s = sensor("POLICY_COMM_LINEAR" ) # create sensor object (stochastic policy) #s = sensor("CONS_V") measure = measurement( bearing_var) #create measurement object m = [] x_est = [] y_est = [] x_vel_est = [] y_vel_est = [] x_truth = [] y_truth = [] x_vel_truth = [] y_vel_truth = [] uncertainty = [] vel_error = [] pos_error = [] iteration = [] innovation = []
import model import pandas as pd import numpy as np import matplotlib.pyplot as plt from measurement import measurement, query, plot_scope, plot_an data = measurement(28) data.model('Acoustical Cavity Transient Response in Frequency Domain', 'Voltage (dBV)', 'Frequency (kHz)', plotSc=False) # mymodel = model.lrcSquare(.33333, numPoints=10000, sampleSpacing=1/2500) # mymodel.model(style='-', alpha=.5) plt.legend() # plt.axvline(x=6.103, ymax=.88, color='k', linestyle='dashed') # plt.xticks(list(plt.xticks()[0]) + [6.103]) # plt.ylim(-155,40) # plt.xlim(0, 10) plt.show() # p1=pd.read_csv('/home/johnny/kod/py/bin/venv/py3/phx444/labs/lab3/data/march1/MarchFirst/F0010CH1.CSV') # p1.columns = ['no','no2','no3','time','volt', 'no4'] # p2=pd.read_csv('/home/johnny/kod/py/bin/venv/py3/phx444/labs/lab3/data/march1/MarchFirst/F0010CH2.CSV') # p2.columns = ['no','no2','no3','time','volt', 'no4'] # time = p1['time'] # volt = p1['volt']
n = 0 metric_obj = metric(1, num_sensors) episode_state = [] for sensor_index in range(0, num_sensors): episode_state.append([]) while episode_condition: t[0].update_location() if t[0].current_location[0] > scen.x_max or t[0].current_location[ 0] < scen.x_min or t[0].current_location[ 1] > scen.y_max or t[0].current_location[ 1] < scen.y_min: break #m.append(measure.generate_bearing(t.current_location, s.current_location)) for sensor_index in range(0, num_sensors): s[sensor_index].gen_measurements(t, measurement(bearing_var), 1, 0) s[sensor_index].update_track_estimaes() #Move the sensor input_states = s[sensor_index].move_sensor( scen, sensor_params[sensor_index], v_max, coeff, alpha1, alpha2, alpha1_, alpha2_, sigma) episode_state[sensor_index].append(input_states[0]) #generate local reward s[sensor_index].gen_sensor_reward(MAX_UNCERTAINTY, window_size, window_lag) #Update estimate of global fusion_agent.update_global_memoryless(s, feedback=True)
def cost(rho0, v_e, S, m, g, CD0, b, e, time_seconds): unit_altitude_cost = -polar(rho0, v_e, S, m, g, CD0, b, e) altitude_cost = unit_altitude_cost * time_seconds return altitude_cost def check_polar(rho0, S, m, g, CD0, b, e): max_error_ratio = 0.0 for v in range(22, 40): Ve = measurement(float(v), 0.0) sink = polar(rho0, Ve, S, m, g, CD0, b, e) error_ratio = sink.get_stdev() / sink.get_mean() if max_error_ratio < error_ratio: max_error_ratio = error_ratio return max_error_ratio rho0 = measurement(1.225, 0.0) S = measurement(14.0, 0.0) b = measurement(15.0, 0.0) m = measurement(350.0, 0.0) g = measurement(9.81, 0.0) e = measurement(0.88, 0.0) cd0 = measurement(0.0125, 0.0) def solve_polar(rho0, S, m, g, b, Ve1, Ve2, D1, D2): A = (rho0 * S)/(m * g * 2) B = (m * g * 2)/(rho0 * math.pi * b**2) CD0_teller = D2 - ((D1 * Ve1)/Ve2) CD0_noemer = A * (Ve2**3 - (Ve1**4 / Ve2)) CD0 = CD0_teller / CD0_noemer e = B / (D1 * Ve1 - A * Ve1**4 * CD0)
from measurement import measurement from particle_filter import particle_filter import numpy as np import matplotlib.pyplot as plt import imageio import cv2 M = 50 # Num. of particles num_img = 1019 p = [] for i in range(M): p.append(np.random.randint(1, 201, size=(2, ))) # Initialize particles for i in range(num_img): img = imageio.imread("../images/snake_color/snake_" + str(i).zfill(4) + ".png") img[:, :, [0, 2]] = img[:, :, [2, 0]] p = particle_filter(p, measurement(img), M)
A - linearized state transition matrix W - Process noise covariance V - Measurement noise covariance rN - Vector measurement in newtonian frame rB - Vector measurement in body frame ''' # get measurement vector rB1 = np.reshape(rB1hist[:, ii + 1], (3, 1)) rB2 = np.reshape(rB2hist[:, ii + 1], (3, 1)) rB = np.vstack((rB1, rB2)) xn, A = predict(xk, whist[:, ii + 1], dt) Pn = A * Pk * np.transpose(A) + W # run measurement step y, R, C = measurement(xn[0:4], rN) # run innovation step to find z z, S = innovation(R, rN, rB, Pn, V, C) # Kalman Gain L = Pn @ np.transpose(C) @ LA.inv(S) # update step xk, Pk = update(L, z, xn, Pn, V, C) q_mekf[0:4, ii + 1] = np.reshape(xk[0:4], 4) b_mekf[0:3, ii + 1] = np.reshape(xk[4:7], 3) q_model[0:4, ii + 1] = np.reshape(xn[0:4], 4) # save to mat file to process in matlab savemat('output_mekf.mat', {
init_for_tracker = [ x + np.random.normal(0, 5), y + np.random.normal(0, 5), np.random.normal(0, 5), np.random.normal(0, 5) ] A, B = t[0].constant_velocity(1E-10) # Get motion model x_var = t[0].x_var y_var = t[0].y_var tracker_object = EKF_tracker(init_for_tracker, init_covariance, A, B, x_var, y_var, bearing_var) clean_agent = clean_tracker_agent([tracker_object]) temp_sensor_object.set_tracker_objects(clean_agent) s = temp_sensor_object measure = measurement(bearing_var) episode_condition = True n = 0 metric_obj = metric(1, 1) episode_state = [] while episode_condition: t[0].update_location() #m.append(measure.generate_bearing(t.current_location, s.current_location)) s.gen_measurements(t, measure, 1, 0) s.update_track_estimaes() #Move the sensor input_states = s.move_sensor(scen, params, v_max, coeff, alpha1, alpha2, alpha1_, alpha2_, sigma) episode_state.append(input_states[0]) #Calculate immediate reward
allEnsembles if options.ensembles=='all' else options.ensembles.split(',')) ensSlice = ((None,None) if not options.ensSlice else tuple(int(i) for i in options.ensSlice.split(':')) if ':' in options.ensSlice else (int(options.ensSlice),1+int(options.ensSlice))) templates = ((0,0) if not options.templates else tuple(int(i) for i in options.templates.split(':')) if ':' in options.templates else (int(options.templates),1+int(options.templates))) if options.batch: chunksize = 10 stack = [] for part in partitions: syschunks = chunk(systs, chunksize) enschunks = chunktuple(ensSlice, chunksize) tmpchunks = chunktuple(templates, chunksize) if syschunks: stack.extend(["./start.py --partition %s --systematics %s"%(part, ','.join(s)) for s in syschunks]) if templates: stack.extend(["./start.py --partition %s --templates %d:%d"%((part,)+t) for t in tmpchunks]) if enschunks: stack.extend(["./start.py --partition %s --ensembles %s --ensSlice %d:%d"%((part, e)+s) for s in enschunks for e in ensembles]) if not any([syschunks,tmpchunks,enschunks]): stack.append("./start.py --partition %s"%part) batch.batch(stack, site=options.site) else: for part in partitions: for tID in ([None] if templates[0]==templates[1] else range(channel_data.nTemplates)[slice(*templates)]): mp = systematics.measurement_pars(partition=part) mp.update({'doVis':options.visualize, 'evalSystematics':systs if tID == None else [], 'ensembles':ensembles if tID == None else [], 'ensSlice':ensSlice, 'templateID':tID}) print mp measurement(**mp)
def run(args): # initialize parameters of interest # Method: # 0: linear policy # 1: RBF policy # 2: MLP policy method = args[0] RBF_components = args[1] MLP_neurons = args[2] process_index = args[3] folder_name = args[4] np.random.seed(1 + 100) vel_var = args[5] np.random.seed(process_index) print("Starting Thread:" + str(process_index)) #Initialize all the parameters params ={0:{},1:{},2:{}} if method==0: params[0]["weight"] = np.random.normal(0, .3, [2, num_states]) #params[0]["weight"] = np.array([[ 1.45702249, -1.17664153, -0.11593174, 1.02967173, -0.25321044, #0.09052774], #[ 0.67730786, 0.3213561 , 0.99580938, -2.39007038, -1.16340594, #-1.77515938]]) elif method==1: featurizer = sklearn.pipeline.FeatureUnion([("rbf1", RBFSampler(gamma=rbf_var, n_components=RBF_components, random_state=1))]) featurizer.fit(np.array(list_of_states)) # Use this featurizer for normalization params[1]["weight"] = np.random.normal(0, 1, [2, RBF_components]) elif method==2: params[2]["weigh1"] = np.random.normal(0, 1, [MLP_neurons, num_states]) params[2]["bias1"] = np.random.normal(0,1,[MLP_neurons,1]) params[2]["weigh2"] = np.random.normal(0, 1, [2, MLP_neurons]) params[2]["bias2"] = np.random.normal(0, 1, [2, 1]) return_saver = [] error_saver = [] episode_counter = 0 weight_saver1 = [] weight_saver2 = [] #for episode_counter in range(0,N_max): #Training parameters avg_reward = [] avg_error = [] var_reward = [] training = True result_folder = base_path+folder_name+"/" reward_file = open(result_folder+"reward_noise:"+str(vel_var)+"_"+str(process_index)+ "_linear_6states.txt","a") error_file = open(result_folder + "error_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a") error_file_median = open(result_folder + "error_median_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") var_file = open(result_folder + "var_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a") var_error_file = open(result_folder + "var_error_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a") weight_file = open(result_folder + "weight_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a") #flatten initial weight and store the values if method==0: weight = params[0]['weight'] flatted_weights = list(weight[0, :]) + list(weight[1, :]) temp = [] [temp.append(str(x)) for x in flatted_weights] weight_file.write("\t".join(temp)+"\n") elif method==1: weight = params[1]['weight'] flatted_weights = list(weight[0, :]) + list(weight[1, :]) temp = [] [temp.append(str(x)) for x in flatted_weights] weight_file.write("\t".join(temp) + "\n") elif method==2: pass #weight = np.reshape(np.array(weights[0]), [2, 6]) sigma = sigma_max while episode_counter<N_max: #sigma = gen_learning_rate(episode_counter,sigma_max,.1,20000) if episode_counter%1500==0 and episode_counter>0: sigma-= .15 sigma = max(.1,sigma) #sigma = sigma_max discounted_return = np.array([]) discount_vector = np.array([]) #print(episodes_counter) scen = scenario(1,1) bearing_var = 1E-2#variance of bearing measurement #Target information x = 10000*random.random()-5000#initial x-location y = 10000 * random.random() - 5000#initial y-location xdot = 10*random.random()-5#initial xdot-value ydot = 10 * random.random() - 5#initial ydot-value init_target_state = [x,y,xdot,ydot]#initialize target state init_for_smc = [x+np.random.normal(0,5),y+np.random.normal(0,5),np.random.normal(0,5),np.random.normal(0,5)]#init state for the tracker (tracker doesn't know about the initial state) #init_for_smc = [x, y, xdot, ydot] init_sensor_state = [10000*random.random()-5000,10000 * random.random() - 5000,3,-2]#initial sensor-state temp_loc = np.array(init_target_state[0:2]).reshape(2,1) init_location_estimate = temp_loc+0*np.random.normal(np.zeros([2,1]),10) init_location_estimate = [init_location_estimate[0][0],init_location_estimate[1][0]] init_velocity_estimate = [6*random.random()-3,6*random.random()-3] init_velocity_estimate = [init_target_state[2],init_target_state[3]] init_estimate = init_location_estimate+init_velocity_estimate init_covariance = np.diag([MAX_UNCERTAINTY,MAX_UNCERTAINTY,MAX_UNCERTAINTY,MAX_UNCERTAINTY])#initial covariance of state estimation t = target(init_target_state[0:2], init_target_state[2], init_target_state[3], vel_var, vel_var, "CONS_V")#constant-velocity model for target motion A, B = t.constant_velocity(1E-10)#Get motion model x_var = t.x_var y_var = t.y_var tracker_object = EKF_tracker(init_for_smc, init_covariance, A,B,x_var,y_var,bearing_var)#create tracker object #smc_object = smc_tracker(A,B,x_var,y_var,bearing_var,1000,np.array(init_for_smc)) #Initialize sensor object if method==0: s = sensor("POLICY_COMM_LINEAR")#create sensor object (stochastic policy) elif method==1: s = sensor("POLICY_COMM_RBF") elif method==2: s = sensor("POLICY_COMM_MLP") measure = measurement(bearing_var)#create measurement object m = [] x_est = []; y_est = []; x_vel_est = []; y_vel_est = [] x_truth = []; y_truth = []; x_vel_truth = []; y_vel_truth = [] uncertainty = [] vel_error = [] pos_error = [] iteration = [] innovation = [] reward = [] episode_condition = True n=0 violation = 0 #store required information episode_state = [] episode_MLP_state = [] episode_actions = [] while episode_condition: t.update_location() m.append(measure.generate_bearing(t.current_location,s.current_location)) tracker_object.update_states(s.current_location, m[-1]) normalized_innovation = (tracker_object.innovation_list[-1])/tracker_object.innovation_var[-1] #print(normalized_innovation) #if (normalized_innovation<1E-4 or n<10) and n<200: #end of episode current_state = list(tracker_object.x_k_k.reshape(len(tracker_object.x_k_k))) + list(s.current_location) #print(current_state) #state normalization x_slope = 2.0/(scen.x_max-scen.x_min) y_slope = 2.0 / (scen.y_max - scen.y_min) x_slope_sensor = 2.0 / (40000) y_slope_sensor = 2.0 / (40000) vel_slope = 2.0/(scen.vel_max-scen.vel_min) #normalization current_state[0] = -1+x_slope*(current_state[0]-scen.x_min) current_state[1] = -1 + y_slope * (current_state[1] - scen.y_min) current_state[2] = -1 + vel_slope * (current_state[2] - scen.vel_min) current_state[3] = -1 + vel_slope * (current_state[3] - scen.vel_min) current_state[4] = -1 + x_slope * (current_state[4] -scen.x_min) current_state[5] = -1 + y_slope * (current_state[5] - scen.y_min) #Refactor states based on the usage if method==0 or method==2: input_state = current_state elif method==1: #Generate states for the RBF input input_state = featurizer.transform(np.array(current_state).reshape(1,len(current_state))) input_state = list(input_state[0]) extra_information = s.update_location_new(params,input_state,sigma) estimate = tracker_object.x_k_k episode_state.append(input_state) if method==2: episode_MLP_state.append(extra_information) #Output of the first layer for Gradient calculation truth = t.current_location x_est.append(estimate[0]) y_est.append(estimate[1]) x_vel_est.append(estimate[2]) y_vel_est.append(estimate[3]) x_truth.append(truth[0]) y_truth.append(truth[1]) x_vel_truth.append(t.current_velocity[0]) y_vel_truth.append(t.current_velocity[1]) vel_error.append(np.linalg.norm(estimate[2:4]-np.array([t.current_velocity[0],t.current_velocity[1]]).reshape(2,1))) pos_error.append(np.linalg.norm(estimate[0:2]-np.array(truth).reshape(2,1))) innovation.append(normalized_innovation[0]) unormalized_uncertainty = np.sum(tracker_object.p_k_k.diagonal()) #if unormalized_uncertainty>MAX_UNCERTAINTY: # normalized_uncertainty = 1 #else: # normalized_uncertainty = (1.0/MAX_UNCERTAINTY)*unormalized_uncertainty uncertainty.append((1.0 / MAX_UNCERTAINTY) * unormalized_uncertainty) if len(uncertainty)<window_size+window_lag: reward.append(0) else: current_avg = np.mean(uncertainty[-window_size:]) prev_avg = np.mean(uncertainty[-(window_size+window_lag):-window_lag]) if current_avg<prev_avg or uncertainty[-1]<.1: #if current_avg < prev_avg: reward.append(1) else: reward.append(0) #reward.append(-1*uncertainty[-1]) #update return discount_vector = gamma*np.array(discount_vector) discounted_return+= (1.0*reward[-1])*discount_vector new_return = 1.0*reward[-1] list_discounted_return = list(discounted_return) list_discounted_return.append(new_return) discounted_return = np.array(list_discounted_return) list_discount_vector = list(discount_vector) list_discount_vector.append(1) discount_vector = np.array(list_discount_vector) iteration.append(n) if n>episode_length: break n+=1 #Based on the return from the episode, update parameters of the policy model #Normalize returns by the length of episode #if episode_counter%10==0 and episode_counter>0: print(weight_saver[-1]) prev_params = dict(params) condition = True if np.mean(pos_error)>10000: continue episode_condition = False episode_counter-=1 #if episode_counter%100==0 and training: #print("Starting the evaluation phase...") #training = False #episode_condition = False condition = True if episode_condition and training: normalized_discounted_return = discounted_return episode_actions = s.sensor_actions #init_weight = np.array(weight) rate = gen_learning_rate(episode_counter,learning_rate,1E-8,10000) total_adjustment = np.zeros(np.shape(weight)) for e in range(0,len(episode_actions)): #calculate gradiant state = np.array(episode_state[e]).reshape(len(episode_state[e]),1) #calculate gradient if method==0: gradiant = ((episode_actions[e].reshape(2,1)-params[0]['weight'].dot(state)).dot(state.transpose()))/sigma**2#This is the gradiant elif method==1: gradiant = ((episode_actions[e].reshape(2, 1) - params[1]['weight'].dot(state)).dot( state.transpose())) / sigma ** 2 # This is the gradiant elif method==2: #Gradient for MLP pass if np.max(np.abs(gradiant))>1E2: continue #clip large gradients if method==0: adjustment_term = gradiant*normalized_discounted_return[e]#an unbiased sample of return params[0]['weight'] += rate * adjustment_term elif method==1: adjustment_term = gradiant * normalized_discounted_return[e] # an unbiased sample of return params[1]['weight'] += rate * adjustment_term elif method==2: #Gradient for MLP pass #if not condition: # weight = prev_weight # continue episode_counter+=1 #flatted_weights = list(weight[0, :]) + list(weight[1, :]) #temp = [] #[temp.append(str(x)) for x in flatted_weights] #weight_file.write("\t".join(temp)+"\n") #weight_saver1.append(weight[0][0]) #weight_saver2.append(weight[0][1]) else: #print("garbage trajectory: no-update") pass #if not training: return_saver.append(sum(reward)) error_saver.append(np.mean(pos_error)) #print(len(return_saver),n) if episode_counter%100 == 0 and episode_counter>0: # if episode_counter%100==0 and episode_counter>0: print(episode_counter, np.mean(return_saver), sigma) #print(params[method]['weight']) #weight = np.reshape(np.array(weights[episode_counter]), [2, 6]) #print(weight) reward_file.write(str(np.mean(sorted(return_saver)[0:int(.95*len(return_saver))]))+"\n") error_file.write(str(np.mean(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n") error_file_median.write(str(np.median(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n") var_error_file.write(str(np.var(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n") var_file.write(str(np.var(sorted(return_saver)[0:int(.95*len(return_saver))]))+"\n") #weight_file.write(str(np.mean(return_saver)) + "\n") avg_reward.append(np.mean(sorted(return_saver)[0:int(.95*len(return_saver))])) avg_error.append(np.mean(sorted(error_saver)[0:int(.95*len(error_saver))])) var_reward.append(np.var(return_saver)) reward_file.close() var_file.close() error_file.close() error_file_median.close() var_error_file.close() weight_file.close() reward_file = open( result_folder + "reward_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") error_file = open( result_folder + "error_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") var_file = open( result_folder + "var_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") var_error_file = open( result_folder + "var_error_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") weight_file = open( result_folder + "weight_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") error_file_median = open( result_folder + "error_median_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a") return_saver = [] error_saver = [] num_episodes.append(n)
#cv2.circle(img,(x,y),3,(255,0,0),-1) self.points.append((x, y)) if __name__ == "__main__": cv2.namedWindow('image') top_flag = 0 x_top = y_top = x_bottom = y_bottom = 0 coord_store = CoordinateStore() cv2.setMouseCallback('image', coord_store.select_point) measure = measurement() # Configure depth and color streams pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.color, 1920, 1080, rs.format.bgr8, 30) # define the upper and lower boundaries of the HSV pixel # intensities to be considered 'skin' lower = np.array([0, 48, 80], dtype="uint8") upper = np.array([20, 255, 255], dtype="uint8") # Start streaming profile = pipeline.start(config)