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