#print("U: ", U[0:2]) q1.set_a_2D_alt_lya(U[0:2], -alt_d) q2.set_a_2D_alt_lya(U[2:4], -alt_d) q3.set_a_2D_alt_lya(U[4:6], -alt_d) q1.step(dt) q2.step(dt) q3.step(dt) # Animation if it%frames == 0: pl.figure(0) axis3d.cla() ani.draw3d(axis3d, q1.xyz, q1.Rot_bn(), quadcolor[0]) ani.draw3d(axis3d, q2.xyz, q2.Rot_bn(), quadcolor[1]) ani.draw3d(axis3d, q3.xyz, q3.Rot_bn(), quadcolor[2]) axis3d.set_xlim(-5, 5) axis3d.set_ylim(-5, 5) axis3d.set_zlim(0, 10) axis3d.set_xlabel('South [m]') axis3d.set_ylabel('East [m]') axis3d.set_zlabel('Up [m]') axis3d.set_title("Time %.3f s" %t) pl.pause(0.001) pl.draw() #namepic = '%i'%it #digits = len(str(it)) #for j in range(0, 5-digits): # namepic = '0' + namepic
v_2D_d = np.array([2, -0.5]) alt_d = -8 q1.yaw_d = np.pi / 3 for t in time: # Simulation q1.set_v_2D_alt_lya(v_2D_d, alt_d) q1.step(dt) # Animation if it % frames == 0: pl.figure(0) axis3d.cla() ani.draw3d(axis3d, q1.xyz, q1.Rot_bn(), quadcolor[0]) axis3d.set_xlim(-10, 10) axis3d.set_ylim(-10, 10) axis3d.set_zlim(0, 15) axis3d.set_xlabel('South [m]') axis3d.set_ylabel('East [m]') axis3d.set_zlabel('Up [m]') axis3d.set_title("Time %.3f s" % t) pl.pause(0.001) pl.draw() # Log q1_log.xyz_h[it, :] = q1.xyz q1_log.att_h[it, :] = q1.att q1_log.w_h[it, :] = q1.w q1_log.v_ned_h[it, :] = q1.v_ned
####################################################TEST SEQUENCE########################################################### #for d in quad_list: #d.group.syncronize(d,quad_list,radius) #rules.flocking(d) #if t > 120: # for d in quad_list: # d.group.syncronize(d,quad_list,radius) # rules.spread(d) for d in quad_list: d.step(dt) if it % frames == 0: axis3d.cla() ani.draw3d(axis3d, q1.xyz, q1.Rot_bn(), quadcolor[0]) ani.draw3d(axis3d, q2.xyz, q2.Rot_bn(), quadcolor[0]) ani.draw3d(axis3d, q3.xyz, q3.Rot_bn(), quadcolor[0]) ani.draw3d(axis3d, q4.xyz, q4.Rot_bn(), quadcolor[0]) ani.draw3d(axis3d, q5.xyz, q5.Rot_bn(), quadcolor[0]) ani.draw3d(axis3d, q6.xyz, q6.Rot_bn(), quadcolor[0]) axis3d.set_xlim(-15, 15) axis3d.set_ylim(-15, 15) axis3d.set_zlim(0, 15) axis3d.set_xlabel('South [m]') axis3d.set_ylabel('East [m]') axis3d.set_zlabel('Up [m]') axis3d.set_title("Time %.3f s" % t)
v_2D_d = np.array([2, -0.5]) alt_d = -8 q1.yaw_d = np.pi/3 for t in time: # Simulation q1.set_v_2D_alt_lya(v_2D_d, alt_d) q1.step(dt) # Animation if it%frames == 0: pl.figure(0) axis3d.cla() ani.draw3d(axis3d, q1.xyz, q1.Rot_bn(), quadcolor[0]) axis3d.set_xlim(-10, 10) axis3d.set_ylim(-10, 10) axis3d.set_zlim(0, 15) axis3d.set_xlabel('South [m]') axis3d.set_ylabel('East [m]') axis3d.set_zlabel('Up [m]') axis3d.set_title("Time %.3f s" %t) pl.pause(0.001) pl.draw() # Log q1_log.xyz_h[it, :] = q1.xyz q1_log.att_h[it, :] = q1.att q1_log.w_h[it, :] = q1.w
def run_simulation(self): radius = 2 angle = 360 / self.number_of_drones angle_list = np.ones(self.number_of_drones)*angle des_dist_list = np.zeros(self.number_of_drones) for idx in range(0,len(angle_list)): # This only works for equal angles for the whole formation. des_dist_list[idx] = self.angle_to_chord(radius, angle_list[idx]) act_dist_list = np.zeros(self.number_of_drones) err_dist_list = np.zeros(self.number_of_drones) unit_vector_list = np.zeros((self.number_of_drones, 3)) inter_angle_list = np.zeros(self.number_of_drones) ke = 0.00285*2 *0.7 # Gain for going towards circle kt = 0.01*7 # Gain for orbiting kf = 0.08 *0.5 # Gain for formation drone_added = True #drone_added = True do_animation = False for self.t in self.time: # If we want to add a new drone if drone_added == False and int(self.t % 100) == 0 and self.t > 1.0: self.add_new_drone() drone_added = True self.number_of_drones = len(self.qc_list) # Recalculate number of drones for all for-loops in the code. if self.number_of_drones > len(act_dist_list): print("[" + str(self.t) + "] - Adding new drone and reinitializing vectors") act_dist_list = np.zeros((self.number_of_drones)) err_dist_list = np.zeros(self.number_of_drones) unit_vector_list = np.zeros((self.number_of_drones, 3)) inter_angle_list = np.zeros(self.number_of_drones) angle = 360 / self.number_of_drones angle_list = np.ones(self.number_of_drones)*angle des_dist_list = np.zeros(self.number_of_drones) for idx in range(0,len(angle_list)): # This only works for equal angles for the whole formation. des_dist_list[idx] = self.angle_to_chord(radius, angle_list[idx]) self.q_log_list = np.append(self.q_log_list, quadlog.quadlog(self.time)) drone_id_list = [] for idx in range(0, self.number_of_drones): drone_id_list.append(self.qc_list[idx].identification) for i in drone_id_list: copter = self.qc_list[i] copter.broadcast_rel_xyz(self.qt) for idx in drone_id_list: act_dist_list[idx] = self.to_euclid(self.qc_list[idx].calc_neighbour_vector(self.qc_list[(idx + 1) % self.number_of_drones])) for idx in drone_id_list: err_dist_list[idx] = act_dist_list[idx] - des_dist_list[idx] #print(unit_vector_list.shape) #print(len(qc_list)) #print(act_dist_list) for idx in drone_id_list: for i in range (0, 3): unit_vector_list[idx][i] = self.qc_list[idx].calc_neighbour_vector(self.qc_list[(idx + 1) % self.number_of_drones])[i] / act_dist_list[idx] # Formation adjustment by rearranging drones in the list depending on the angles between them for idx in drone_id_list: i = idx - 1 # A delayed counter that wraps in the beginning. o = idx + 1 # A hastened counter that wraps in the end. if idx == 0: i = self.number_of_drones - 1 if idx == self.number_of_drones - 1: o = 0 inter_angle_list[idx] = self.angle_between_unit_vectors(self.qc_list[i].xyz - self.qc_list[idx].xyz, self.qc_list[o].xyz - self.qc_list[idx].xyz) for idx in range (0, self.number_of_drones): if inter_angle_list[idx] < 30: if self.qc_list[idx].cooldown < 0 and self.qc_list[i].cooldown < 0 and self.qc_list[o].cooldown < 0: i = idx - 1 # A delayed counter that wraps in the beginning. o = idx + 1 # A hastened counter that wraps in the end. if idx == 0: i = self.number_of_drones - 1 if idx == self.number_of_drones - 1: o = 0 print("[" + str(self.t) + "] - Swapping drone q" + str(o+1) + " and q" + str(idx+1)) # Swap the drones get = self.qc_list[o], self.qc_list[idx] self.qc_list[idx], self.qc_list[o] = get # Reset the cooldown on these drones self.qc_list[idx].cooldown = self.qc_list[o].cooldown = self.qc_list[i].cooldown = 150*20 self.number_of_swaps += 1 # Swap the colors as well, so it won't look weird in the graphs #get_color = quadcolor[o+1], quadcolor[idx+1] #quadcolor[idx+1], quadcolor[o+1] = get_color #print("Desired: ", des_dist_list) #print("Actual: ", act_dist_list) #print("Errors: ", err_dist_list) #print("unit_vectors: " , unit_vector_list) # Simulation X = V = [] for idx in drone_id_list: X = np.append(X, self.qc_list[idx].xyz[0:2]) V = np.append(V, self.qc_list[idx].v_ned[0:2]) grad_list = np.empty((0,2)) for i in drone_id_list: grad_list = np.append(grad_list, np.array([[(X[i*2] - self.qt.xyz[0])*2, (X[i*2 + 1] - self.qt.xyz[1])*2]]), axis=0) tangent_list = np.empty((0,2)) for idx in drone_id_list: tangent_list = np.append(tangent_list, np.array([self.tangentHelp.dot((grad_list[idx]/la.norm(grad_list[idx])))]), axis=0) e_list = np.array([]) for i in drone_id_list: e_list = np.append(e_list, self.impPath(X[i*2], X[i*2 + 1], radius)) # This small u is our circle following control input (velocity) # The form is: # ControlInput = FollowCircleError + RotateOnCircle + StayInFormationDistance formation_list = np.empty((0,2)) for idx in drone_id_list: i = idx - 1 if idx == 0: i = self.number_of_drones - 1 # A delayed counter that wraps in the beginning. formation_list = np.append(formation_list, np.array([[(err_dist_list[idx]*unit_vector_list[idx][0] + err_dist_list[i]*unit_vector_list[idx][0])*kf, (err_dist_list[idx]*unit_vector_list[idx][1] + err_dist_list[idx]*unit_vector_list[i][1])*kf]]), axis=0) #print(err_dist_list) u_list = np.empty((0,2)) for idx in drone_id_list: u_list = np.append(u_list, np.array([ke*(-e_list[idx])*grad_list[idx] + kt*tangent_list[idx] + formation_list[idx]]), axis=0) for idx in drone_id_list: u_list[idx] = self.sigmoid(u_list[idx]) #zero = np.array([0, 0]) ut = np.array([0.25, 0.25]) # move the middle drone qt a little bit in x and y #ut = np.array([0, 0]) #ut = impPath(qt.xyz[0], qt.xyz[1], 2) self.qt.set_v_2D_alt_lya(ut, -self.alt_d) for idx in drone_id_list: self.qc_list[idx].set_v_2D_alt_lya(u_list[idx] + ut, -self.alt_d) self.qt.step(self.dt) for idx in drone_id_list: self.qc_list[idx].step(self.dt) # Animation if self.it % self.frames == 0 and do_animation == True: pl.figure(0) self.axis3d.cla() ani.draw3d(self.axis3d, self.qt.xyz, self.qt.Rot_bn(), self.quadcolor[0]) for idx in drone_id_list: ani.draw3d(self.axis3d, self.qc_list[idx].xyz, self.qc_list[idx].Rot_bn(), self.quadcolor[idx+1]) #ani.draw3d(axis3d, q1.xyz, q1.Rot_bn(), quadcolor[1]) #ani.draw3d(axis3d, q2.xyz, q2.Rot_bn(), quadcolor[2]) #ani.draw3d(axis3d, q3.xyz, q3.Rot_bn(), quadcolor[3]) self.axis3d.set_xlim(-10, 10) self.axis3d.set_ylim(-10, 10) self.axis3d.set_zlim(0, 10) self.axis3d.set_xlabel('South [m]') self.axis3d.set_ylabel('East [m]') self.axis3d.set_zlabel('Up [m]') self.axis3d.set_title("Time %.3f s" % self.t) pl.pause(0.001) pl.draw() # namepic = '%i'%it # digits = len(str(it)) # for j in range(0, 5-digits): # namepic = '0' + namepic # pl.savefig("./images/%s.png"%namepic) #cid = fig.canvas.mpl_connect('key_press_event', on_key) # Log p = 0 for idx in drone_id_list: self.q_log_list[p].xyz_h[self.it, :] = self.qc_list[idx].xyz self.q_log_list[p].att_h[self.it, :] = self.qc_list[idx].att self.q_log_list[p].w_h[self.it, :] = self.qc_list[idx].w self.q_log_list[p].v_ned_h[self.it, :] = self.qc_list[idx].v_ned self.q_log_list[p].formation_error[self.it] = err_dist_list[idx] self.q_log_list[p].circle_error[self.it] = e_list[idx] p += 1 self.qt_log.xyz_h[self.it, :] = self.qt.xyz self.qt_log.att_h[self.it, :] = self.qt.att self.qt_log.w_h[self.it, :] = self.qt.w self.qt_log.v_ned_h[self.it, :] = self.qt.v_ned self.it += 1 # Stop if crash for q in self.qc_list: if q.crashed==1: break return self.q_log_list, self.qt_log, drone_id_list # and also return qt_log...
def run(self, method, run_animation=False): if not (method == 'NF' or method == 'KF' or method == 'PF' or method == 'PKF' or \ method == 'NF4' or method == 'KF4' or method == 'PF4' or method == 'PKF4' or method == 'PF2' or method == 'PKF2'): print ("Wrong Input, your in put was: ", method) return -1 use = 4 use4 = False if len(method) > 2: if method == 'PKF4' or method == 'PKF' or method == 'PKF2': if method == 'PKF4': method = method[0:3] use4 = True elif method == 'PKF2': method = method[0:3] use4 = True use = 2 else: use4 = True method = method[0:2] dt = self.dt it = self.it kalmanStarted = False PFstarted = False PKFstarted = False quadcolor = ['r', 'g', 'b'] pl.close("all") pl.ion() fig = pl.figure(0) axis3d = fig.add_subplot(111, projection='3d') frames = self.frames for t in self.time: acc_err = np.random.normal(0, 0.012, 1)[0] #HANDLE RANGE MEASUREMENTS: if it % 50 == 0 or it == 0: # or method == 'NF': #print(t) self.UAV_agent.handle_range_msg(self.RA0.id, self.get_dist(self.UAV.xyz, self.uwb0.xyz)) self.UAV_agent.handle_range_msg(self.RA1.id, self.get_dist(self.UAV.xyz, self.uwb1.xyz)) self.UAV_agent.handle_range_msg(self.RA2.id, self.get_dist(self.UAV.xyz, self.uwb2.xyz)) self.UAV_agent.handle_range_msg(self.RA3.id, self.get_dist(self.UAV.xyz, self.uwb3.xyz)) self.UAV_agent.handle_range_msg(self.RA4.id, self.get_dist(self.UAV.xyz, self.uwb4.xyz)) self.UAV_agent.handle_range_msg(self.RA5.id, self.get_dist(self.UAV.xyz, self.uwb5.xyz)) self.UAV_agent.handle_range_msg(self.RA6.id, self.get_dist(self.UAV.xyz, self.uwb6.xyz)) if PFstarted and method == 'PF': self.UAV_agent.PFupdate(use4=use4, use=use) if PKFstarted and method == 'PKF': self.UAV_agent.updatePKF(use4=use4, use=use) if kalmanStarted and method == 'KF': alg_pos = self.UAV_agent.calc_pos_alg(use4=use4) #HANDLE POS NO FILTER: if method == 'NF': alg_pos = self.UAV_agent.calc_pos_alg(use4=use4) #HANDLE KALMAN FILTER: if method == 'KF': if self.UAV.xyz[2] < -3 and not kalmanStarted: self.R, self.Q = self.UAV_agent.startKF(self.UAV.xyz, v_ned=self.UAV.v_ned, dt=dt) kalmanStarted = True if kalmanStarted: self.UAV_agent.KFpredict( self.UAV.acc + acc_err ) alg_pos = self.UAV_agent.get_kf_state() else: alg_pos = self.UAV_agent.calc_pos_alg(use4=use4) #HANDLE PARTICLE FILTER if method == 'PF': if self.UAV.xyz[2] < -3 and not PFstarted: self.n_of_particles, self.std_add = self.UAV_agent.startPF(start_vel=self.UAV.v_ned, dt=dt) PFstarted = True if PFstarted: alg_pos = self.UAV_agent.getPFpos() #print (PF_pos) self.UAV_agent.PFpredict(self.UAV.acc + acc_err) else: alg_pos = self.UAV.xyz #HANDLE PARTICLE KALMAN FILTER if method == 'PKF': if self.UAV.xyz[2] < -3 and not PKFstarted: self.R, self.Q, self.n_of_particles, self.std_add = self.UAV_agent.startPKF(self.UAV.acc + acc_err, dt=dt, xyz=self.UAV.xyz, v_ned=self.UAV.v_ned) PKFstarted = True if PKFstarted: alg_pos = self.UAV_agent.get_PKFstate() self.UAV_agent.predictPKF(self.UAV.acc + acc_err) else: alg_pos = self.UAV.xyz #print("Estimated pos:",alg_pos) #print("True pos: ",self.UAV.xyz) #HANDLE UAV MOVEMENT: x_err = abs(self.wp[self.state][0] - self.UAV.xyz[0]) y_err = abs(self.wp[self.state][1] - self.UAV.xyz[1]) if self.state == 0: self.UAV.set_v_2D_alt_lya(np.array([x_err*0.03, y_err*0.03]), -3) if self.get_dist_clean(self.UAV.xyz, self.wp[self.state]) < 0.4: self.state = 2 elif self.state == 1: self.UAV.set_v_2D_alt_lya(np.array([x_err*0.03, -y_err*0.03]),-3) if self.get_dist_clean(self.UAV.xyz, self.wp[self.state]) < 0.4: self.state = 0 elif self.state == 2: self.UAV.set_v_2D_alt_lya(np.array([-x_err*0.03, -y_err*0.03]), -3) if self.get_dist_clean(self.UAV.xyz, self.wp[self.state]) < 0.4: self.state = 3 elif self.state == 3: self.UAV.set_v_2D_alt_lya(np.array([-x_err*0.03, y_err*0.03]), -3) if self.get_dist_clean(self.UAV.xyz, self.wp[self.state]) < 0.4: self.state = 1 self.UAV.step(dt) #LOGS: #self.est_pos[it] = alg_pos #self.gt_pos[it] = self.UAV.xyz if kalmanStarted or PFstarted or PKFstarted or method == 'NF': self.Ed_log[it, :] = np.array([ self.get_dist_clean(alg_pos, self.UAV.xyz) ]) self.Ed2d_log[it, :] = np.array([ self.get_dist_clean(alg_pos[0:2], self.UAV.xyz[0:2]) ]) self.Edalt_log[it, :] = np.array([ self.get_dist_clean(alg_pos[2], self.UAV.xyz[2]) ]) self.Ed_vel_log[it, :] = np.array([ self.get_dist_clean(alg_pos, self.UAV.v_ned) ]) self.alg_log.xyz_h[it, :] = alg_pos self.UAV_log.xyz_h[it, :] = self.UAV.xyz self.UAV_log.att_h[it, :] = self.UAV.att self.UAV_log.w_h[it, :] = self.UAV.w self.UAV_log.v_ned_h[it, :] = self.UAV.v_ned it+=1 # Stop if crash if (self.UAV.crashed == 1): break if run_animation: if it%frames == 0: pl.figure(0) axis3d.cla() ani.draw3d(axis3d, self.uwb0.xyz, self.uwb0.Rot_bn(), quadcolor[0]) ani.draw3d(axis3d, self.uwb1.xyz, self.uwb1.Rot_bn(), quadcolor[2]) ani.draw3d(axis3d, self.uwb2.xyz, self.uwb2.Rot_bn(), quadcolor[2]) ani.draw3d(axis3d, self.uwb3.xyz, self.uwb3.Rot_bn(), quadcolor[2]) ani.draw3d(axis3d, self.uwb4.xyz, self.uwb4.Rot_bn(), quadcolor[2]) ani.draw3d(axis3d, self.uwb5.xyz, self.uwb5.Rot_bn(), quadcolor[2]) ani.draw3d(axis3d, self.uwb6.xyz, self.uwb6.Rot_bn(), quadcolor[2]) ani.draw3d(axis3d, self.UAV.xyz, self.UAV.Rot_bn(), quadcolor[1]) axis3d.set_xlim(-6, 6) axis3d.set_ylim(-6, 6) axis3d.set_zlim(0, 10) axis3d.set_xlabel('South [m]') axis3d.set_ylabel('East [m]') axis3d.set_zlabel('Up [m]') axis3d.set_title("Time %.3f s" %t) pl.pause(0.001) pl.draw() alg_log = np.array([self.alg_log.xyz_h[:,0], self.alg_log.xyz_h[:,1], self.alg_log.xyz_h[:,2]]) uav_log = np.array([self.UAV_log.xyz_h[:,0], self.UAV_log.xyz_h[:,1], self.UAV_log.xyz_h[:,2]]) #print(self.Ed_log) return [uav_log, alg_log, self.Ed_log, self.Ed2d_log, self.Edalt_log]
U = fc.u_acc(X, V) q1.set_a_2D_alt_lya(U[0:2], -alt_d) q2.set_a_2D_alt_lya(U[2:4], -alt_d) q3.set_a_2D_alt_lya(U[4:6], -alt_d) q1.step(dt) q2.step(dt) q3.step(dt) # Animation if it%frames == 0: pl.figure(0) axis3d.cla() ani.draw3d(axis3d, q1.xyz, q1.Rot_bn(), quadcolor[0]) ani.draw3d(axis3d, q2.xyz, q2.Rot_bn(), quadcolor[1]) ani.draw3d(axis3d, q3.xyz, q3.Rot_bn(), quadcolor[2]) axis3d.set_xlim(-5, 5) axis3d.set_ylim(-5, 5) axis3d.set_zlim(0, 10) axis3d.set_xlabel('South [m]') axis3d.set_ylabel('East [m]') axis3d.set_zlabel('Up [m]') axis3d.set_title("Time %.3f s" %t) pl.pause(0.001) pl.draw() #namepic = '%i'%it #digits = len(str(it)) #for j in range(0, 5-digits): # namepic = '0' + namepic