def analyze_measdata(filename=None): if filename is not None: measfile_rel_path = path.relpath('Measurements/' + filename + '.txt') else: measfile_rel_path = hc_tools.select_file( functionname='analyze_measdata') lambda_t, gamma_t = rf_tools.analyze_measdata_from_file( analyze_tx=[1, 2], measfile_path=measfile_rel_path) return lambda_t, gamma_t
def start_field_measurement_file_select(self): #read data from waypoint file wplist_filename = hc_tools.select_file() print(wplist_filename) measdata_filename = hc_tools.save_as_dialog() print(measdata_filename) self.start_RfEar() freqtx, numtx, tx_abs_pos = self.__oRf.get_txparams() print(freqtx) print(numtx) print(tx_abs_pos) self.process_measurement_sequence(wplist_filename, measdata_filename, numtx, tx_abs_pos, freqtx)
def position_estimation(x_start=None, filename=None, cal_param_file=None, sym_meas=None): if filename is not None: if sym_meas is True: measfile_rel_path = path.relpath('Simulated_measurements/' + filename + '.txt') else: measfile_rel_path = path.relpath('Measurements/' + filename + '.txt') else: measfile_rel_path = hc_tools.select_file( functionname='position estimation') est.main(sym_meas, x_start, measfile_rel_path, cal_param_file, make_plot=True)
def start_field_measurement_file_select(self): # read data from waypoint file wplist_filename = hc_tools.select_file() print(wplist_filename) measdata_filename = hc_tools.save_as_dialog() print(measdata_filename) meas_description = hc_tools.write_descrition() #self.start_RfEar() #freqtx, numtx, tx_abs_pos = self.__oRf.get_txparams() #print(freqtx) #print(numtx) #print(tx_abs_pos) #meas_description gehoert ans ende self.process_measurement_sequence( wplist_filename, measdata_filename, meas_description) #numtx, tx_abs_pos, freqtx, meas_description)
def follow_wp_path_opt_take_measurements( self, num_plot_points=250, model_type='log', b_take_meas=False, b_log_data=False, tolmm=10, tolrad=10e-3, start_wp=[1000, 1000, 0.5 * np.pi], sample_size=32): """ :param b_take_meas: :param start_wp: :param sample_size: :return: """ if b_take_meas is True: # dont start SDR if not RSS measurements will be taken self.start_RfEar() self.__oRf.set_samplesize(sample_size) sample_size = self.__oRf.get_samplesize() wplist_filename = hc_tools.select_file() print(wplist_filename) wp_append_list = [] with open(wplist_filename, 'r') as wpfile: for i, line in enumerate(wpfile): # print('line = ' + line) # print(line.split(',')) temp_list = line.split(',') wp_append_list.append(map(float, temp_list[0:-1])) # print(str(np.asarray(wp_append_list))) wp_list = np.asarray(wp_append_list) if b_log_data: measdata_filename = hc_tools.save_as_dialog() # print(measdata_filename) num_wp = len(wp_list) print('Number of way points: ' + str(num_wp)) start_time = t.time() data_list = [] meas_counter = 0 time_elapsed = 0.0 tolx_mm = tolmm # mm toly_mm = tolmm # mm tola_rad = tolrad # rad b_ekf = True if b_ekf is True: # init EKF EKF = estimator.ExtendedKalmanFilter(model_type) import estimator_plot_tools EKF_plotter = estimator_plot_tools.EKF_Plot( EKF.get_tx_pos(), model_type) # follow wp sequence for wp in wp_list: print('wp in list = ' + str(wp)) # go to wp self.set_target_wp(wp) self.start_moving_gantry_to_target() not_arrived_at_wp = True print('Moving to wp = ' + str(wp)) # following sequence while not_arrived_at_wp: meas_counter += 1 time_elapsed = t.time() - start_time pos_x_mm, pos_y_mm, pos_a_rad = self.get_gantry_pos_xya_mmrad() if b_ekf is True: EKF.ekf_prediction() EKF.ekf_update(-85) # EKF.check_valid_position_estimate() # print(EKF.get_x_est()) EKF_plotter.add_x_est_to_plot(EKF.get_x_est()) EKF_plotter.update_meas_circles(EKF.get_z_meas(), EKF.get_tx_alpha(), EKF.get_tx_gamma(), True, EKF.get_y_est()) EKF_plotter.plot_gantry_pos([pos_x_mm, pos_y_mm]) EKF_plotter.plot_ekf_pos_live(True, num_plot_points) # EKF_plotter.add_p_cov_to_plot(EKF.get_p_mat()) #EKF_plotter.plot_p_cov(num_plot_points) if b_take_meas is True: # taking measurements freq_den_max, pxx_den_max = self.__oRf.get_rss_peaks() data_row = np.append([ meas_counter, time_elapsed, pos_x_mm, pos_y_mm, pos_a_rad ], pxx_den_max) else: data_row = [ meas_counter, time_elapsed, pos_x_mm, pos_y_mm, pos_a_rad ] # add new data to list data_list.append(data_row) # arrived at wp? -> go to next if self.confirm_arrived_at_target_wp(tolx_mm, toly_mm, tola_rad): not_arrived_at_wp = False meas_freq = meas_counter / time_elapsed print('Logging with avg. ' + str(meas_freq) + ' Hz') if b_log_data: with open(measdata_filename, 'w') as measfile: measfile.write('Measurement file for trajectory following\n') measfile.write('Measurement was taken on ' + t.ctime() + '\n') measfile.write('### begin grid settings\n') if b_take_meas is True: measfile.write('sample size = ' + str(sample_size) + ' [*1024]\n') measfile.write('avg. meas frequency = ' + str(meas_freq) + ' Hz\n') measfile.write('start_point =' + str(start_wp) + '\n') measfile.write('wp_list =' + str(wp_list) + '\n') if b_take_meas is True: measfile.write( 'data format = [meas_counter, time_elapsed, pos_x_mm, pos_y_mm, pos_a_rad], pxx_den_max\n' ) else: measfile.write( 'data format = [meas_counter, time_elapsed, pos_x_mm, pos_y_mm, pos_a_rad]\n' ) measfile.write('### begin data log\n') data_mat = np.asarray(data_list) for row in data_mat: row_string = '' for i in range(len(row)): row_string += str(row[i]) + ',' row_string += '\n' measfile.write(row_string) measfile.close() return True
def follow_wp_and_take_measurements(self, start_wp=[1000, 1000, 0.5 * np.pi], sample_size=32): self.start_RfEar() self.__oRf.set_samplesize(sample_size) sample_size = self.__oRf.get_samplesize() wplist_filename = hc_tools.select_file() print(wplist_filename) wp_append_list = [] with open(wplist_filename, 'r') as wpfile: for i, line in enumerate(wpfile): print('line = ' + line) print(line.split(',')) temp_list = line.split(',') wp_append_list.append(map(float, temp_list[0:-1])) print(str(np.asarray(wp_append_list))) wp_list = np.asarray(wp_append_list) measdata_filename = hc_tools.save_as_dialog() print(measdata_filename) num_wp = len(wp_list) print('Number of way points: ' + str(num_wp)) start_time = t.time() self.set_target_wp(start_wp) self.start_moving_gantry_to_target() print('Moving to start position = ' + str(start_wp)) while not self.confirm_arrived_at_target_wp(): t.sleep(0.2) print('Arrived at start point') t.sleep(0.5) print('Start following way point sequence') data_list = [] meas_counter = 0 time_elapsed = 0.0 for wp in wp_list: # go to wp self.set_target_wp(wp) self.start_moving_gantry_to_target() not_arrived_at_wp = True print('Moving to wp = ' + str(wp)) # taking measurements while not_arrived_at_wp: meas_counter += 1 time_elapsed = t.time() - start_time pos_x_mm, pos_y_mm, pos_a_rad = self.get_gantry_pos_xya_mmrad() freq_den_max, pxx_den_max = self.__oRf.get_rss_peaks() data_row = np.append([ meas_counter, time_elapsed, pos_x_mm, pos_y_mm, pos_a_rad ], pxx_den_max) data_list.append(data_row) if self.confirm_arrived_at_target_wp(): not_arrived_at_wp = False meas_freq = meas_counter / time_elapsed print('Logging with avg. ' + str(meas_freq) + ' Hz') with open(measdata_filename, 'w') as measfile: measfile.write('Measurement file for trajectory following\n') measfile.write('Measurement was taken on ' + t.ctime() + '\n') measfile.write('### begin grid settings\n') measfile.write('sample size = ' + str(sample_size) + ' [*1024]\n') measfile.write('avg. meas frequency = ' + str(meas_freq) + ' Hz\n') measfile.write('start_point =' + str(start_wp) + '\n') measfile.write('wp_list =' + str(wp_list) + '\n') measfile.write( 'data format = [meas_counter, time_elapsed, pos_x_mm, pos_y_mm, pos_a_rad], pxx_den_max\n' ) measfile.write('### begin data log\n') data_mat = np.asarray(data_list) for row in data_mat: row_string = '' for i in range(len(row)): row_string += str(row[i]) + ',' row_string += '\n' measfile.write(row_string) measfile.close() return True
def analyze_measdata_from_file(model_type='log', analyze_tx=[1, 2, 3, 4, 5, 6], meantype='db_mean', b_onboard=False, measfilename='path'): """ :param analyze_tx: :param txpos_tuning: :param meantype: :return: """ analyze_tx[:] = [x - 1 for x in analyze_tx ] # substract -1 as arrays begin with index 0 if b_onboard is True: measdata_filename = measfilename else: measdata_filename = hc_tools.select_file() # 123???todo with open(measdata_filename, 'r') as measfile: load_description = True load_grid_settings = False load_measdata = False meas_data_append_list = [] plotdata_mat_lis = [] totnumwp = 0 measured_wp_list = [] for i, line in enumerate(measfile): if line == '### begin grid settings\n': # print('griddata found') load_description = False load_grid_settings = True load_measdata = False continue elif line == '### begin measurement data\n': load_description = False load_grid_settings = False load_measdata = True # print('Measurement data found') continue if load_description: # print('file description') print(line) if load_grid_settings and not load_measdata: #print(line) grid_settings = map(float, line.split(',')) x0 = [grid_settings[0], grid_settings[1], grid_settings[2]] xn = [grid_settings[3], grid_settings[4], grid_settings[5]] grid_dxdyda = [ grid_settings[6], grid_settings[7], grid_settings[8] ] timemeas = grid_settings[9] data_shape_file = [ int((xn[0] - x0[0]) / grid_dxdyda[0] + 1), int((xn[1] - x0[1]) / grid_dxdyda[1] + 1), int((xn[2] - x0[2]) / grid_dxdyda[2] + 1) ] print('data shape = ' + str(data_shape_file)) numtx = int(grid_settings[10]) txdata = grid_settings[11:( 11 + 4 * numtx)] # urspruenglich [(2+numtx):(2+numtx+3*numtx)] # read tx positions txpos_list = [] for itx in range(numtx): itxpos = txdata[3 * itx:3 * itx + 3] # urspruenglich [2*itx:2*itx+2] txpos_list.append(itxpos) txpos = np.asarray(txpos_list) # read tx frequencies freqtx_list = [] for itx in range(numtx): freqtx_list.append( txdata[3 * numtx + itx]) # urspruenglich (txdata[2*numtx+itx]) freqtx = np.asarray(freqtx_list) # print out print('filename = ' + measdata_filename) print('num_of_gridpoints = ' + str(data_shape_file[0] * data_shape_file[1])) print('x0 = ' + str(x0)) print('xn = ' + str(xn)) print('grid_shape = ' + str(data_shape_file)) print('steps_dxdyda = ' + str(grid_dxdyda)) print('tx_pos = ' + str(txpos_list)) print('freqtx = ' + str(freqtx)) startx = x0[0] endx = xn[0] stepx = data_shape_file[0] starty = x0[1] endy = xn[1] stepy = data_shape_file[1] starta = x0[2] enda = xn[2] stepa = data_shape_file[2] xpos = np.linspace(startx, endx, stepx) ypos = np.linspace(starty, endy, stepy) apos = np.linspace(starta, enda, stepa) wp_matx, wp_maty, wp_mata = np.meshgrid(xpos, ypos, apos) # print(xpos) if load_measdata and not load_grid_settings: # print('read measdata') totnumwp += 1 meas_data_line = map(float, line[0:-3].split(', ')) meas_data_append_list.append(meas_data_line) meas_data_mat_line = np.asarray(meas_data_line) measured_wp_list.append(int(meas_data_mat_line[3])) num_tx = int(meas_data_mat_line[4]) num_meas = int(meas_data_mat_line[5]) first_rss = 6 + num_tx meas_data_mat_rss = meas_data_mat_line[first_rss:] rss_mat_raw = meas_data_mat_rss.reshape( [num_tx, num_meas]) # mat_dim: num_tx x num_meas def reject_outliers(data, m=5.): d = np.abs(data - np.median(data)) mdev = np.median(d) s = d / mdev if mdev else 0. # print('kicked out samples' + str([s < m])) return data[s < m] if meantype is 'lin': rss_mat_lin = 10**(rss_mat_raw / 10) mean_lin = np.mean(rss_mat_lin, axis=1) var_lin = np.var(rss_mat_lin, axis=1) mean = 10 * np.log10(mean_lin) var = 10 * np.log10(var_lin) else: mean = np.zeros([numtx]) var = np.zeros([numtx]) for itx in range(numtx): rss_mat_row = reject_outliers(rss_mat_raw[itx, :]) # print('kicked out samples:' + str(len(rss_mat_raw[itx, :]) - len(rss_mat_row))) mean[itx] = np.mean(rss_mat_row) var[itx] = np.var(rss_mat_row) # print('var = ' + str(var)) wp_pos = [ meas_data_mat_line[0], meas_data_mat_line[1], meas_data_mat_line[2] ] plotdata_line = np.concatenate( (wp_pos, mean, var), axis=0) # -> x,y,a,meantx1,...,meantxn,vartx1,...vartxn plotdata_mat_lis.append(plotdata_line) measfile.close() data_shape = [ data_shape_file[0], data_shape_file[1], data_shape_file[2] ] # data_shape: n_x, n_y, n_a plotdata_mat = np.asarray(plotdata_mat_lis) """ Model fit ---> with 3D todo: change with compensation """ if model_type == 'log': def rsm_model(dist_rsm, alpha_rsm, gamma_rsm): """Range Sensor Model (RSM) structure.""" return -20 * np.log10( dist_rsm) - alpha_rsm * dist_rsm - gamma_rsm # rss in db elif model_type == 'lin': def rsm_model(dist_rsm, alpha_rsm, gamma_rsm): """Range Sensor Model (RSM) structure.""" return alpha_rsm * dist_rsm + gamma_rsm # rss in db alpha = [] gamma = [] rdist = [] for itx in analyze_tx: rdist_vec = plotdata_mat[:, 0:2] - txpos[ itx, 0: 2] # +[250 , 30, 0] # r_wp -r_txpos -> dim: num_meas x 2or3 (3 if z is introduced) # todo: previous line: change from 2 to 3 if z is introduced if itx == 5: print('r_dist ' + str(rdist_vec)) rdist_temp = np.asarray(np.linalg.norm( rdist_vec, axis=1)) # distance norm: |r_wp -r_txpos| -> dim: num_meas x 1 rssdata = plotdata_mat[:, 3 + itx] # rss-mean for each wp popt, pcov = curve_fit(rsm_model, rdist_temp, rssdata) del pcov alpha.append(round(popt[0]), 4) gamma.append(round(popt[1]), 4) # print('tx #' + str(itx+1) + ' alpha= ' + str(alpha[itx]) + ' gamma= ' + str(gamma[itx])) rdist.append(rdist_temp) rdist_temp = np.reshape(rdist, [num_tx, totnumwp]) if model_type == 'log': print('\nVectors for convenient copy/paste') print('alpha_log = ' + str(alpha)) print('gamma_log = ' + str(gamma)) elif model_type == 'lin': print('\nVectors for convenient copy/paste') print('alpha_lin = ' + str(alpha)) print('gamma_lin = ' + str(gamma)) """ Plots """ if b_onboard is False: x = plotdata_mat[:, 0] y = plotdata_mat[:, 1] fig = plt.figure(6) for itx in analyze_tx: pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos) ax.errorbar(rdist, rss_mean, yerr=rss_var, fmt='ro', markersize='1', ecolor='g', label='Original Data') rdata = np.linspace(50, np.max(rdist), num=1000) ax.plot(rdata, rsm_model(rdata, alpha[itx], gamma[itx]), label='Fitted Curve') ax.legend(loc='upper right') ax.grid() ax.set_ylim([-110, -10]) ax.set_xlabel('Distance [mm]') ax.set_ylabel('RSS [dB]') ax.set_title('RSM for TX# ' + str(itx + 1)) rss_mean = plotdata_mat[:, 3 + itx] rss_var = plotdata_mat[:, 3 + num_tx + itx] rss_mat_ones = np.ones(np.shape(wp_matx)) * ( -200) # set minimum value for not measured points rss_full_vec = np.reshape( rss_mat_ones, (len(xpos) * len(ypos) * len(apos), 1)) measured_wp_list = np.reshape(measured_wp_list, (len(measured_wp_list), 1)) rss_mean = np.reshape(rss_mean, (len(rss_mean), 1)) rss_full_vec[measured_wp_list, 0] = rss_mean rss_full_mat = np.reshape(rss_full_vec, data_shape) # mask all points which were not measured rss_full_mat = np.ma.array(rss_full_mat, mask=rss_full_mat < -199) val_sequence = np.linspace(-100, -20, 80 / 5 + 1) # CS = ax.contour(wp_matx[::2,::2], wp_maty[::2,::2], rss_full_mat[::2,::2], val_sequence) # takes every second value CS = ax.contour(wp_matx, wp_maty, rss_full_mat, val_sequence) ax.clabel(CS, inline=0, fontsize=10) for itx_plot in analyze_tx: ax.plot(txpos[itx_plot - 1, 0], txpos[itx_plot - 1, 1], 'or') ax.grid() ax.set_xlabel('x [mm]') ax.set_ylabel('y [mm]') ax.axis('equal') ax.set_title('RSS field for TX# ' + str(itx + 1)) plot_fig1 = False if plot_fig1: fig = plt.figure(1) for itx in analyze_tx: pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos, projection='3d') rss_mean = plotdata_mat[:, 3 + itx] rss_var = plotdata_mat[:, 3 + num_tx + itx] ax.plot_trisurf(x, y, rss_mean, cmap=plt.cm.Spectral) ax.grid() ax.set_xlabel('x [mm]') ax.set_ylabel('y [mm]') ax.set_zlabel('rss [dB]') ax.set_zlim([-110, -20]) ax.set_title('RSS field for TX# ' + str(itx + 1)) plot_fig2 = True if plot_fig2: fig = plt.figure(2) for itx in analyze_tx: pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos, projection='3d') rss_mean = plotdata_mat[:, 3 + itx] rss_var = plotdata_mat[:, 3 + num_tx + itx] ax.plot_trisurf(x, y, rss_var, cmap=plt.cm.Spectral) ax.grid() ax.set_xlabel('x [mm]') ax.set_ylabel('y [mm]') ax.set_zlabel('rss_var [dB]') ax.set_title('RSS field variance for TX# ' + str(itx + 1)) plot_fig3 = True if plot_fig3: fig = plt.figure(3) for itx in analyze_tx: rss_mean = plotdata_mat[:, 3 + itx] rss_var = plotdata_mat[:, 3 + num_tx + itx] rdist = np.array(rdist_temp[itx, :], dtype=float) rss_mean = np.array(rss_mean, dtype=float) rss_var = np.array(rss_var, dtype=float) pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos) ax.errorbar(rdist, rss_mean, yerr=rss_var, fmt='ro', markersize='1', ecolor='g', label='Original Data') rdata = np.linspace(np.min(rdist), np.max(rdist), num=1000) ax.plot(rdata, rsm_model(rdata, alpha[itx], gamma[itx]), label='Fitted Curve') ax.legend(loc='upper right') ax.grid() ax.set_ylim([-110, -10]) ax.set_xlabel('Distance [mm]') ax.set_ylabel('RSS [dB]') ax.set_title('RSM for TX# ' + str(itx + 1)) plot_fig4 = False if plot_fig4: fig = plt.figure(4) for itx in analyze_tx: rss_mean = plotdata_mat[:, 3 + itx] rss_var = plotdata_mat[:, 3 + num_tx + itx] rdist = np.array(rdist_temp[itx, :], dtype=float) rss_mean = np.array(rss_mean, dtype=float) rss_var = np.array(rss_var, dtype=float) pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos) rssdata = np.linspace(-10, -110, num=1000) ax.plot(rssdata, lambertloc(rssdata, alpha[itx], gamma[itx]), label='Fitted Curve') ax.plot(rss_mean, rdist, 'r.') ax.grid() ax.set_xlabel('RSS [dB]') ax.set_ylabel('Distance [mm]') plot_fig5 = False if plot_fig5: fig = plt.figure(5) for itx in analyze_tx: rss_mean = plotdata_mat[:, 3 + itx] rss_var = plotdata_mat[:, 3 + num_tx + itx] rdist = np.array(rdist_temp[itx, :], dtype=float) rss_mean = np.array(rss_mean, dtype=float) rss_var = np.array(rss_var, dtype=float) r_dist_est = lambertloc(rss_mean, alpha[itx], gamma[itx]) sorted_indices = np.argsort(rdist) r_dist_sort = rdist[sorted_indices] r_dist_est_sort = r_dist_est[sorted_indices] dist_error = r_dist_sort - r_dist_est_sort data_temp = [] bin = np.linspace(0, 2000, 21) ibin = 1 bin_mean = [] bin_var = [] for i in range(len(r_dist_sort)): if r_dist_sort[i] >= bin[-1]: break elif bin[ibin - 1] <= r_dist_sort[i] < bin[ibin]: data_temp.append(dist_error[i]) else: bin_mean_temp = np.mean(data_temp) bin_var_temp = np.var(data_temp) bin_mean.append(bin_mean_temp) bin_var.append(bin_var_temp) #print('bin_high_bound :' + str(bin[ibin]) + ' bin_mean:' + str(bin_mean_temp)) data_temp = [] # reset bin data_temp.append(dist_error[i]) ibin += 1 #print('ibin ' + str(ibin)) pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos) # rssdata = np.linspace(-10, -110, num=1000) # ax.plot(rssdata, lambertloc(rssdata, alpha[itx], gamma[itx]), label='Fitted Curve') # ax.errorbar(bin[1:-1], bin_mean, yerr=bin_var, fmt='ro', ecolor='g', label='Original Data') ax.plot(bin[1:-1], bin_mean, '.') # print('bin_means = ' + str(bin_mean)) # print('bin_var = ' + str(bin_var)) # ax.plot(r_dist_sort, dist_error, '.') ax.grid() ax.set_xlabel('Distance to tx [mm]') ax.set_ylabel('Error [mm]') plt.show() return alpha, gamma
def analyze_measdata_from_file(model_type='log', analyze_tx=[1, 2], meantype='db_mean', b_onboard=False, measfilename='path'): """ :param analyze_tx: :param txpos_tuning: :param meantype: :return: """ analyze_tx[:] = [x - 1 for x in analyze_tx ] # substract -1 as arrays begin with index 0 print analyze_tx if b_onboard is True: measdata_filename = measfilename else: measdata_filename = hc_tools.select_file() with open(measdata_filename, 'r') as measfile: load_description = True load_grid_settings = False load_measdata = False meas_data_append_list = [] plotdata_mat_lis = [] totnumwp = 0 measured_wp_list = [] for i, line in enumerate(measfile): if line == '### begin grid settings\n': # print('griddata found') load_description = False load_grid_settings = True load_measdata = False continue elif line == '### begin measurement data\n': load_description = False load_grid_settings = False load_measdata = True # print('Measurement data found') continue if load_description: # print('file description') print(line) if load_grid_settings and not load_measdata: #print(line) grid_settings = map(float, line[:-2].split(' ')) x0 = [grid_settings[0], grid_settings[1], grid_settings[2]] xn = [grid_settings[3], grid_settings[4], grid_settings[5]] grid_dxdyda = [ grid_settings[6], grid_settings[7], grid_settings[8] ] timemeas = grid_settings[9] data_shape_file = [] for i in range(3): # range(num_dof) try: shapei = int((xn[i] - x0[i]) / grid_dxdyda[i] + 1) except ZeroDivisionError: shapei = 1 data_shape_file.append(shapei) # old: data_shape_file = [int((xn[0]-x0[0]) / grid_dxdyda[0] + 1), int((xn[1]-x0[1]) / grid_dxdyda[1] + 1), int((xn[2]-x0[2]) / grid_dxdyda[2] + 1)] print('data shape = ' + str(data_shape_file)) numtx = int(grid_settings[10]) txdata = grid_settings[11:( 11 + 4 * numtx)] # urspruenglich [(2+numtx):(2+numtx+3*numtx)] # read tx positions txpos_list = [] for itx in range(numtx): itxpos = txdata[3 * itx:3 * itx + 3] # urspruenglich [2*itx:2*itx+2] txpos_list.append(itxpos) txpos = np.asarray(txpos_list) # read tx frequencies freqtx_list = [] for itx in range(numtx): freqtx_list.append( txdata[3 * numtx + itx]) # urspruenglich (txdata[2*numtx+itx]) freqtx = np.asarray(freqtx_list) # print out print('filename = ' + measdata_filename) print('num_of_gridpoints = ' + str(data_shape_file[0] * data_shape_file[1])) print('x0 = ' + str(x0)) print('xn = ' + str(xn)) print('grid_shape = ' + str(data_shape_file)) print('steps_dxdyda = ' + str(grid_dxdyda)) print('tx_pos = ' + str(txpos_list)) print('freqtx = ' + str(freqtx)) startx = x0[0] endx = xn[0] stepx = data_shape_file[0] starty = x0[1] endy = xn[1] stepy = data_shape_file[1] startz = x0[2] endz = xn[2] stepz = data_shape_file[2] xpos = np.linspace(startx, endx, stepx) ypos = np.linspace(starty, endy, stepy) zpos = np.linspace(startz, endz, stepz) # wp_matx, wp_maty, wp_matz = np.meshgrid(xpos, ypos, zpos) # old wp-creation with z axis being main moving axis wp_maty, wp_matz, wp_matx = np.meshgrid( ypos, zpos, xpos ) # put least moving axis second, then second least moving first, then quickest last # print(xpos) if load_measdata and not load_grid_settings: # print('read measdata') totnumwp += 1 meas_data_line = map(float, line[:-2].split(' ')) meas_data_append_list.append(meas_data_line) meas_data_mat_line = np.asarray(meas_data_line) measured_wp_list.append(int(meas_data_mat_line[3])) num_tx = int(meas_data_mat_line[4]) num_meas = int(meas_data_mat_line[5]) first_rss = 6 + num_tx meas_data_mat_rss = meas_data_mat_line[first_rss:] rss_mat_raw = meas_data_mat_rss.reshape( [num_tx, num_meas]) # mat_dim: num_tx x num_meas def reject_outliers(data, m=5.): d = np.abs(data - np.median(data)) mdev = np.median(d) s = d / mdev if mdev else 0. # print('kicked out samples' + str([s < m])) return data[s < m] if meantype is 'lin': rss_mat_lin = 10**(rss_mat_raw / 10) mean_lin = np.mean(rss_mat_lin, axis=1) var_lin = np.var(rss_mat_lin, axis=1) mean = 10 * np.log10(mean_lin) var = 10 * np.log10(var_lin) else: mean = np.zeros([numtx]) var = np.zeros([numtx]) for itx in range(numtx): rss_mat_row = reject_outliers(rss_mat_raw[itx, :]) # print('kicked out samples:' + str(len(rss_mat_raw[itx, :]) - len(rss_mat_row))) mean[itx] = np.mean(rss_mat_row) var[itx] = np.var(rss_mat_row) # print('var = ' + str(var)) wp_pos = np.array([ meas_data_mat_line[0], meas_data_mat_line[1], meas_data_mat_line[2] ]) # antenna_orientation = np.array([[0.0], [0.64278760968], [0.76604444311]]) antenna_orientation = np.array([[0.0], [0.0], [1.0]]) # antenna_orientation = np.array([[0], [0.34202014332], [0.93969262078]]) # todo: Enter antenna orientation for correct parameter calibration here! wp_angles = [0.0] * num_tx * 4 for itx in range(num_tx): wp_angles[itx * 4:itx * 4 + 4] = get_angles( np.transpose(wp_pos[0:2][np.newaxis]), np.transpose(txpos[itx, 0:2][np.newaxis]), txpos[itx, 2], antenna_orientation, wp_pos[2]) wp_angles = np.asarray(wp_angles) plotdata_line = np.concatenate( (wp_pos, mean, var, wp_angles), axis=0) # -> x,y,a,meantx1,...,meantxn,vartx1,...vartxn plotdata_mat_lis.append(plotdata_line) measfile.close() # data_shape = [data_shape_file[0], data_shape_file[1], data_shape_file[2]] # data_shape: n_x, n_y, n_z data_shape = [ data_shape_file[1], data_shape_file[0], data_shape_file[2] ] # data_shape: n_x, n_y, n_z plotdata_mat = np.asarray(plotdata_mat_lis) """ Model fit """ if model_type == 'log': ''' def rsm_model(rsm_params, lambda_rsm, gamma_rsm, n_t_rsm, n_r_rsm): """Range Sensor Model (RSM) structure.""" dist_rsm, psi_low_rsm, theta_cap_rsm, theta_low_rsm = rsm_params return -20 * np.log10(dist_rsm) + lambda_rsm * dist_rsm + gamma_rsm + np.log10(np.cos(abs(psi_low_rsm))) + n_t_rsm * np.log10(abs(np.cos(theta_cap_rsm))) + n_r_rsm * np.log10(abs(np.cos(theta_cap_rsm + theta_low_rsm))) # rss in db def rsm_model(rsm_params, lambda_rsm, gamma_rsm, n_r_rsm): """Range Sensor Model (RSM) structure.""" dist_rsm, psi_low_rsm, theta_cap_rsm, theta_low_rsm = rsm_params return -20 * np.log10(dist_rsm) + lambda_rsm * dist_rsm + gamma_rsm + np.log10(np.cos(abs(psi_low_rsm))) + n_r_rsm * np.log10(abs(np.cos(theta_low_rsm))) # rss in db def rsm_model(rsm_params, lambda_rsm, gamma_rsm, n_t_rsm): """Range Sensor Model (RSM) structure.""" dist_rsm, theta_cap_rsm, psi_low_rsm, theta_low_rsm = rsm_params return -20 * np.log10(dist_rsm) + lambda_rsm * dist_rsm + gamma_rsm + 2 * n_t_rsm * np.log10(abs(np.cos(theta_cap_rsm))) # rss in db ''' def rsm_model(rsm_params, lambda_rsm, gamma_rsm): """Range Sensor Model (RSM) structure.""" dist_rsm, theta_cap_rsm, psi_low_rsm, theta_low_rsm = rsm_params return -20 * np.log10( dist_rsm) + lambda_rsm * dist_rsm + gamma_rsm + np.log10( 3.83135740649**2) # rss in db elif model_type == 'lin': # todo: OLD: consider linearized Angles when use is desired. decide to not bother linearizing and throw this out of the code. def rsm_model(dist_rsm, lambda_rsm, gamma_rsm): """Range Sensor Model (RSM) structure.""" return lambda_rsm * dist_rsm + gamma_rsm # rss in db rdist = [] calibration_mode = True # True = measurement had a straight antenna and was performed on transmitter heights if calibration_mode: lambda_t = [] gamma_t = [] n_t = [] n_r = [] else: lambda_t = [] gamma_t = [ ] # todo: enter correct values here for no calibration use! n_t = [] n_r = [] for itx in analyze_tx: print analyze_tx print plotdata_mat[:, 0:3] print txpos[itx, 0:3] rdist_vec = plotdata_mat[:, 0:3] - txpos[ itx, 0: 3] # + [0, 0, 0] # r_wp -r_txpos -> dim: num_meas x 2or3 (3 if z is introduced) rdist_temp = np.asarray(np.linalg.norm( rdist_vec, axis=1)) # distance norm: |r_wp -r_txpos| -> dim: num_meas x 1 if calibration_mode: rssdata = plotdata_mat[:, 3 + itx] # rss-mean for each wp theta_cap = plotdata_mat[:, 3 + num_tx * 2 + 1 + itx * 4] psi_low = plotdata_mat[:, 3 + num_tx * 2 + 2 + itx * 4] theta_low = plotdata_mat[:, 3 + num_tx * 2 + 3 + itx * 4] rsm_paramtuple = rdist_temp, theta_cap, psi_low, theta_low popt, pcov = curve_fit( rsm_model, rsm_paramtuple, rssdata) #, bounds=([-np.inf, -np.inf, 0], np.inf) del pcov lambda_t.append(round(popt[0], 7)) gamma_t.append(round(popt[1], 4)) #n_t.append(round(popt[2], 4)) #n_r.append(round(popt[2], 4)) rdist.append(rdist_temp) rdist_temp = np.reshape(rdist, [num_tx, totnumwp]) if model_type == 'log': print('\nVectors for convenient copy/paste') print('lambda_t = np.array(' + str(lambda_t) + ') # ' + measdata_filename) print('gamma_t = np.array(' + str(gamma_t) + ') # ' + measdata_filename) print('tx_n = np.array(' + str(n_t) + ') # ' + measdata_filename) print('rx_n = np.array(' + str(n_r) + ') # ' + measdata_filename) elif model_type == 'lin': print('\nVectors for convenient copy/paste') print('lambda_lin = ' + str(lambda_t)) print('gamma_lin = ' + str(gamma_t)) """ Plots """ if b_onboard is False: x = plotdata_mat[:, 0] y = plotdata_mat[:, 1] plot_fig0 = False if plot_fig0: # 2D contour plot fig = plt.figure(0) analyze_tx = [3] for itx in analyze_tx: pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos) rdata = np.linspace(1, np.max(rdist), num=1000) phi_cap = np.array( [0.0] * len(rdata) ) # plotdata_mat[:, 3 + num_tx * 2 + 0 + itx * 4] # theta_cap = np.array( [0.0] * len(rdata) ) # plotdata_mat[:, 3 + num_tx * 2 + 1 + itx * 4] # psi_low = np.array( [0.0] * len(rdata) ) # plotdata_mat[:, 3 + num_tx * 2 + 2 + itx * 4] # theta_low = np.array( [0.0] * len(rdata) ) # plotdata_mat[:, 3 + num_tx * 2 + 3 + itx * 4] # ax.legend(loc='upper right') ax.grid() ax.set_ylim([-110, -10]) ax.set_xlabel('Distance [mm]') ax.set_ylabel('RSS [dB]') ax.set_title('RSM for TX# ' + str(itx + 1)) # ax.errorbar(rdist[itx], plotdata_mat[:, 3 + itx], yerr=plotdata_mat[:, 3 + num_tx + itx], fmt='ro', markersize='1', ecolor='g', label='Original Data', zorder=1) for iter in np.linspace(0, 600, 7): height_for_plot = iter for i in range(len(rdata)): phi_cap[i], theta_cap[i], psi_low[i], theta_low[ i] = get_angles( np.array([[txpos[itx][0] + rdata[i]], [txpos[itx][1]]]), np.array([[txpos[itx][0]], [txpos[itx][1]]]), txpos[itx][2], antenna_orientation, height_for_plot) rsm_paramtuple_plot = rdata, theta_cap, psi_low, theta_low ax.plot(rdata, rsm_model(rsm_paramtuple_plot, lambda_t[itx], gamma_t[itx], n_t[itx]), label='Fitted Curve', zorder=2) # , n_r[itx] fig.subplots_adjust(hspace=0.4) fig = plt.figure(1, figsize=(18, 10)) # fig = plt.figure(1, figsize=(5,2.5)) for itx in analyze_tx: pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos) rss_mean = plotdata_mat[:, 3 + itx] rss_var = plotdata_mat[:, 3 + num_tx + itx] rss_mat_ones = np.ones(np.shape(wp_matx)) * ( -200) # set minimum value for not measured points rss_full_vec = np.reshape( rss_mat_ones, (len(xpos) * len(ypos) * len(zpos), 1)) measured_wp_list = np.reshape(measured_wp_list, (len(measured_wp_list), 1)) measured_wp_list[:] -= measured_wp_list[ 0] # In case that measurements have been selected manually and measurements are not the first ones -> first measurement is meas zero and so on rss_mean = np.reshape(rss_mean, (len(rss_mean), 1)) rss_full_vec[measured_wp_list, 0] = rss_mean rss_full_mat = np.reshape(rss_full_vec, data_shape) # mask all points which were not measured # rss_full_mat = np.ma.array(rss_full_mat, mask=rss_full_mat < -199) # np.ones(np.shape(wp_maty))*(-60) val_sequence = np.linspace(-100, -20, 80 / 5 + 1) # CS = ax.contour(wp_matx[::2, ::2], wp_maty[::2, ::2], rss_full_mat[::2, ::2], val_sequence) # takes every second value CS = ax.contour(wp_matx[0, :, :], wp_maty[0, :, :], rss_full_mat[:, :, 0], val_sequence, cmap=plt.cm.jet, label='RSS Contours') ax.clabel(CS, inline=0, fontsize=10) for itx_plot in analyze_tx: if itx_plot == itx: ax.plot(txpos[itx_plot, 0], txpos[itx_plot, 1], 'or', c='orangered', markersize=15) else: ax.plot(txpos[itx_plot, 0], txpos[itx_plot, 1], 'or') ax.grid() ax.set_xlabel('x [mm]') ax.set_ylabel('y [mm]') ax.axis('equal') ax.set_title('RSS field for TX# ' + str(itx + 1)) fig.subplots_adjust(hspace=0.4) fig = plt.figure(8) for itx in analyze_tx: pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos, projection='3d') rss_2_plot = -70 rss_2_plot_var = 1 same_rss_indexes = np.where( np.logical_and( plotdata_mat[:, 3 + itx] <= (rss_2_plot + rss_2_plot_var), plotdata_mat[:, 3 + itx] >= (rss_2_plot - rss_2_plot_var))) CS = ax.scatter( plotdata_mat[same_rss_indexes[0], 0], plotdata_mat[same_rss_indexes[0], 1], plotdata_mat[same_rss_indexes[0], 2], label='Scatter 3D for same RSS' ) # for coloring kwag: c=plotdata_mat[same_rss_indexes[0], 3+itx] # CS = ax.scatter(wp_matx[:, :, 0], wp_maty[:, :, 0], wp_matz[:, :, 0], val_sequence) ax.clabel(CS, inline=0, fontsize=10) ax.scatter(txpos[itx, 0], txpos[itx, 1], txpos[itx, 2], color='r', s=100) ax.grid() ax.set_xlabel('x [mm]') ax.set_ylabel('y [mm]') ax.axis('equal') ax.set_title('RSS field for TX# ' + str(itx + 1)) fig.subplots_adjust(hspace=0.4) plot_fig2 = False if plot_fig2: fig = plt.figure(2) for itx in analyze_tx: pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos, projection='3d') rss_mean = plotdata_mat[:, 3 + itx] rss_var = plotdata_mat[:, 3 + num_tx + itx] ax.plot_trisurf(x, y, rss_mean, cmap=plt.cm.Spectral) ax.grid() ax.set_xlabel('x [mm]') ax.set_ylabel('y [mm]') ax.set_zlabel('rss [dB]') ax.set_zlim([-110, -20]) ax.set_title('RSS field for TX# ' + str(itx + 1)) plot_fig3 = False if plot_fig3: fig = plt.figure(3) for itx in analyze_tx: pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos, projection='3d') rss_mean = plotdata_mat[:, 3 + itx] rss_var = plotdata_mat[:, 3 + num_tx + itx] ax.plot_trisurf(x, y, rss_var, cmap=plt.cm.Spectral) ax.grid() ax.set_xlabel('x [mm]') ax.set_ylabel('y [mm]') ax.set_zlabel('rss_var [dB]') ax.set_title('RSS field variance for TX# ' + str(itx + 1)) plot_fig4 = True if plot_fig4: fig = plt.figure(4) for itx in analyze_tx: rss_mean = plotdata_mat[:, 3 + itx] rss_var = plotdata_mat[:, 3 + num_tx + itx] rdist = np.array(rdist_temp[itx, :], dtype=float) rss_mean = np.array(rss_mean, dtype=float) rss_var = np.array(rss_var, dtype=float) pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos) ax.errorbar(rdist, rss_mean, yerr=rss_var, fmt='ro', markersize='1', ecolor='g', label='Original Data') rdata = np.linspace(np.min(rdist), np.max(rdist), num=1000) rsm_paramtuple_plot = rdata, theta_cap, psi_low, theta_low ax.plot(rdata, rsm_model(rsm_paramtuple_plot, lambda_t[itx], gamma_t[itx]), label='Fitted Curve') ax.legend(loc='upper right') ax.grid() ax.set_ylim([-110, -10]) ax.set_xlabel('Distance [mm]') ax.set_ylabel('RSS [dB]') ax.set_title('RSM for TX# ' + str(itx + 1)) plot_fig5 = False if plot_fig5: fig = plt.figure(5) for itx in analyze_tx: rss_mean = plotdata_mat[:, 3 + itx] rss_var = plotdata_mat[:, 3 + num_tx + itx] rdist = np.array(rdist_temp[itx, :], dtype=float) rss_mean = np.array(rss_mean, dtype=float) rss_var = np.array(rss_var, dtype=float) pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos) rssdata = np.linspace(-10, -110, num=1000) ax.plot(rssdata, lambertloc(rssdata, lambda_t[itx], gamma_t[itx]), label='Fitted Curve') ax.plot(rss_mean, rdist, 'r.') ax.grid() ax.set_xlabel('RSS [dB]') ax.set_ylabel('Distance [mm]') plot_fig6 = False if plot_fig6: fig = plt.figure(6) for itx in analyze_tx: rss_mean = plotdata_mat[:, 3 + itx] rss_var = plotdata_mat[:, 3 + num_tx + itx] rdist = np.array(rdist_temp[itx, :], dtype=float) rss_mean = np.array(rss_mean, dtype=float) rss_var = np.array(rss_var, dtype=float) r_dist_est = lambertloc(rss_mean, lambda_t[itx], gamma_t[itx]) sorted_indices = np.argsort(rdist) r_dist_sort = rdist[sorted_indices] r_dist_est_sort = r_dist_est[sorted_indices] dist_error = r_dist_sort - r_dist_est_sort data_temp = [] bin = np.linspace(0, 2000, 21) ibin = 1 bin_mean = [] bin_var = [] for i in range(len(r_dist_sort)): if r_dist_sort[i] >= bin[-1]: break elif bin[ibin - 1] <= r_dist_sort[i] < bin[ibin]: data_temp.append(dist_error[i]) else: bin_mean_temp = np.mean(data_temp) bin_var_temp = np.var(data_temp) bin_mean.append(bin_mean_temp) bin_var.append(bin_var_temp) # print('bin_high_bound :' + str(bin[ibin]) + ' bin_mean:' + str(bin_mean_temp)) data_temp = [] # reset bin data_temp.append(dist_error[i]) ibin += 1 # print('ibin ' + str(ibin)) pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos) # rssdata = np.linspace(-10, -110, num=1000) # ax.plot(rssdata, lambertloc(rssdata, lambda_t[itx], gamma_t[itx]), label='Fitted Curve') # ax.errorbar(bin[1:-1], bin_mean, yerr=bin_var, fmt='ro', ecolor='g', label='Original Data') ax.plot(bin[1:-1], bin_mean, '.') # print('bin_means = ' + str(bin_mean)) # print('bin_var = ' + str(bin_var)) # ax.plot(r_dist_sort, dist_error, '.') ax.grid() ax.set_xlabel('Distance to tx [mm]') ax.set_ylabel('Error [mm]') plot_fig7 = False ''' Polar Directional Diagrams for Antenna measurements ''' if plot_fig7: fig = plt.figure(7, figsize=(6, 6)) for itx in analyze_tx: pos = 321 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos, projection='polar') rss_max = plotdata_mat[:, 3 + itx].max() rss_max_index = np.where(plotdata_mat[:, 3 + itx] == rss_max) rss_min = plotdata_mat[:, 3 + itx].min() rss_min_index = np.where(plotdata_mat[:, 3 + itx] == rss_min) rss_hpbw = rss_max - 3 itx_2 = rss_max_index[0][0] while plotdata_mat[itx_2, 3 + itx] > rss_hpbw: itx_2 += 1 if itx_2 == totnumwp: itx_2 = 0 else: rss_hpbw_positiveitx_rss = plotdata_mat[itx_2 - 1, 3 + itx] rss_hpbw_positiveitx_rad = plotdata_mat[itx_2 - 1, 2] itx_2 = rss_max_index[0][0] while plotdata_mat[itx_2, 3 + itx] > rss_hpbw: itx_2 -= 1 if itx_2 == -1: itx_2 = totnumwp - 1 else: rss_hpbw_negativeitx_rss = plotdata_mat[itx_2 + 1, 3 + itx] rss_hpbw_negativeitx_rad = plotdata_mat[itx_2 + 1, 2] if abs(rss_hpbw_positiveitx_rss - rss_hpbw_negativeitx_rss) > 0.5: print( '~~~~~> Possible ERROR: HPBW-RSS-measurements are far apart: ' + str( abs(rss_hpbw_positiveitx_rss - rss_hpbw_negativeitx_rss))) print( 'HPBW No1: ' + str( abs(rss_hpbw_positiveitx_rad - rss_hpbw_negativeitx_rad)) + ' rad / ' + str( abs(rss_hpbw_positiveitx_rad - rss_hpbw_negativeitx_rad) * 180 / np.pi) + ' deg') pot_max_2 = 2 * rss_min_index[0][0] - rss_max_index[0][0] if pot_max_2 < 0: pot_max_2 += totnumwp if pot_max_2 >= totnumwp: pot_max_2 -= totnumwp itx_3 = 0 if plotdata_mat[pot_max_2, 3 + itx] < plotdata_mat[pot_max_2 + 1, 3 + itx]: itx_3 += 1 while plotdata_mat[pot_max_2 + itx_3, 3 + itx] < plotdata_mat[pot_max_2 + itx_3 + 1, 3 + itx]: itx_3 += 1 else: itx_3 -= 1 while plotdata_mat[pot_max_2 + itx_3, 3 + itx] < plotdata_mat[pot_max_2 + itx_3 - 1, 3 + itx]: itx_3 -= 1 rss_max_2 = plotdata_mat[pot_max_2 + itx_3, 3 + itx] rss_max_2_index = np.where(plotdata_mat[:, 3 + itx] == rss_max_2) rss_hpbw_2 = rss_max_2 - 3 itx_4 = rss_max_2_index[0][0] while plotdata_mat[itx_4, 3 + itx] > rss_hpbw_2: itx_4 += 1 if itx_4 == totnumwp: itx_4 = 0 else: rss_hpbw_2_positiveitx_rss = plotdata_mat[itx_4 - 1, 3 + itx] rss_hpbw_2_positiveitx_rad = plotdata_mat[itx_4 - 1, 2] itx_4 = rss_max_2_index[0][0] while plotdata_mat[itx_4, 3 + itx] > rss_hpbw_2: itx_4 -= 1 if itx_4 == -1: itx_4 = totnumwp - 1 else: rss_hpbw_2_negativeitx_rss = plotdata_mat[itx_4 + 1, 3 + itx] rss_hpbw_2_negativeitx_rad = plotdata_mat[itx_4 + 1, 2] if abs(rss_hpbw_2_positiveitx_rss - rss_hpbw_2_negativeitx_rss) > 0.5: print( '~~~~~> Possible ERROR: HPBW-RSS-measurements are far apart: ' + str( abs(rss_hpbw_2_positiveitx_rss - rss_hpbw_2_negativeitx_rss))) print( 'HPBW No2: ' + str( abs(rss_hpbw_2_positiveitx_rad - rss_hpbw_2_negativeitx_rad)) + ' rad / ' + str( abs(rss_hpbw_2_positiveitx_rad - rss_hpbw_2_negativeitx_rad) * 180 / np.pi) + ' deg') ax.plot(plotdata_mat[:, 2] + np.pi * 0.0, plotdata_mat[:, 3 + itx], label='Radiation Pattern') ax.set_rmax(rss_max) ax.set_rmin(rss_min) rticks = np.round( np.append(np.linspace(rss_min, rss_max, 5), rss_hpbw), 2) ax.set_rticks(rticks) # or [rss_max, rss_min] ax.set_rlabel_position(65) # ax.set_rticks([rss_hpbw_negativeitx_rss]) # <- alternative # ax.set_rlabel_position(rss_hpbw_negativeitx_rad*180/np.pi) # <- alternative # ax.set_title('Radiation Pattern for TX# ' + str(itx + 1), fontsize=20) plt.show() return lambda_t, gamma_t
def analyze_measdata_from_file(analyze_tx=[1,2,3,4], meantype='db_mean'): """ :param analyze_tx: :param txpos_tuning: :param meantype: :return: """ analyze_tx[:] = [x - 1 for x in analyze_tx] # substract -1 as arrays begin with index 0 measdata_filename = hc_tools.select_file() with open(measdata_filename, 'r') as measfile: load_description = True load_grid_settings = False load_measdata = False meas_data_append_list = [] plotdata_mat_lis = [] for i, line in enumerate(measfile): if line == '### begin grid settings\n': #print('griddata found') load_description = False load_grid_settings = True load_measdata = False continue elif line == '### begin measurement data\n': load_description = False load_grid_settings = False load_measdata = True #print('Measurement data found') continue if load_description: #print('file description') print(line) if load_grid_settings and not load_measdata: #print(line) grid_settings = map(float, line[0:-3].split(',')) x0 = [grid_settings[0], grid_settings[1]] xn = [grid_settings[2], grid_settings[3]] grid_dxdy = [grid_settings[4], grid_settings[5]] timemeas = grid_settings[6] data_shape_file = [int((xn[0]-x0[0]) / grid_dxdy[0] + 1), int((xn[1]-x0[1]) / grid_dxdy[1] + 1)] numtx = int(grid_settings[7]) txdata = grid_settings[8:8+3*numtx] # read tx positions txpos_list = [] for itx in range(numtx): itxpos = txdata[2*itx:2*itx+2] txpos_list.append(itxpos) txpos = np.asarray(txpos_list) # read tx frequencies freqtx_list = [] for itx in range(numtx): freqtx_list.append(txdata[2*numtx+itx]) freqtx = np.asarray(freqtx_list) # print out print('filename = ' + measdata_filename) print('num_of_gridpoints = ' + str(data_shape_file[0]*data_shape_file[1])) print('x0 = ' + str(x0)) print('xn = ' + str(xn)) print('grid_shape = ' + str(data_shape_file)) print('steps_dxdy = ' + str(grid_dxdy)) print('tx_pos = ' + str(txpos_list)) print('freqtx = ' + str(freqtx)) if load_measdata and not load_grid_settings: #print('read measdata') meas_data_line = map(float, line[0:-3].split(', ')) meas_data_append_list.append(meas_data_line) meas_data_mat_line = np.asarray(meas_data_line) num_wp = int(meas_data_mat_line[2]) num_tx = int(meas_data_mat_line[3]) num_meas = int(meas_data_mat_line[4]) # @todo add numtx to data file freq_vec = freqtx first_rss = 5 + num_tx meas_data_mat_rss = meas_data_mat_line[first_rss:] rss_mat = meas_data_mat_rss.reshape([num_tx, num_meas]) if meantype is 'lin': rss_mat_lin = 10**(rss_mat/10) mean_lin = np.mean(rss_mat_lin, axis=1) var_lin = np.var(rss_mat_lin, axis=1) mean = 10 * np.log10(mean_lin) var = 10 * np.log10(var_lin) else: mean = np.mean(rss_mat, axis=1) var = np.var(rss_mat, axis=1) # print('var = ' + str(var)) wp_pos = [meas_data_mat_line[0], meas_data_mat_line[1]] plotdata_line = np.concatenate((wp_pos, mean, var), axis=1) plotdata_mat_lis.append(plotdata_line) measfile.close() totnumwp = num_wp + 1 # counting starts with zero plotdata_mat = np.asarray(plotdata_mat_lis) #print('Number of gridpoints: ' + str(plotdata_mat.shape[0])) """ Model fit """ def rsm_model(dist, alpha, gamma): """Range Sensor Model (RSM) structure.""" return -20 * np.log10(dist) - alpha * dist - gamma # rss in db alpha = [] gamma = [] rdist = [] for itx in analyze_tx: rdist_vec = plotdata_mat[:, 0:2] - txpos[itx, 0:2] # r_wp -r_txpos rdist_temp = np.asarray(np.linalg.norm(rdist_vec, axis=1)) # distance norm: |r_wp -r_txpos| rssdata = plotdata_mat[:, 2+itx] # rss-mean for each wp popt, pcov = curve_fit(rsm_model, rdist_temp, rssdata) del pcov alpha.append(popt[0]) gamma.append(popt[1]) # print('tx #' + str(itx+1) + ' alpha= ' + str(alpha[itx]) + ' gamma= ' + str(gamma[itx])) rdist.append(rdist_temp) rdist_temp = np.reshape(rdist, [num_tx, totnumwp]) print('\nVectors for convenient copy/paste') print('alpha = ' + str(alpha)) print('gamma = ' + str(gamma)) """ Plots """ x = plotdata_mat[:, 0] y = plotdata_mat[:, 1] fig = plt.figure(6) for itx in analyze_tx: pos = 221 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos) rss_mean = plotdata_mat[:, 2 + itx] rss_var = plotdata_mat[:, 2 + num_tx + itx] #data_shape = [16,30] #data_shape = [31, 59]# #print('shape : ' + str(np.shape(x))) #print('shape_from_file ' + str(data_shape_file)) data_shape = [data_shape_file[1], data_shape_file[0]] xx = np.reshape(x, data_shape) yy = np.reshape(y, data_shape) rss = np.reshape(rss_mean, data_shape) val_sequence = np.linspace(-100,-20, 80/5+1) CS = ax.contour(xx, yy, rss, val_sequence) ax.clabel(CS, inline=0, fontsize=10) for itx_plot in analyze_tx: ax.plot(txpos[itx_plot-1, 0], txpos[itx_plot-1, 1], 'or') ax.grid() ax.set_xlabel('x [mm]') ax.set_ylabel('y [mm]') # ax.axis('equal') ax.set_title('RSS field for TX# ' + str(itx + 1)) plot_fig1 = True if plot_fig1: fig = plt.figure(1) for itx in analyze_tx: pos = 221 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos, projection='3d') rss_mean = plotdata_mat[:, 2 + itx] rss_var = plotdata_mat[:, 2 + num_tx + itx] ax.plot_trisurf(x, y, rss_mean, cmap=plt.cm.Spectral) ax.grid() ax.set_xlabel('x [mm]') ax.set_ylabel('y [mm]') ax.set_zlabel('rss [dB]') ax.set_zlim([-110, -20]) ax.set_title('RSS field for TX# ' + str(itx+1)) plot_fig2 = True if plot_fig2: fig = plt.figure(2) for itx in analyze_tx: pos = 221 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos, projection='3d') rss_mean = plotdata_mat[:, 2 + itx] rss_var = plotdata_mat[:, 2 + num_tx + itx] ax.plot_trisurf(x, y, rss_var, cmap=plt.cm.Spectral) ax.grid() ax.set_xlabel('x [mm]') ax.set_ylabel('y [mm]') ax.set_zlabel('rss_var [dB]') ax.set_title('RSS field variance for TX# ' + str(itx + 1)) plot_fig3 = True if plot_fig3: fig = plt.figure(3) for itx in analyze_tx: rss_mean = plotdata_mat[:, 2 + itx] rss_var = plotdata_mat[:, 2 + num_tx + itx] rdist = np.array(rdist_temp[itx, :], dtype=float) rss_mean = np.array(rss_mean, dtype=float) rss_var = np.array(rss_var, dtype=float) pos = 221 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos) ax.errorbar(rdist, rss_mean, yerr=rss_var, fmt='ro', ecolor='g', label='Original Data') rdata = np.linspace(np.min(rdist), np.max(rdist), num=1000) ax.plot(rdata, rsm_model(rdata, alpha[itx], gamma[itx]), label='Fitted Curve') ax.legend(loc='upper right') ax.grid() ax.set_ylim([-110, -10]) ax.set_xlabel('Distance [mm]') ax.set_ylabel('RSS [dB]') ax.set_title('RSM for TX# ' + str(itx + 1)) plot_fig4 = False if plot_fig4: fig = plt.figure(4) for itx in analyze_tx: rss_mean = plotdata_mat[:, 2 + itx] rss_var = plotdata_mat[:, 2 + num_tx + itx] rdist = np.array(rdist_temp[itx, :], dtype=float) rss_mean = np.array(rss_mean, dtype=float) rss_var = np.array(rss_var, dtype=float) pos = 221 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos) rssdata = np.linspace(-10, -110, num=1000) ax.plot(rssdata, lambertloc(rssdata, alpha[itx], gamma[itx]), label='Fitted Curve') ax.plot(rss_mean, rdist, 'r.') ax.grid() ax.set_xlabel('RSS [dB]') ax.set_ylabel('Distance [mm]') plot_fig5 = False if plot_fig5: fig = plt.figure(5) for itx in analyze_tx: rss_mean = plotdata_mat[:, 2 + itx] rss_var = plotdata_mat[:, 2 + num_tx + itx] rdist = np.array(rdist_temp[itx, :], dtype=float) rss_mean = np.array(rss_mean, dtype=float) rss_var = np.array(rss_var, dtype=float) r_dist_est = lambertloc(rss_mean, alpha[itx], gamma[itx]) sorted_indices = np.argsort(rdist) r_dist_sort = rdist[sorted_indices] r_dist_est_sort = r_dist_est[sorted_indices] dist_error = r_dist_sort - r_dist_est_sort data_temp = [] bin = np.linspace(0, 2000, 21) ibin = 1 bin_mean = [] bin_var = [] for i in range(len(r_dist_sort)): if r_dist_sort[i] >= bin[-1]: break elif bin[ibin-1] <= r_dist_sort[i] < bin[ibin]: data_temp.append(dist_error[i]) else: bin_mean_temp = np.mean(data_temp) bin_var_temp = np.var(data_temp) bin_mean.append(bin_mean_temp) bin_var.append(bin_var_temp) #print('bin_high_bound :' + str(bin[ibin]) + ' bin_mean:' + str(bin_mean_temp)) data_temp = [] # reset bin data_temp.append(dist_error[i]) ibin += 1 #print('ibin ' + str(ibin)) pos = 221 + itx if len(analyze_tx) == 1: pos = 111 ax = fig.add_subplot(pos) #rssdata = np.linspace(-10, -110, num=1000) #ax.plot(rssdata, lambertloc(rssdata, alpha[itx], gamma[itx]), label='Fitted Curve') #ax.errorbar(bin[1:-1], bin_mean, yerr=bin_var, fmt='ro', ecolor='g', label='Original Data') ax.plot(bin[1:-1], bin_mean, '.') #print('bin_means = ' + str(bin_mean)) #print('bin_var = ' + str(bin_var)) #ax.plot(r_dist_sort, dist_error, '.') ax.grid() ax.set_xlabel('Distance to tx [mm]') ax.set_ylabel('Error [mm]') plt.show() return alpha, gamma
def read_measfile_header(object, analyze_tx=[1, 2, 3, 4, 5, 6], measfile_path=None): """ function writes data from the measurement header to the EKF object :param: object: existing object of EKF to write values in :param analyze_tx: Number of used tx :param measfile_path: relative path to measfile :return: True """ print('analyze_tx = ') print analyze_tx analyze_tx[:] = [x - 1 for x in analyze_tx ] # substract -1 as arrays begin with index 0 if measfile_path is not None: measdata_filename = measfile_path else: measdata_filename = hc_tools.select_file( functionname='read_measfile_header') if os.stat(str(measdata_filename)).st_size == 0: print('Chosen file is empty.') exit(1) with open(measdata_filename, 'r') as measfile: load_description = True load_grid_settings = False load_measdata = False meas_data_append_list = [] totnumwp = 0 measured_wp_list = [] freqtx = [] txpos_list = [] for i, line in enumerate(measfile): if line == '### begin grid settings\n': # print('griddata found') load_description = False load_grid_settings = True load_measdata = False continue elif line == '### begin measurement data\n': load_description = False load_grid_settings = False load_measdata = True # print('Measurement data found') continue if load_description: # print('file description') print(line) found_dis = True if load_grid_settings and not load_measdata: found_grid = True a = line[:-2].split(' ') grid_settings = map(float, line[:-2].split(' ')) x0 = [grid_settings[0], grid_settings[1], grid_settings[2]] xn = [grid_settings[3], grid_settings[4], grid_settings[5]] grid_dxdyda = [ grid_settings[6], grid_settings[7], grid_settings[8] ] timemeas = grid_settings[9] data_shape_file = [] for i in range(3): # range(num_dof) try: shapei = int((xn[i] - x0[i]) / grid_dxdyda[i] + 1) except ZeroDivisionError: shapei = 1 data_shape_file.append(shapei) print('data shape = ' + str(data_shape_file)) numtx = int(grid_settings[10]) txdata = grid_settings[11:( 11 + 4 * numtx)] # urspruenglich [(2+numtx):(2+numtx+3*numtx)] # read tx positions txpos_list = [] for itx in range(numtx): itxpos = txdata[3 * itx:3 * itx + 3] # urspruenglich [2*itx:2*itx+2] txpos_list.append(itxpos) txpos = np.asarray(txpos_list) # read tx frequencies freqtx_list = [] for itx in range(numtx): freqtx_list.append( txdata[3 * numtx + itx]) # urspruenglich (txdata[2*numtx+itx]) freqtx = np.asarray(freqtx_list) # print out print('filename = ' + measdata_filename) print('num_of_gridpoints = ' + str(data_shape_file[0] * data_shape_file[1])) print('x0 = ' + str(x0)) print('xn = ' + str(xn)) print('grid_shape = ' + str(data_shape_file)) print('steps_dxdyda = ' + str(grid_dxdyda)) print('tx_pos = ' + str(txpos_list)) print('freqtx = ' + str(freqtx)) # startx = x0[0] # endx = xn[0] # stepx = data_shape_file[0] # # starty = x0[1] # endy = xn[1] # stepy = data_shape_file[1] # # startz = x0[2] # endz = xn[2] # stepz = data_shape_file[2] # xpos = np.linspace(startx, endx, stepx) # ypos = np.linspace(starty, endy, stepy) # zpos = np.linspace(startz, endz, stepz) # wp_maty, wp_matz, wp_matx = np.meshgrid(ypos, zpos, xpos) if load_measdata and not load_grid_settings: found_meas = True totnumwp += 1 if found_dis and found_grid and found_meas is not True: print('Not all data found! -> check file') exit(1) ''' write data into object ''' object.set_tx_freq(freqtx) object.set_tx_pos(txpos_list) object.set_tx_num(len(freqtx)) object.set_num_meas(totnumwp) return True
def measurement_simulation(self, cal_param_file=None, covariance_of=False, description=None): """ simulates a measurement -> writes header like real measurements and in measdata rss values with variance :return: """ '''get values from waypoint file''' if self.__way_filename is not None: wplist_filename = path.relpath('Waypoints/' + self.__way_filename + '.txt') else: wplist_filename = hc_tools.select_file( functionname='simulate_field_measurement') if self.__meas_filename is not None: measdata_filename = path.relpath('Simulated_measurements/' + self.__meas_filename + '.txt') else: measdata_filename = hc_tools.save_as_dialog() print measdata_filename if description is None: meas_description = hc_tools.write_descrition() else: meas_description = description print meas_description self.__numtx = len(self.__freqtx) print('freqtx ' + str(self.__freqtx)) print('numtx ' + str(self.__numtx)) print('tx_pos ' + str(self.__tx_pos)) '''get values from waypoint file''' with open(wplist_filename, 'r') as wpfile: load_description = True load_grid_settings = False load_wplist = False wp_append_list = [] for i, line in enumerate(wpfile): if line == '### begin grid settings\n': print('griddata found') load_description = False load_grid_settings = True load_wplist = False continue elif line == '### begin wp_list\n': load_description = False load_grid_settings = False load_wplist = True print('### found') continue if load_description: print('file description') print(line) if load_grid_settings and not load_wplist: grid_settings = map(float, line.split(' ')) x0 = [grid_settings[0], grid_settings[1], grid_settings[2]] xn = [grid_settings[3], grid_settings[4], grid_settings[5]] grid_dxdyda = [ grid_settings[6], grid_settings[7], grid_settings[8] ] timemeas = grid_settings[9] data_shape = [] for i in range(3): # range(num_dof) try: shapei = int((xn[i] - x0[i]) / grid_dxdyda[i] + i) except ZeroDivisionError: shapei = 1 data_shape.append(shapei) if load_wplist and not load_grid_settings: # print('read wplist') wp_append_list.append(map(float, line[:-2].split(' '))) num_wp = len(wp_append_list) wp_data_mat = np.asarray(wp_append_list) # print('wp_data_mat: ' + str(wp_data_mat)) # wp = range(num_wp) # for itx in range(num_wp): # wp[itx] = wp_data_mat[:, 1:4] self.__wp = np.asarray(wp_data_mat[:, 1:4]) '''write values in measurement file''' with open(measdata_filename, 'w') as measfile: # write header to measurement file file_description = ('Measurement simulation file\n' + 'Simulation was performed on ' + t.ctime() + '\n' + 'Description: ' + meas_description + '\n') txdata = str(self.__numtx) + ' ' for itx in range(self.__numtx): txpos = self.__tx_pos[itx] txdata += str(txpos[0]) + ' ' + str(txpos[1]) + ' ' + str( txpos[2]) + ' ' for itx in range(self.__numtx): txdata += str(self.__freqtx[itx]) + ' ' print('txdata = ' + txdata) measfile.write(file_description) measfile.write('### begin grid settings\n') measfile.write( str(x0[0]) + ' ' + str(x0[1]) + ' ' + str(x0[2]) + ' ' + str(xn[0]) + ' ' + str(xn[1]) + ' ' + str(xn[2]) + ' ' + str(grid_dxdyda[0]) + ' ' + str(grid_dxdyda[1]) + ' ' + str(grid_dxdyda[2]) + ' ' + str(timemeas) + ' ' + txdata + str(self.__alpha) + '\n') measfile.write('### begin measurement data\n') print wp_data_mat.shape[0] self.set_cal_params(cal_param_file) self.set_init_values() for i in range(wp_data_mat.shape[0]): wp_pos = wp_data_mat[i][1:4] # needed for error plots for itx in range(self.__numtx): self.rss_value_generator( itx, i) # generates a rss value for certain distance self.measurement_covariance_model(itx, i, covariance_of) measfile.write( str(wp_pos[0]) + ' ' + str(wp_pos[1]) + ' ' + str(wp_pos[2]) + ' ') for itx in range(self.__numtx): measfile.write(str(self.__mean[itx][0]) + ' ') for itx in range(self.__numtx): measfile.write(str(self.__var[itx]) + ' ') for itx in range(self.__numtx): measfile.write(str(self.get_distance(itx, i)) + ' ') measfile.write( '\n') # -> x,y,z,meantx1,...,meantxn,vartx1,...vartxn print('The simulated values are saved in :\n' + str(measdata_filename)) return True