def __init__(self): self.c = carController.carController() self.box = boundingBox.boundingBox() self.lastbox1 = boundingBox.boundingBox() self.lastbox2 = boundingBox.boundingBox() self.lastbox3 = boundingBox.boundingBox() self.lastbox4 = boundingBox.boundingBox() self.lastbox5 = boundingBox.boundingBox() self.empty = boundingBox.boundingBox() self.empty.setPrediction(1) self.car = boundingBox.carCharacteristics() self.flag = 0 self.pred = "" #min_area=10000.0 #max_area=42000.0 self.check = 0 self.time = rospy.get_time() self.rtime = 0.0 self.input = "" self.count = 0 self.sub = rospy.Subscriber("/detectnet/detections", String, self.callback) self.manualControl = 0 self.sub2 = rospy.Subscriber("joy", Joy, self.callback2) self.sub3 = rospy.Subscriber("depthwitherror", Float32MultiArray, self.callback3) self.kf = kalman_filter.kalman_filter(1023, 0.125, 32, 0)
def fake_path_trick(alpha_kalman, sigma_e, sigma_o, sigma_u, sigma_r, sigma_v, sigma_w, p_a, p_c, L, L_train, S, t_c_fixed, c_fixed, t_r_fixed, r_fixed): L_train = len(alpha_kalman) mu_0_fp = 50 * np.random.rand() delta_0_fp = 0. gamma_0_fp = 0.5 * np.random.rand(S - 1) - 0.25 (y_fp, mu_fp, delta_fp, gamma_fp, mu_with_initial_values_fp, delta_with_initial_values_fp, gamma_with_initial_values_fp, gamma_vec_fp, z_a_fp, z_c_fp) = generative_procedure_with_evaluation_period( mu_0_fp, delta_0_fp, gamma_0_fp, sigma_e, sigma_o, sigma_u, sigma_r, sigma_v, sigma_w, p_a, p_c, L_train, L_train, S, t_c_fixed, c_fixed, t_r_fixed, r_fixed) d = 1000. a_1 = kalman_filter.gen_a_1(S, y_fp) P_1 = kalman_filter.gen_P_1(S, d) Z_t = kalman_filter.gen_Z_t(S) T_t = kalman_filter.gen_T_t(S) R_t = kalman_filter.gen_R_t(S) H_e = sigma_e**2 H_o = sigma_o**2 Q_eta = kalman_filter.gen_Q_eta(sigma_r, sigma_v, sigma_w) Q_xi = kalman_filter.gen_Q_eta(sigma_u, sigma_v, sigma_w) a_t, P_t, K_t, F_t, v_t = kalman_filter.kalman_filter( L_train, y_fp, a_1, P_1, Z_t, p_a, p_c, T_t, H_e, H_o, Q_eta, Q_xi, R_t) alpha_hat, V = kalman_smoothing(L_train, v_t, a_t, F_t, K_t, P_t, T_t, Z_t) alpha_fp = [] for i in range(len(y_fp)): alpha_fp_t = torch.zeros((S + 1, 1), dtype=torch.float64) alpha_fp_t[0] = mu_fp[i] alpha_fp_t[1] = delta_fp[i] alpha_fp_t[2:, 0] = gamma_vec_fp[i] alpha_fp.append(alpha_fp_t) return [ a_fp - a_hat + a_kalman for a_fp, a_hat, a_kalman in zip(alpha_fp, alpha_hat, alpha_kalman) ]
def run_tracking_demo(filename, plot=True): ss = 4 # state size os = 2 # observation size A = np.matrix('1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1') C = np.matrix('1 0 0 0; 0 1 0 0') Q = 0.1 * np.identity(ss) R = 1 * np.identity(os) init_x = np.matrix([10, 10, 1, 0]).T init_V = 10 * np.identity(ss) T = 15 # Generate new data np.random.seed(0) x, y = sample_lds(A, C, Q, R, init_x, T) # # Use mat file to reproduce Murphy's example # mat = io.loadmat(filename) # x = mat['x'] # y = mat['y'] # A = mat['F'][:,:,0] # C = mat['H'][:,:,0] # Q = mat['Q'][:,:,0] # R = mat['R'] # init_x = mat['initx'] # init_V = mat['initV'] xfilt, Vfilt, VVfilt, loglik = kalman_filter(y, A, C, Q, R, init_x, init_V) xsmooth, Vsmooth, VVsmooth, loglik = kalman_smoother(y, A, C, Q, R, init_x, init_V) dfilt = x[[0,1],:] - xfilt[[0,1],:] mse_filt = np.sqrt(np.sum(dfilt**2)) print "mse_filt: %f" % mse_filt dsmooth = x[[0,1],:] - xsmooth[[0,1],:] mse_dsmooth = np.sqrt(np.sum(dsmooth**2)) print "mse_dsmooth: %f" % mse_dsmooth # Plot if plot: f, (ax1, ax2) = plt.subplots(1, 2, sharex=True,sharey=True) ax1.plot(x[0,:], x[1,:], 'ks-') ax1.plot(xfilt[0,:], xfilt[1,:], 'rx:') ax1.set_title('Kalman Filter') plot_2d_contours(ax1, xfilt, Vfilt) ax2.plot(x[0,:], x[1,:], 'ks-') ax2.plot(xsmooth[0,:], xsmooth[1,:], 'rx:') ax2.set_title('Kalman Smoother') plot_2d_contours(ax2, xsmooth, Vsmooth) plt.show()
def usbl_move(pos): broadcaster = tf.TransformBroadcaster() if usbl_move.start: dt = 2 loc = np.matrix([[0],#v_x [0],#v_y [pos.pose.position.x],#x [pos.pose.position.y]])#z A = np.matrix([[1, 0, 0, 0,], [0, 1, 0, 0,], [dt, 0, 1, 0,], [0, dt, 0, 1,]]) B = np.matrix([0]) C = np.eye(loc.shape[0]) Q = np.eye(loc.shape[0])*0.5 R = np.eye(loc.shape[0])*5000 P = np.eye(loc.shape[0]) U = np.matrix( [[0]]) Z = np.matrix( [[0],[0],[0],[0]]) usbl_move.kalman = kalman_filter.kalman_filter(A,B,C,Q,P,R,loc) usbl_move.md_filter = md_filter.md_filter(2, [1.75, 1.6], 10, [0, 1]) usbl_move.start = 0 if usbl_move.md_filter.update( [pos.pose.position.x,pos.pose.position.y] ): #update.ax.scatter(pos.position.x,pos.position.y,-1*current.position.z,color='b') current.position.x = pos.pose.position.x current.position.y = pos.pose.position.y #current.position.z = pos.pose.position.z #update('b') Z = np.matrix( [[0],[0],[pos.pose.position.x],[pos.pose.position.y] ]) U = np.matrix( [[0]]) usbl_move.kalman.move(U,Z) kalman_pos = usbl_move.kalman.getState() current.position.y = kalman_pos[2] current.position.x = kalman_pos[3] broadcaster.sendTransform( (current.position.x,current.position.y,current.position.z), (current.orientation.x,current.orientation.y,current.orientation.z,current.orientation.w), rospy.Time.now(), "body", "odom" )
def main(): """ For the Kalman filter, find and plot the true and estimated signal and the true and predicted error """ y = generate_obs_seq() x_true = np.zeros(n0) x_pred = np.zeros(n0) P_true = np.zeros(n0) P_pred = np.zeros(n0) x_true[0] = y[0] x_pred[0] = y[0] P_true[0] = 1 P_pred[0] = 1 for i in range(1, n0): x_true[i], x_pred[i], P_true[i], P_pred[i] = kalman_filter( y[i], sigma_v, sigma_w, x_true[i - 1], P_true[i - 1]) # Plot and save the required plots plt.figure() plt.plot(x_true, label="True x[n]") plt.plot(x_pred, label="Estimated x[n]") plt.xlabel("n") plt.ylabel("x[n]") plt.title("True and Estimated signal") plt.legend() plt.savefig('./results/Kalman_1.png') plt.figure() plt.plot(P_true, label="True Error") plt.plot(P_pred, label="Estimated Error") plt.xlabel("n") plt.ylabel("Error") plt.title("True and Predicted error") plt.legend() plt.savefig('./results/Kalman_2.png')
0, 0, dt, 0, 0, 1, ]]) B = np.matrix([0]) C = np.eye(pos.shape[0]) Q = np.eye(pos.shape[0]) * .05 R = np.eye(pos.shape[0]) * 50000 P = np.eye(pos.shape[0]) U = np.matrix([[0]]) Z = np.matrix([[0], [0], [0], [0], [0], [0]]) kalman = kalman_filter.kalman_filter(A, B, C, Q, P, R, pos) md_filter = md_filter.md_filter(3, [1.6, 1.5], 70, [-30, 30]) out_x = [] out_y = [] out_z = [] new_x = [] new_y = [] new_z = [] last = pos outliers = 0 for i in xrange(len(t)): if (md_filter.update([x[i], y[i], z[i]])):
def load_frame(obj_name, trial_times, GtDict, roi_mask, start_id, end_id, ParamOptions): """ Get the formatted sub-image_list, append in polar or cart subimage list. data_name given the source binary data file name. frame_id, is indexed for several sequences in sequential time. GT_dict give all the ground truth information for a specific target roi_mask is the region of interesting for a tracking environment """ print ('Tracking %s:...' % obj_name) resPath = '../results/Res_KF/' #*_tbbs.txt for averaged boundingbox only #*_blob.txt for matched blob tracker_res_name = resPath + obj_name + '_blobs_%02d_option.txt' % trial_times tracker_tbb_name = resPath + obj_name + '_KF_Tbbs.txt' track_res_file = open(tracker_res_name, 'w') tracker_tbbs_file = open(tracker_tbb_name, 'w') # ParamOptions= {'BlobSeg': {'MorphIter':2, 'DilateIter':3, 'DistThreash':0.6, 'SubWindow':(128,64)}, # 'VotePsr':5, # 'VoteDistThreash':100, # 'PrioSigmaFactor':3./4, # 'BlobScore':0.2 # } segBlobParam = ParamOptions['BlobSeg'] # VotePsrThreash= ParamOptions['VotePsr'] # VoteDistThreash = ParamOptions['VoteDistThreash'] # BlobScoreThreash= ParamOptions['BlobScore'] # PrioSigmaFactor = ParamOptions['PrioSigmaFactor'] # NumberOfTrackers = ParamOptions['NumberOfTrackers'] data_name = [] frame = np.array([]) #polar frame in float32 dframe = np.array([]) #polar frame in uint8 uframe = np.array([]) #cartesian frame in float32 dispmat = np.array([]) #udispmat frame in uint8 udispmat = np.array([]) #pre and cur frame for flow computing preframe = np.array([]) curframe = np.array([]) #tracker_dict = {} tracker_list = [] track_rect_list = [] #ignore the damaged frames damaged_frames = [1, 116, 232, 348] frame_id = 0 # starting frame_id #start_id = 60 frame_counter = 0 total_time_consum = 0 frame_mode = 'polar' # reading the sequential 4 files in a loop for i in np.arange(4): file_prefix = '/Users/yizhou/Radar_Datasets/Zhicheng/zhicheng_20130808_22.' file_suffix = '_5minutes_600x2048.double.data' data_name = '%s%d%s' % (file_prefix, 28 + i * 5, file_suffix) print (data_name) fdata_obj = open(data_name, 'rb') # loading file data and to convert from polar to cartesian while True: if frame_id > end_id: #using empty frame to break the while loop frame = np.array([]) break else: # read a frame from local file frame = np.fromfile(fdata_obj, 'float64', 600 * 2048) if frame.size != 0: frame_id += 1 if frame_id >= start_id and frame_id not in damaged_frames: key = 'frame %d' % frame_id objGtElem = {} if key not in GtDict: print ('%s is not in GT files!!!' % key) else: objGtElem = GtDict[key] frame = frame.reshape([2048, 600]) frame = frame.T # 通过计算均值mean # fmean = frame.mean(axis=1) # mfm = np.tile(fmean,(600,1)).transpose() frame = frame - 4000 # frame.mean() # 归一化操作 # dframe is for computing in double float dframe = uti.frame_normalize(frame) # #uframe is for displaying,optical flow computing and finding blob uframe = (dframe * 255).astype(np.uint8) canvas_polar = cv2.cvtColor(uframe, cv2.COLOR_GRAY2BGR) cv2.putText(canvas_polar, obj_name+' frame ' + str(frame_id), org=(10, 50), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1, color=(0, 255, 255), thickness=2, lineType=cv2.LINE_AA) #cv2.imshow('polar', canvas_polar) # for cartesian-coordinates dispmat = corcon.polar2disp_njit(dframe, np.array([])) dispmat = uti.frame_normalize(dispmat) udispmat = (dispmat * 255).astype(np.uint8) canvas_disp = cv2.cvtColor(udispmat, cv2.COLOR_GRAY2BGR) cv2.putText(canvas_disp, 'frame ' + str(frame_id), org=(10, 50), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1, color=(0, 255, 255), thickness=2, lineType=cv2.LINE_AA) #cv2.imshow('disp', canvas_disp) # cv2.moveWindow('polar',0,0) #cv2.waitKey(1) if frame_mode == 'polar': tframe = dframe * roi_mask #echoThreash plays a role of CFAR. echoThreash = np.mean(tframe) tuframe = uframe canvas_frame = canvas_polar else: tframe = dispmat tuframe = udispmat canvas_frame = canvas_disp if frame_id == start_id : # init previous frame preframe = tuframe obj_bbox = objGtElem['BoundingBox'] # frame60 Alice # Uncomment the line below to select a different bounding box # bbox = cv2.selectROI(frame, False) # Initialize tracker with first frame and bounding box #tracker = KCF_ST.KCFTracker_status() cx = obj_bbox[0] + obj_bbox[2]/2 cy = obj_bbox[1] + obj_bbox[3]/2 tracker = KF.kalman_filter() ok = tracker.init(cx,cy) X_ = tracker.X0 P_ = tracker.P0 blob_list, seg_roi, sub_blob_list = uti.blob_seg(tframe, obj_bbox) obj_blob, blob_score, blob_id = uti.vote_blob(blob_list, [obj_bbox]) if obj_blob=={}: print ('No blob is found in first frame!!!') else: blob_key = 'frame ' + str(frame_id) linedict = {} linedict[blob_key] = obj_blob track_res_file.writelines(str(linedict) + '\n') track_res_file.flush() bb_linedict = {} tbb_key = 'frame ' + str(frame_id) bb_linedict[tbb_key] = {'BoundingBox':obj_bbox} tracker_tbbs_file.writelines(str(bb_linedict) + '\n') track_rect_list.append(obj_bbox) uti.draw_rect(canvas_frame, obj_bbox, (0, 0, 255), 2) if frame_id > start_id: # only counting the tracking time #timer = uti.msTimer() #ms_start = timer.start() #tstart = time.clock() tstart = time.clock() #seg window is initialized by last frame's boundingbox blob_list, seg_roi, sub_blob_list = uti.blob_seg(tframe, obj_bbox, segBlobParam['MorphIter'], segBlobParam['DilateIter'], segBlobParam['DistThreash'], segBlobParam['SubWindow']) cx = obj_bbox[0] + obj_bbox[2]/2 cy = obj_bbox[1] + obj_bbox[3]/2 dist_min = 200 dist = 0 for blob in blob_list: (bcx,bcy) = blob['Centroid'] dist = np.sqrt((cx-bcx)**2+(cy-bcy)**2) if dist < dist_min: dist_min = dist z_x = bcx z_y = bcy if dist_min < 200: (X_, P_, X_pr) = tracker.update(X_, P_, z_x, z_y) obj_bbox = (int(X_[0][0] - obj_bbox[2]/2), int(X_[2][0]-obj_bbox[3]/2), obj_bbox[2], obj_bbox[3]) #print ('speed x direction %f, y direction %f\n' % (X_[1][0], X_[3][0])) cv2.circle(canvas_frame, (int(X_pr[0][0]), int(X_pr[2][0])),2,(0,255,255),2) cv2.circle(canvas_frame, (int(X_[0][0]), int(X_[2][0]) ),2,(0, 0, 255),2) else: #no blob selected, using prior_estimated information #computing offset based on the last 4 frame's obj_bbox'center. #using the average center shift as the (offset_x, offset_y) if len(track_rect_list)<4: prio_offset = (0,0) else: dif_rect = [] for iter in [-1,-2,-3]: dif_rect.append(np.array(track_rect_list[iter]) - np.array(track_rect_list[iter-1])) offset_rect = np.mean(dif_rect,0) prio_offset = (offset_rect[0] + offset_rect[2]/2, offset_rect[1] + offset_rect[3]/2) print( 'offset is (%d, %d)' %(prio_offset[0], prio_offset[1])) prio_obj_bbox = (int(obj_bbox[0] + prio_offset[0]), int(obj_bbox[1] + prio_offset[1]), obj_bbox[2], obj_bbox[3]) obj_bbox = prio_obj_bbox track_rect_list.append(obj_bbox) total_time_consum = total_time_consum + time.clock() - tstart frame_counter += 1. if(DETAIL_MODE==True): print ('frame %d cost %.4f seconds' % (frame_id, (time.clock() - tstart))) print ('frame %d cost %.4f seconds' % (frame_id, mscost)) uti.draw_rect(canvas_frame, obj_bbox, (0, 0, 255), 2) bb_linedict = {} tbb_key = 'frame ' + str(frame_id) bb_linedict[tbb_key] = {'BoundingBox':obj_bbox} tracker_tbbs_file.writelines(str(bb_linedict) + '\n') # copy seg_roi to the canvas_frame roih, roiw = seg_roi.shape[0:2] canvas_frame[420:420 + roih, 770:770 + roiw, :] = seg_roi uti.draw_rect(canvas_frame, (770, 420, roiw, roih), (255, 255, 255)) if frame_mode == 'polar': cv2.imshow('polar', canvas_frame) if frame_counter == 1: cv2.moveWindow('polar', 0, 0) else: cv2.imshow('disp', canvas_frame) cv2.waitKey(1) img_res_name = '%s%s\\%04d.jpg' % (resPath,obj_name, frame_id) #cv2.imwrite(img_res_name, canvas_frame) else:#readed file is empty break #closing dataset file fdata_obj.close() track_res_file.close() time_per_frame = total_time_consum/frame_counter print ('%s: average time consum per frame %fs \n' % (obj_name, time_per_frame)) return time_per_frame
observations = [] bar = ProgressBar() for sim in bar(range(num_sims)): # Create Random Physical Landmarks landmarksx = np.empty(num_landmarks) landmarksy = np.empty(num_landmarks) seed() for i in range(num_landmarks): landmarksx[i] = uniform(-landmark_range, landmark_range) landmarksy[i] = uniform(-landmark_range, landmark_range) # Initialize Car and Kalman Filter robot = robotcar(2, 0.5, num_landmarks, ts=ts) kf = kalman_filter(robot) # Initialize empty arrays for plotting t = np.empty(n) e = np.empty(n) x = np.empty(n) y = np.empty(n) # features in an observation include: # left_encoder # right_encoder # x_odom # y_odom # theta_odom # for each landmark: # bearing
[0, 1, 0, 0, 0, 0, 0, 0, ], [0, 0, 1, 0, 0, 0, 0, 0, ], [0, 0, 0, 1, 0, 0, 0, 0, ], [dt, 0, 0, 0, 1, 0, 0, 0, ], [0, dt, 0, 0, 0, 1, 0, 0, ], [0, 0, dt, 0, 0, 0, 1, 0, ], [0, 0, 0, dt, 0, 0, 0, 1, ]]) B = np.matrix([0]) C = np.eye(loc.shape[0]) Q = np.eye(loc.shape[0]) * q R = np.eye(loc.shape[0]) * r P = np.eye(loc.shape[0]) U = np.matrix([[0]]) Z = np.matrix([[0], [0], [0], [0], [0], [0], [0], [0] ]) kalman = kalman_filter.kalman_filter(A, B, C, Q, P, R, loc) #loop through data and apply Kalman filter for i in xrange(len(t)): kalman.move(U, Z) temp = kalman.getState() new_x.append(temp[5]) new_y.append(temp[4]*-1) new_z.append(temp[6]*-1) new_w.append(temp[7]) Z = np.matrix([[0], [0], [0], [0], [my_x[i]], [my_y[i]], [my_z[i]], [my_w[i]] ]) U = np.matrix([[0]]) #print raw and filtered data
# initial state variance parameters var_px0 = 10. var_py0 = 100. var_vx0 = 1. var_vy0 = 1. # initial state covariance PI = NP.array([[var_px0, 0, 0, 0], [ 0, var_vx0, 0, 0], [ 0, 0, var_py0, 0], [ 0, 0, 0, var_vy0]]) # execute filter x_hat, P = kalman_filter(y, H, R, F, Q, mu, PI, z=z) # gather results and plot px_hat = [x_hat_i[0] for x_hat_i in x_hat] py_hat = [x_hat_i[2] for x_hat_i in x_hat] fig = PL.figure(figsize=(11, 4)) ax = fig.add_subplot(111) PL.plot([x_i[0] for x_i in x], [x_i[2] for x_i in x], label='Truth') PL.plot(px_hat, py_hat, color='r', ls='-', mec='None',
def pose_move(pos): #pos.position.z is in kPa, has to be convereted to depth # P = P0 + pgz ----> pos.position.z = P0 + pg*z_real #z_real = -(1000*pos.position.z-101.325)/9.81 z_real = -pos.position.z toDegree = 180 / math.pi current.position.z = z_real broadcaster = tf.TransformBroadcaster() #set up the Kalman Filter #tf.transformations.quaternion_from_euler() (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([ -1 * pos.orientation.y, pos.orientation.x, -1 * pos.orientation.z, pos.orientation.w ]) roll = roll * toDegree pitch = pitch * toDegree yaw = yaw * toDegree s = 'The value of roll is ' + repr(roll) + ', and pitch is ' + repr( pitch) + ', yaw = ' + repr(yaw) print s if pose_move.start: dt = .02 pos = np.matrix([ [0], # v_x [0], # v_y [0], # v_z [roll], # x [pitch], # y [yaw] ]) # z A = np.matrix([[ 1, 0, 0, 0, 0, 0, ], [ 0, 1, 0, 0, 0, 0, ], [ 0, 0, 1, 0, 0, 0, ], [ dt, 0, 0, 1, 0, 0, ], [ 0, dt, 0, 0, 1, 0, ], [ 0, 0, dt, 0, 0, 1, ]]) B = np.matrix([0]) C = np.eye(pos.shape[0]) Q = np.eye(pos.shape[0]) * .5 R = np.eye(pos.shape[0]) * 500 P = np.eye(pos.shape[0]) U = np.matrix([[0]]) Z = np.matrix([[0], [0], [0], [0], [0], [0]]) pose_move.kalman = kalman_filter.kalman_filter(A, B, C, Q, P, R, pos) pose_move.start = 0 Z = np.matrix([[0], [0], [0], [roll], [pitch], [yaw]]) U = np.matrix([[0]]) pose_move.kalman.move(U, Z) pos = pose_move.kalman.getState() roll = pos[3] pitch = pos[4] yaw = pos[5] # quad = tf.transformations.quaternion_from_euler(roll/toDegree, pitch/toDegree, yaw/toDegree ) quad = tf.transformations.quaternion_from_euler(0, 0, yaw / toDegree) current.orientation.x = quad[0] current.orientation.y = quad[1] current.orientation.z = quad[2] current.orientation.w = quad[3] broadcaster.sendTransform( (current.position.x, current.position.y, current.position.z), (current.orientation.x, current.orientation.y, current.orientation.z, current.orientation.w), rospy.Time.now(), "body", "odom")
img = np.array(image, dtype=np.uint8) img.resize([resolution[1], resolution[0], 3]) vision_sensor_output = cv2.rotate(img, cv2.ROTATE_180) try: parsing_data = frame_buffer(vision_sensor_output) elapsed_time = time.time() - starting_time fps = frame_id / elapsed_time #Unpacking float datas from gyro and accel floatValues1 = vrep.simxUnpackFloats( gyro_out) floatValues2 = vrep.simxUnpackFloats( accel_out) #parsing data to alpha trimmmer CF_out = ComplementaryFilter(floatValues2, floatValues1, eulerAngles, RPY) #parsing data to alpha trimmmer KF_Out = kalman_filter(filtered_data, 0, 0, 0) display_frame = info_frame(parsing_data, filtered_data, fps, KF_Out, CF_out[2], position, position2) cv2.imshow("Image", display_frame) # print("Detection successful") cv2.waitKey(100) except: cv2.imshow("Image", vision_sensor_output) elif ERRORCODE == vrep.simx_return_novalue_flag: pass else: print(ERRORCODE) else: vrep.simxFinish(clientID) cv2.destroyAllWindows()
def usbl_move(pos): broadcaster = tf.TransformBroadcaster() if usbl_move.start: dt = 2 loc = np.matrix([ [0], #v_x [0], #v_y [pos.pose.position.x], #x [pos.pose.position.y] ]) #z A = np.matrix([[ 1, 0, 0, 0, ], [ 0, 1, 0, 0, ], [ dt, 0, 1, 0, ], [ 0, dt, 0, 1, ]]) B = np.matrix([0]) C = np.eye(loc.shape[0]) Q = np.eye(loc.shape[0]) * 0.5 R = np.eye(loc.shape[0]) * 5000 P = np.eye(loc.shape[0]) U = np.matrix([[0]]) Z = np.matrix([[0], [0], [0], [0]]) usbl_move.kalman = kalman_filter.kalman_filter(A, B, C, Q, P, R, loc) usbl_move.md_filter = md_filter.md_filter(2, [1.75, 1.6], 10, [0, 1]) usbl_move.start = 0 if usbl_move.md_filter.update([pos.pose.position.x, pos.pose.position.y]): #update.ax.scatter(pos.position.x,pos.position.y,-1*current.position.z,color='b') current.position.x = pos.pose.position.x current.position.y = pos.pose.position.y #current.position.z = pos.pose.position.z #update('b') Z = np.matrix([[0], [0], [pos.pose.position.x], [pos.pose.position.y]]) U = np.matrix([[0]]) usbl_move.kalman.move(U, Z) kalman_pos = usbl_move.kalman.getState() current.position.y = kalman_pos[2] current.position.x = kalman_pos[3] broadcaster.sendTransform( (current.position.x, current.position.y, current.position.z), (current.orientation.x, current.orientation.y, current.orientation.z, current.orientation.w), rospy.Time.now(), "body", "odom")
def pose_move(pos): #pos.position.z is in kPa, has to be convereted to depth # P = P0 + pgz ----> pos.position.z = P0 + pg*z_real #z_real = -(1000*pos.position.z-101.325)/9.81 z_real = -pos.position.z toDegree = 180/math.pi current.position.z = z_real broadcaster = tf.TransformBroadcaster() #set up the Kalman Filter #tf.transformations.quaternion_from_euler() (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([-1*pos.orientation.y, pos.orientation.x, -1*pos.orientation.z, pos.orientation.w]) roll = roll*toDegree pitch = pitch*toDegree yaw = yaw*toDegree s = 'The value of roll is ' + repr(roll) + ', and pitch is ' + repr(pitch) + ', yaw = ' + repr(yaw) print s if pose_move.start: dt = .02 pos = np.matrix([[0], # v_x [0], # v_y [0], # v_z [roll], # x [pitch], # y [yaw]]) # z A = np.matrix([[1, 0, 0, 0, 0, 0, ], [0, 1, 0, 0, 0, 0, ], [0, 0, 1, 0, 0, 0, ], [dt, 0, 0, 1, 0, 0, ], [0, dt, 0, 0, 1, 0, ], [0, 0, dt, 0, 0, 1, ]]) B = np.matrix([0]) C = np.eye(pos.shape[0]) Q = np.eye(pos.shape[0]) * .5 R = np.eye(pos.shape[0]) * 500 P = np.eye(pos.shape[0]) U = np.matrix([[0]]) Z = np.matrix([[0], [0], [0], [0], [0], [0] ]) pose_move.kalman = kalman_filter.kalman_filter(A,B,C,Q,P,R,pos) pose_move.start = 0 Z = np.matrix([[0], [0], [0], [roll], [pitch], [yaw]]) U = np.matrix([[0]]) pose_move.kalman.move(U, Z) pos = pose_move.kalman.getState() roll = pos[3] pitch = pos[4] yaw = pos[5] # quad = tf.transformations.quaternion_from_euler(roll/toDegree, pitch/toDegree, yaw/toDegree ) quad = tf.transformations.quaternion_from_euler(0, 0, yaw/toDegree ) current.orientation.x = quad[0] current.orientation.y = quad[1] current.orientation.z = quad[2] current.orientation.w = quad[3] broadcaster.sendTransform( (current.position.x,current.position.y,current.position.z), (current.orientation.x,current.orientation.y,current.orientation.z, current.orientation.w), rospy.Time.now(), "body", "odom" )
controller = LQR(K, max_input=20, max_input_on=False) """ OBSERVER REPRESENTATION """ # Convert to discrete time inverse_pendulum_plant_d_kalman = inverse_pendulum_plant.to_discrete( dt_control) # Extract discrete state matrices A_discrete_k = inverse_pendulum_plant_d_kalman.A B_discrete_k = inverse_pendulum_plant_d_kalman.B C_discrete_k = inverse_pendulum_plant_d_kalman.C D_discrete_k = inverse_pendulum_plant_d_kalman.D # Kalman Filter kf = kalman_filter(A_discrete_k, B_discrete_k, C_discrete_k, Q_kalman, R_kalman) # initilize states and covariance kf.set_x_last(states) kf.set_P_last(np.eye(np.size(A, 1)) * noise_val**2) #kf = kalman_filter(A, B, C, Q, R) """ UTILITY FUNCTIONS """ # Take a state input and add random noise def add_noise(states): for i in range(len(states - 1)): noise = (random.random() * 2 - 1) * noise_val states[i] += noise
def pos_move(pos, R, correction): # Note pos and current position have x and y swapped. # -> pos.position.x corresponds to current.position.y # Check that the values aren't crazy if correction: max = 5 # The pos is in world coordinates need to translate to kalman filter coordinates (x->y, y->x) a = quaternion_matrix([pos.pose.orientation.x, pos.pose.orientation.y, pos.pose.orientation.z, pos.pose.orientation.w]) a[0,3] = pos.pose.position.x a[1,3] = pos.pose.position.y b = np.array([current.position.x, current.position.y, 0, 1]) c = a.dot(b) pos.pose.position.x = c[1] pos.pose.position.y = c[0] else: max = 10 if(abs(pos.pose.position.x - current.position.y) > max or abs(pos.pose.position.y - current.position.x) > max ): print "CRAZY POSITION" return broadcaster = tf.TransformBroadcaster() if pos_move.start: dt = 0.0 loc = np.matrix([[0],#v_x [0],#v_y [pos.pose.position.x],#x [pos.pose.position.y]])#y A = np.matrix([[1, 0, 0, 0,], [0, 1, 0, 0,], [dt, 0, 1, 0,], [0, dt, 0, 1,]]) B = np.matrix([0]) C = np.eye(loc.shape[0]) Q = np.eye(loc.shape[0])*0.5 # R = np.eye(loc.shape[0])*5000 P = np.eye(loc.shape[0]) U = np.matrix( [[0]]) Z = np.matrix( [[0],[0],[0],[0]]) pos_move.kalman = kalman_filter.kalman_filter(A,B,C,Q,P,R,loc) pos_move.md_filter = md_filter.md_filter(2, [1.75, 1.6], 10, [0, 1]) pos_move.path = Path() pos_move.path.header.frame_id="odom" pos_move.start = 0 if not correction: pos.pose.position.x += pos_move.xd pos.pose.position.y += pos_move.yd # if pos_move.md_filter.update( [pos.pose.position.x,pos.pose.position.y] ) or correction or not correction: Z = np.matrix( [[0],[0],[pos.pose.position.x],[pos.pose.position.y] ]) U = np.matrix( [[0]]) pos_move.kalman.move(U,Z, R) kalman_pos = pos_move.kalman.getState() if correction: # We are subscribing to the inverse translation, thus the minus sign. pos_move.xd += kalman_pos.item(2) - current.position.y pos_move.yd += kalman_pos.item(3) - current.position.x # print "addition: ", pose_move.yaw_addition # print "pos:", "x:", current.position.x, "y:", current.position.y # print "mx: ", pos_move.xd # print "my: ", pos_move.yd # print "yaw:", pose_move.yaw current.position.y = kalman_pos.item(2) current.position.x = kalman_pos.item(3) # else: # print "NOT ACCEPTED" path_now = PoseStamped() path_now.pose.position.x = current.position.x path_now.pose.position.y = current.position.y path_now.pose.position.z = 0 # from odom (parent) to body (child) broadcaster.sendTransform( (current.position.x,current.position.y,current.position.z), (current.orientation.x,current.orientation.y,current.orientation.z,current.orientation.w), rospy.Time.now(), "body", "odom" ) # from buffer (parent) to odom (child) broadcaster.sendTransform((-pos_move.yd, -pos_move.xd, 0), tf.transformations.quaternion_from_euler(0, 0, -pose_move.yaw/180*math.pi), rospy.Time.now(),"odom","buffer") pos_move.path.poses.append(path_now) path.publish(pos_move.path) pose_kalman = PoseStamped(); pose_kalman.pose.position.x = current.position.x pose_kalman.pose.position.y = current.position.y pose_kalman.pose.position.z = current.position.z pose_kalman.pose.orientation.x = current.orientation.x pose_kalman.pose.orientation.y = current.orientation.y pose_kalman.pose.orientation.z = current.orientation.z pose_kalman.pose.orientation.w = current.orientation.w pose_kalman.header.frame_id = "odom" pub_pose.publish(pose_kalman)
def pose_move(pos, R, correction): toDegree = 180/math.pi broadcaster = tf.TransformBroadcaster() (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([-1*pos.orientation.y, pos.orientation.x, -1*pos.orientation.z, pos.orientation.w]) roll = roll*toDegree pitch = pitch*toDegree yaw = yaw*toDegree if correction: if abs(yaw) > 45: print "CRAZY YAW" return roll = pose_move.kalman.getState().item(3) pitch = pose_move.kalman.getState().item(4) yaw = pose_move.kalman.getState().item(5) - yaw if pose_move.start: dt = .00 position = np.matrix([[0], # v_x [0], # v_y [0], # v_z [roll], # x [pitch], # y [yaw]]) # z A = np.matrix([[1, 0, 0, 0, 0, 0, ], [0, 1, 0, 0, 0, 0, ], [0, 0, 1, 0, 0, 0, ], [dt, 0, 0, 1, 0, 0, ], [0, dt, 0, 0, 1, 0, ], [0, 0, dt, 0, 0, 1, ]]) B = np.matrix([0]) C = np.eye(position.shape[0]) Q = np.eye(position.shape[0]) * .5 R = np.eye(position.shape[0]) * 50 P = np.eye(position.shape[0]) U = np.matrix([[0]]) Z = np.matrix([[0], [0], [0], [0], [0], [0] ]) pose_move.kalman = kalman_filter.kalman_filter(A,B,C,Q,P,R,position) pose_move.start = 0 if not correction: yaw += pose_move.yaw current.position.z = -pos.position.z # current.position.z = 0 # Z = np.matrix([[0], [0], [0], [roll], [pitch], [yaw+pose_move.yaw_addition]]) Z = np.matrix([[0], [0], [0], [roll], [pitch], [yaw]]) # Z = np.matrix([[0], [0], [0], [0], [0], [yaw]]) U = np.matrix([[0]]) tmp_yaw = pose_move.kalman.getState().item(5) pose_move.kalman.move(U, Z, R) pos = pose_move.kalman.getState() if correction: # print "yaw input:", yaw # print "yaw before:", tmp_yaw # print "yaw after:", pos[5] # print "yaw change:", yaw - tmp_yaw pose_move.yaw += pos.item(5) - tmp_yaw # print "yaw addition:", pose_move.yaw roll = pos[3] pitch = pos[4] yaw = pos[5] quad = tf.transformations.quaternion_from_euler(roll/toDegree, pitch/toDegree, yaw/toDegree ) # quad = tf.transformations.quaternion_from_euler(0, 0, yaw/toDegree ) current.orientation.x = quad[0] current.orientation.y = quad[1] current.orientation.z = quad[2] current.orientation.w = quad[3] broadcaster.sendTransform( (current.position.x,current.position.y,current.position.z), (current.orientation.x,current.orientation.y,current.orientation.z, current.orientation.w), rospy.Time.now(), "body", "odom" ) broadcaster.sendTransform((-pos_move.yd,-pos_move.xd, 0), tf.transformations.quaternion_from_euler(0, 0, -pose_move.yaw/180*math.pi), rospy.Time.now(),"odom","buffer")