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)
