diff --git a/src/calibration_complete.py b/src/calibration.py similarity index 69% rename from src/calibration_complete.py rename to src/calibration.py index f67f67d..81a9264 100644 --- a/src/calibration_complete.py +++ b/src/calibration.py @@ -4,14 +4,17 @@ from datetime import datetime from threading import Thread import numpy as np from numpy.lib.scimath import sqrt as csqrt -import scipy.optimize -from scipy import linalg import matplotlib.pyplot as plt +from matplotlib.figure import Figure +from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg +from tkinter import LabelFrame +import scipy.optimize +from scipy import linalg as linalg_scipy from src.utility import ui_print from src.exceptions import DeviceBusy, DeviceAccessError import src.globals as g -#from src.user_interface import CalibrateMagnetometer.mgm_to_helmholtz_cos_trans +# from src.user_interface import CalibrateMagnetometerComplete.mgm_to_helmholtz_cos_trans class AmbientFieldCalibration(Thread): @@ -238,7 +241,171 @@ class CoilConstantCalibration(Thread): self.view_queue.put({'cmd': command, 'arg': arg}) -class MagnetometerCalibration(Thread): +class MagnetometerCalibrationSimple(Thread): + TEST_VECTOR_MAGNITUDE = 100e-6 # In Tesla. Chosen so it can be achieved with a 3A PSU. + + def __init__(self, view_queue, calibration_points, calibration_interval, mgm_to_helmholtz_cos_trans): + Thread.__init__(self) + self.view_queue = view_queue + self.calibration_points = calibration_points + self.calibration_interval = calibration_interval + self.matrix_trans_mgm_to_hh = [[x.get() for x in row] for row in mgm_to_helmholtz_cos_trans] + + # Hardware checks are done in the init method to allow for exception handling in main thread + # This means the run method should/must be called directly after Thread object creation. + + # Make sure we really have magnetometer data + if not g.MAGNETOMETER.connected: + ui_print("\nError: The magnetometer is not connected. Required for ambient field calibration.") + raise DeviceAccessError("The magnetometer is not connected. Required for ambient field calibration.") + + # Acquire cage device. This resource will only be released after the thread is ended. + try: + self.cage_dev = g.CAGE_DEVICE.request_proxy() + except DeviceBusy: + ui_print("\nError: Failed to acquire coil control. Required for ambient field calibration.") + raise DeviceAccessError("Failed to acquire coil control. Required for ambient field calibration.") + + def run(self): + try: + self.calibration_procedure() + self.put_message('finished', None) + except Exception as e: + self.put_message('failed', e) + finally: + self.cage_dev.close() + + def calibration_procedure(self): + # According to method outlined in: + # Zikmund, A. & Janosek, Michal. (2014). Calibration procedure for triaxial magnetometers without a compensating + # system or moving parts. + + # This contains the raw experiment data for exporting + # Each row is a dict containing the applied vector and measured vector + raw_data = [] + + # Find sensor offsets. They must be found prior to applying the chosen calibration algorithm + # This will be accurate if the cage was recently calibrated + self.cage_dev.set_field_compensated([0, 0, 0]) + # Sleep for a certain duration to allow psu to stabilize output and magnetometer to supply readings + time.sleep(self.calibration_interval) + matrix_trans_mgm_to_hh_np = np.array(self.matrix_trans_mgm_to_hh) + # The offsets can easily be read from the magnetometer + offsets = matrix_trans_mgm_to_hh_np.dot(g.MAGNETOMETER.field) + # Save data point to raw_data list + raw_data.append({'applied_x': 0, 'applied_y': 0, 'applied_z': 0, + 'measured_x': offsets[0], 'measured_y': offsets[1], 'measured_z': offsets[2]}) + # Set new progress indicator for UI + self.set_progress(True, 0) + + # Generate our set of test vectors + test_vectors = self.fibonacci_sphere(self.calibration_points) + + # Holds the knowns for each row of our system of equations. These are M, B_x, B_y, B_z + # (B_E is constant for the test and not stored in the array) + # Each sensor axis has its own independent system of equations + samples = [[], [], []] + # Collect sensor data for each test vector + for vec_idx, test_vec in enumerate(test_vectors): + # Command output + applied_vec = test_vec * self.TEST_VECTOR_MAGNITUDE + self.cage_dev.set_field_raw(applied_vec) + + # Sleep for a certain duration to allow psu to stabilize output and magnetometer to supply readings + time.sleep(self.calibration_interval) + + # Read output and save to array for solver later + raw_reading = matrix_trans_mgm_to_hh_np.dot(g.MAGNETOMETER.field) + reading = raw_reading - offsets + for i in range(3): + row = {'m': reading[i], 'b_x': applied_vec[0], 'b_y': applied_vec[1], 'b_z': applied_vec[2]} + # print("[Axis {}] {}".format(i, row)) + samples[i].append(row) + + # Save data point to raw_data list + raw_data.append({'applied_x': applied_vec[0], 'applied_y': applied_vec[1], 'applied_z': applied_vec[2], + 'measured_x': raw_reading[0], 'measured_y': raw_reading[1], 'measured_z': raw_reading[2]}) + + # Set new progress indicator for UI + self.set_progress(True, vec_idx + 1) + + # Put device into an off and ready state + self.cage_dev.idle() + + # Use collected data to build and solve system of equations + sensor_parameters = self.solve_system(samples, offsets) + + # Pass results to UI + self.put_message('calibration_data', {'results': sensor_parameters, 'raw_data': raw_data}) + + def set_progress(self, offset_complete, test_vec_index): + progress = int(offset_complete) * 0.2 + (test_vec_index / self.calibration_points) * 0.8 + self.put_message('progress', progress) + + def solve_system(self, samples, offset_data): + # Calculate magnitude of ambient field + b_e_x = g.CAGE_DEVICE.axes[0].ambient_field + b_e_y = g.CAGE_DEVICE.axes[1].ambient_field + b_e_z = g.CAGE_DEVICE.axes[2].ambient_field + b_e = sqrt(b_e_x**2 + b_e_y**2 + b_e_z**2) + + # Perform least squares optimization on all magnetometer axes + sensor_parameters = [] + for axis, axis_samples in enumerate(samples): + result = scipy.optimize.least_squares(self.residual_function, (1.0, pi/4, pi/4, pi/4), args=(b_e, axis_samples), gtol=1e-13) + s, alpha_e, alpha, beta = result.x + residual = np.max(np.abs(result.fun)) + sensor_parameters.append({'sensitivity': s, + 'offset': offset_data[axis], + 'alpha_e': alpha_e, + 'alpha': alpha, + 'beta': beta, + 'residual': residual}) + + return sensor_parameters + + # Function passed to scipy for the optimization + @staticmethod + def residual_function(x, b_e, samples): + # Unpack vector. These unknown parameters are described in the calibration paper + s, alpha_e, alpha, beta = x + # Residual vector + res = [] + for sample in samples: + # Unpack row coefficients: + m = sample['m'] + b_x = sample['b_x'] + b_y = sample['b_y'] + b_z = sample['b_z'] + res.append(m - s * (b_e*sin(alpha_e) + b_x*cos(alpha)*cos(beta) + b_y*cos(alpha)*sin(beta) + b_z*sin(alpha))) + return res + + @staticmethod + def fibonacci_sphere(samples): + """ + Algorithm to generate roughly equally spaced points on a sphere + From https://stackoverflow.com/a/26127012""" + points = [] + phi = pi * (3.0 - sqrt(5.0)) # golden angle in radians + + for i in range(samples): + y = 1 - (i / float(samples - 1)) * 2 # y goes from 1 to -1 + radius = sqrt(1 - y * y) # radius at y + + theta = phi * i # golden angle increment + + x = cos(theta) * radius + z = sin(theta) * radius + + points.append(np.array([x, y, z])) + + return points + + def put_message(self, command, arg): + self.view_queue.put({'cmd': command, 'arg': arg}) + + +class MagnetometerCalibrationComplete(Thread): TEST_VECTOR_MAGNITUDE = 100e-6 # In Tesla. Chosen so it can be achieved with a 3A PSU. def __init__(self, view_queue, calibration_points, calibration_interval, mgm_to_helmholtz_cos_trans): @@ -330,7 +497,7 @@ class MagnetometerCalibration(Thread): self.cage_dev.idle() # Use collected data to build and solve system of equations - sensor_parameters = self.solve_system(samples, raw_data) + sensor_parameters = self.solve_system(raw_data) # FLAG: compare to csv import # Pass results to UI self.put_message('calibration_data', {'results': sensor_parameters, 'raw_data': raw_data}) @@ -339,7 +506,7 @@ class MagnetometerCalibration(Thread): progress = int(offset_complete) * 0.2 + (test_vec_index / self.calibration_points) * 0.8 self.put_message('progress', progress) - def solve_system(self, raw_data, offsets): + def solve_system(self, raw_data, matrix_trans_mgm_to_hh_tk): u_tesla = 10 ** 6 # Unpack data: mag_x_set = np.zeros(len(raw_data)) @@ -355,6 +522,23 @@ class MagnetometerCalibration(Thread): mag_x_m[i] = raw_data[i]['measured_x'] mag_y_m[i] = raw_data[i]['measured_y'] mag_z_m[i] = raw_data[i]['measured_z'] + + # Apply coordinate transformation from Helmholtz system to MGM system + matrix_trans_mgm_to_hh_np = np.zeros((3, 3)) + for row in range(3): + for col in range(3): + matrix_trans_mgm_to_hh_np[row][col] = matrix_trans_mgm_to_hh_tk[row][col].get() + # matrix_trans_mgm_to_hh_np = [[-1, 0, 0], [0, 1, 0], [0, 0, -1]] # FLAG: Hardcoded! for MGM 1 / 3 + # matrix_trans_mgm_to_hh_np = [[0, 1, 0], [-1, 0, 0], [0, 0, 1]] # FLAG: Hardcoded! for MGM 0 / 2 + ui_print("Applying transformation matrix to data. h_{set,mgm} = mgm_T_hh * h_{set,hh}") + ui_print(matrix_trans_mgm_to_hh_np) + # Transform hh set magnetic field cos to sensor cos + mag_xyz_set = np.c_[mag_x_set, mag_y_set, mag_z_set] + mag_xyz_set_hh = np.matmul(matrix_trans_mgm_to_hh_np, mag_xyz_set.T).T + mag_x_set = mag_xyz_set_hh[:, 0] + mag_y_set = mag_xyz_set_hh[:, 1] + mag_z_set = mag_xyz_set_hh[:, 2] + # FLAG: Currently the coordinate transformation is applied two times (see calibration class) # Calculate total error err_x = 0 err_y = 0 @@ -364,15 +548,15 @@ class MagnetometerCalibration(Thread): err_y += (mag_y_m[i] - mag_y_set[i]) ** 2 err_z += (mag_z_m[i] - mag_z_set[i]) ** 2 err_t = np.sqrt(err_x ** 2 + err_y ** 2 + err_z ** 2) - self.put_message('Error in X/Y/Z/total: {:.2e} / {:.2e} / {:.2e} / {:.2e} [uT]'.format(err_x * u_tesla, err_y * u_tesla, - err_z * u_tesla, - err_t * u_tesla), None) + ui_print('Error in X/Y/Z/total: {:.2e} / {:.2e} / {:.2e} / {:.2e} [uT]'.format(err_x * u_tesla, err_y * u_tesla, + err_z * u_tesla, err_t * u_tesla)) + # Filter raw data try: mag_x_set, mag_y_set, mag_z_set, mag_x_m, mag_y_m, mag_z_m = \ - MagnetometerCalibration.filter_magnetometer_data(self, mag_x_set, mag_y_set, mag_z_set, mag_x_m, mag_y_m, mag_z_m) + MagnetometerCalibrationComplete.filter_magnetometer_data(self, mag_x_set, mag_y_set, mag_z_set, mag_x_m, mag_y_m, mag_z_m) except Warning as waring_filter_mgm_data: - self.put_message(f"{waring_filter_mgm_data}", None) + ui_print(f"{waring_filter_mgm_data}") # Get general magnitude of the set magnetic filed mag_amp_set = np.sqrt( mag_x_set[1:len(mag_x_set)] ** 2 + mag_y_set[1:len(mag_x_set)] ** 2 + mag_z_set[1:len(mag_x_set)] ** 2) @@ -383,13 +567,15 @@ class MagnetometerCalibration(Thread): # Calculate ellipsoid fit try: - q_mat, n, d = MagnetometerCalibration.fit_ellipsoid(self, mag_x_m, mag_y_m, mag_z_m) - self.put_message('Q matrix', q_mat) - self.put_message('n',n) + q_mat, n, d = MagnetometerCalibrationComplete.fit_ellipsoid(self, mag_x_m, mag_y_m, mag_z_m) + ui_print('Q matrix =') + ui_print(q_mat) + ui_print('n =') + ui_print(n) # Retrieve calibration parameters q_mat_inv = np.linalg.inv(q_mat) b = -np.dot(q_mat_inv, n) - a_mat_inv = np.real(1 / csqrt(np.dot(n.T, np.dot(q_mat_inv, n)) - d) * linalg.sqrtm(q_mat)) + a_mat_inv = np.real(1 / csqrt(np.dot(n.T, np.dot(q_mat_inv, n)) - d) * linalg_scipy.sqrtm(q_mat)) a_mat = np.linalg.inv(a_mat_inv) # Calculate error cal_x = np.zeros(mag_x_m.shape) @@ -408,27 +594,29 @@ class MagnetometerCalibration(Thread): # Scale unity solution back to original scaling a_mat_inv = a_mat_inv * mag_amp_avg_set # Console output - self.put_message("Average magnitude: mag_amp_avg_set = {:.4f} uT, mag_amp_avg_m = {:.4f} uT".format( - mag_amp_avg_set * 10 ** 6, mag_amp_avg_m * 10 ** 6), None) - self.put_message('Soft-iron correction matrix a_mat_inv = ', a_mat_inv) - self.put_message('Hard-iron offset b =', b) - self.put_message('Total error E = ', total_error) - MagnetometerCalibration.plot_magnetometer_calibration(self, mag_x_set, mag_y_set, mag_z_set, mag_x_m, mag_y_m, mag_z_m, cal_x, cal_y, - cal_z, mag_amp_avg_set) - sensor_parameters = [] - sensor_parameters.append({'a_mat': a_mat.tolist(), - 'a_mat_inv': a_mat_inv.tolist(), - 'b': b.tolist(), - 'q_mat': q_mat.tolist(), - 'n': n.tolist(), - 'mag_amp_avg_set': mag_amp_avg_set.tolist(), - 'total_error': total_error.tolist()}) - return sensor_parameters - except: - self.put_message('A_inv could not be calculated! An unknown error occurred.', None) - self.put_message('Please check if transformation matrix is input correctly.', None) - - + ui_print("Average magnitude: mag_amp_avg_set = {:.4f} uT, mag_amp_avg_m = {:.4f} uT".format( + mag_amp_avg_set * 10 ** 6, mag_amp_avg_m * 10 ** 6)) + ui_print('Soft-iron correction matrix a_mat_inv = ') + ui_print(a_mat_inv) + ui_print('Hard-iron offset b =') + ui_print(b) + ui_print('Total error E = ', total_error) + sensor_parameters = [{'a_mat': a_mat.tolist(), + 'a_mat_inv': a_mat_inv.tolist(), + 'b': b.tolist(), + 'q_mat': q_mat.tolist(), + 'n': n.tolist(), + 'mag_amp_avg_set': mag_amp_avg_set.tolist(), + 'total_error': total_error.tolist()}] + return sensor_parameters, mag_x_set, mag_y_set, mag_z_set, mag_x_m, mag_y_m, mag_z_m, cal_x, cal_y, cal_z, mag_amp_avg_set + except Warning as warning_message: + ui_print('A_inv could not be calculated! A warning occurred.') + ui_print('Please check if transformation matrix is input correctly.') + ui_print(f"{warning_message}") + except Exception as exception_message: + ui_print('A_inv could not be calculated! An unknown error occurred.') + ui_print('Please check if transformation matrix is input correctly.') + ui_print(f"{exception_message}") def filter_magnetometer_data(self, mag_x_set, mag_y_set, mag_z_set, mag_x_m, mag_y_m, mag_z_m): flag_validity = np.ones(len(mag_x_set)) @@ -474,9 +662,9 @@ class MagnetometerCalibration(Thread): if np.abs(mag_x_set[i] - mag_x_m[i]) > thresh or np.abs(mag_y_set[i] - mag_y_m[i]) > thresh or np.abs( mag_z_set[i] - mag_z_m[i]) > thresh: flag_validity[i] = 0 - # self.put_message("Ix {} X_set: {:.2e}, X_m: {:.2e}, Y_set: {:.2e}, Y_m: {:.2e}, Z_set: {:.2e}, Z_m: {:.2e}" + # ui_print("Ix {} X_set: {:.2e}, X_m: {:.2e}, Y_set: {:.2e}, Y_m: {:.2e}, Z_set: {:.2e}, Z_m: {:.2e}" # .format(i, mag_x_set[i], mag_x_m[i], mag_y_set[i], mag_y_m[i], mag_z_set[i], mag_z_m[i])) - # self.put_message("Ix {} X_set-X_m: {:.2e}, Y_set-Y_m: {:.2e}, Z_set-Z_m: {:.2e}" + # ui_print("Ix {} X_set-X_m: {:.2e}, Y_set-Y_m: {:.2e}, Z_set-Z_m: {:.2e}" # .format(i, np.abs(mag_x_set[i]-mag_x_m[i]), np.abs(mag_y_set[i]-mag_y_m[i]), np.abs(mag_z_set[i]-mag_z_m[i]))) # Issue: first element is zero-field measurement and always zero -> might lead to issues during calculation flag_validity[0] = 0 @@ -488,7 +676,7 @@ class MagnetometerCalibration(Thread): if flag_validity[i] == 0: ix_del[j] = int(i - 0) j = j + 1 - self.put_message('{} flagged measurements with indices {} removed.'.format(len(ix_del), ix_del), None) + ui_print('{} flagged measurements with indices {} removed.'.format(len(ix_del), ix_del)) mag_x_set = np.delete(mag_x_set, ix_del.astype(int), 0) mag_y_set = np.delete(mag_y_set, ix_del.astype(int), 0) @@ -558,17 +746,27 @@ class MagnetometerCalibration(Thread): return q_mat, n, d def plot_magnetometer_calibration(self, mag_x_set, mag_y_set, mag_z_set, mag_x_m, mag_y_m, mag_z_m, cal_x, cal_y, - cal_z, mag_amp_avg): + cal_z, mag_amp_avg_set): + plot_fontsize = 5 + + # Plot frame (overwrite plotframe) + plot_frame = LabelFrame(self.right_column, text="Result plots:") + plot_frame.grid(row=1, column=0, padx=(100, 0), pady=20, sticky="nw") + # Plot calibrated results - fig1 = plt.figure(1) + fig1 = plt.figure('MGM_cal_complete_left', figsize=(2.5, 3), dpi=100) + fig1.clf() # clear figure from previous use + canvas1 = FigureCanvasTkAgg(fig1, plot_frame) u_tesla = 10 ** 6 # Tesla to microTesla conversion factor meas_no = len(mag_x_set) # Measurement number # uncalibrated result plot - ax1 = fig1.add_subplot(121, projection='3d') - ax1.set_xlabel(r'$B_x [{\mu}T]$') - ax1.set_ylabel(r'$B_y [{\mu}T]$') - ax1.set_zlabel(r'$B_z [{\mu}T]$') + ax1 = fig1.add_subplot(211, projection='3d') + # ax1.clf() + ax1.set_xlabel(r'$B_x [{\mu}T]$', fontsize=plot_fontsize) + ax1.set_ylabel(r'$B_y [{\mu}T]$', fontsize=plot_fontsize) + ax1.set_zlabel(r'$B_z [{\mu}T]$', fontsize=plot_fontsize) + ax1.tick_params(axis='both', labelsize=plot_fontsize) # linking lines between measured and calibrated points for i, j, k, l, m, n in zip(mag_x_set * u_tesla, mag_y_set * u_tesla, mag_z_set * u_tesla, mag_x_m * u_tesla, mag_y_m * u_tesla, @@ -583,18 +781,19 @@ class MagnetometerCalibration(Thread): # plot sphere with magnitude u = np.linspace(0, 2 * np.pi, 100) v = np.linspace(0, np.pi, 100) - x = np.outer(np.cos(u), np.sin(v)) * mag_amp_avg - y = np.outer(np.sin(u), np.sin(v)) * mag_amp_avg - z = np.outer(np.ones(np.size(u)), np.cos(v)) * mag_amp_avg + x = np.outer(np.cos(u), np.sin(v)) * mag_amp_avg_set + y = np.outer(np.sin(u), np.sin(v)) * mag_amp_avg_set + z = np.outer(np.ones(np.size(u)), np.cos(v)) * mag_amp_avg_set ax1.plot_wireframe(x * u_tesla, y * u_tesla, z * u_tesla, rstride=10, cstride=10, alpha=0.7, color='y') ax1.plot_surface(x * u_tesla, y * u_tesla, z * u_tesla, alpha=0.3, color='y') - ax1.legend(loc='upper right') + ax1.legend(loc='upper right', fontsize=plot_fontsize) # Calibrated result plot - ax2 = fig1.add_subplot(122, projection='3d') - ax2.set_xlabel(r'$B_x [\mu T]$') - ax2.set_ylabel(r'$B_y [\mu T]$') - ax2.set_zlabel(r'$B_z [\mu T]$') + ax2 = fig1.add_subplot(212, projection='3d') + ax2.set_xlabel(r'$B_x [\mu T]$', fontsize=plot_fontsize) + ax2.set_ylabel(r'$B_y [\mu T]$', fontsize=plot_fontsize) + ax2.set_zlabel(r'$B_z [\mu T]$', fontsize=plot_fontsize) + ax2.tick_params(axis='both', labelsize=plot_fontsize) # linking lines between measured and calibrated points for i, j, k, l, m, n in zip(mag_x_set * u_tesla, mag_y_set * u_tesla, mag_z_set * u_tesla, cal_x * u_tesla, cal_y * u_tesla, @@ -609,69 +808,57 @@ class MagnetometerCalibration(Thread): # plot sphere with magnitude u = np.linspace(0, 2 * np.pi, 100) v = np.linspace(0, np.pi, 100) - x = np.outer(np.cos(u), np.sin(v)) * mag_amp_avg - y = np.outer(np.sin(u), np.sin(v)) * mag_amp_avg - z = np.outer(np.ones(np.size(u)), np.cos(v)) * mag_amp_avg + x = np.outer(np.cos(u), np.sin(v)) * mag_amp_avg_set + y = np.outer(np.sin(u), np.sin(v)) * mag_amp_avg_set + z = np.outer(np.ones(np.size(u)), np.cos(v)) * mag_amp_avg_set ax2.plot_wireframe(x * u_tesla, y * u_tesla, z * u_tesla, rstride=10, cstride=10, alpha=0.7, color='y') ax2.plot_surface(x * u_tesla, y * u_tesla, z * u_tesla, alpha=0.3, color='y') - ax2.legend(loc='upper right') + ax2.legend(loc='upper right', fontsize=plot_fontsize) + canvas1.draw() + canvas1.get_tk_widget().grid(row=0, column=0) # 2d_math plots - fig2 = plt.figure(2) + fig2 = plt.figure('MGM_cal_complete_right',figsize=(4, 3), dpi=100) + fig2.clf() # clear figure from previous use + canvas2 = FigureCanvasTkAgg(fig2, plot_frame) # x panel ax3 = fig2.add_subplot(311) - ax3.set_ylabel(r'$B_x [\mu T]$') + ax3.set_ylabel(r'$B_x [\mu T]$', fontsize=plot_fontsize) ax3.plot(np.linspace(1, meas_no, meas_no), mag_x_set * u_tesla, linewidth=0.5, color='k', linestyle='solid', label=r'$B_{x,set}$') ax3.plot(np.linspace(1, meas_no, meas_no), mag_x_m * u_tesla, linewidth=0.5, color='b', linestyle='solid', label=r'$B_{x,m}$') ax3.plot(np.linspace(1, meas_no, meas_no), cal_x * u_tesla, linewidth=0.5, color='r', linestyle='solid', label=r'$B_{x,el}$') - ax3.legend(loc='upper right') + ax3.legend(loc='upper right', fontsize=plot_fontsize) + ax3.tick_params(axis='both', labelsize=plot_fontsize) + ax3.axes.get_xaxis().set_visible(False) # y panel ax4 = fig2.add_subplot(312) - ax4.set_ylabel(r'$B_y [\mu T]$') + ax4.set_ylabel(r'$B_y [\mu T]$', fontsize=plot_fontsize) ax4.plot(np.linspace(1, meas_no, meas_no), mag_y_set * u_tesla, linewidth=0.5, color='k', linestyle='solid', label=r'$B_{y,set}$') ax4.plot(np.linspace(1, meas_no, meas_no), mag_y_m * u_tesla, linewidth=0.5, color='b', linestyle='solid', label=r'$B_{y,m}$') ax4.plot(np.linspace(1, meas_no, meas_no), cal_y * u_tesla, linewidth=0.5, color='r', linestyle='solid', label=r'$B_{y,el}$') - ax4.legend(loc='upper right') + ax4.legend(loc='upper right', fontsize=plot_fontsize) + ax4.tick_params(axis='both', labelsize=plot_fontsize) + ax4.axes.get_xaxis().set_visible(False) + # z panel ax5 = fig2.add_subplot(313) - ax5.set_xlabel("Measurement number") - ax5.set_ylabel(r'$B_z [\mu T]$') + ax5.set_xlabel("Measurement number", fontsize=plot_fontsize) + ax5.set_ylabel(r'$B_z [\mu T]$', fontsize=plot_fontsize) ax5.plot(np.linspace(1, meas_no, meas_no), mag_z_set * u_tesla, linewidth=0.5, color='k', linestyle='solid', label=r'$B_{z,set}$') ax5.plot(np.linspace(1, meas_no, meas_no), mag_z_m * u_tesla, linewidth=0.5, color='b', linestyle='solid', label=r'$B_{z,m}$') ax5.plot(np.linspace(1, meas_no, meas_no), cal_z * u_tesla, linewidth=0.5, color='r', linestyle='solid', label=r'$B_{z,el}$') - ax5.legend(loc='upper right') - - plt.show() - - def solve_system_old(self, samples, offset_data): - # Calculate magnitude of ambient field - b_e_x = g.CAGE_DEVICE.axes[0].ambient_field - b_e_y = g.CAGE_DEVICE.axes[1].ambient_field - b_e_z = g.CAGE_DEVICE.axes[2].ambient_field - b_e = sqrt(b_e_x**2 + b_e_y**2 + b_e_z**2) - - # Perform least squares optimization on all magnetometer axes - sensor_parameters = [] - for axis, axis_samples in enumerate(samples): - result = scipy.optimize.least_squares(self.residual_function, (1.0, pi/4, pi/4, pi/4), args=(b_e, axis_samples), gtol=1e-13) - s, alpha_e, alpha, beta = result.x - residual = np.max(np.abs(result.fun)) - sensor_parameters.append({'sensitivity': s, - 'offset': offset_data[axis], - 'alpha_e': alpha_e, - 'alpha': alpha, - 'beta': beta, - 'residual': residual}) - - return sensor_parameters + ax5.legend(loc='upper right', fontsize=plot_fontsize) + ax5.tick_params(axis='both', labelsize=plot_fontsize) + canvas2.draw() + canvas2.get_tk_widget().grid(row=0, column=1) # Function passed to scipy for the optimization @staticmethod diff --git a/src/calibration_simple.py b/src/calibration_simple.py deleted file mode 100644 index 84f88ac..0000000 --- a/src/calibration_simple.py +++ /dev/null @@ -1,399 +0,0 @@ -from math import pi, sqrt, sin, cos -import time -from datetime import datetime -from threading import Thread -import numpy as np -import scipy.optimize - -from src.utility import ui_print -from src.exceptions import DeviceBusy, DeviceAccessError -import src.globals as g -#from src.user_interface import CalibrateMagnetometer.mgm_to_helmholtz_cos_trans - - -class AmbientFieldCalibration(Thread): - """Varies the coil-generated fields until a configuration is reached which zeros the connected magnetometer. - The magnetometer does not need to be centered. The axes of the magnetometer must match the coil configuration!""" - - # Timeout/settling time for the calibration procedure. An acceptable duration for the PI is required - SETTLE_TIME = 45 - # PID controller time delta - TIME_DELTA = 0.5 - P_CONTROL = -7e3 # 0.2 A/s slew-rate at 40uT - I_CONTROL = 0 # -1e4 # 0.01A/s slew-rate for 1uTs - I_LIMIT = 1e-7 # uTs, Limit I to 0.025 A/s slew-rate to prevent wind-up - # D_CONTROL = Not implemented for now - - def __init__(self, view_queue): - Thread.__init__(self) - self.view_queue = view_queue - - # Axis currents. Incremented by PID loop - self.axis_currents = np.array([0, 0, 0], dtype=float) - # Used for I control - self.error_integral = np.array([0, 0, 0], dtype=float) - - # Hardware checks are done in the init method to allow for exception handling in main thread - # This means the run method should/must be called directly after Thread object creation. - - # Make sure we really have magnetometer data - if not g.MAGNETOMETER.connected: - ui_print("\nError: The magnetometer is not connected. Required for ambient field calibration.") - raise DeviceAccessError("The magnetometer is not connected. Required for ambient field calibration.") - - # Acquire cage device. This resource will only be released after the thread is ended. - try: - self.cage_dev = g.CAGE_DEVICE.request_proxy() - except DeviceBusy: - ui_print("\nError: Failed to acquire coil control. Required for ambient field calibration.") - raise DeviceAccessError("Failed to acquire coil control. Required for ambient field calibration.") - - def run(self): - try: - self.calibration_procedure() - self.put_message('finished', None) - except Exception as e: - self.put_message('failed', e) - finally: - self.cage_dev.close() - - def calibration_procedure(self): - raw_experiment_data = [] - - start_time = datetime.now() - target_time = 0 - current_time = datetime.now() - while (current_time - start_time).seconds < self.SETTLE_TIME: - # Each axis runs its own PID controller. They are slightly coupled by unorthogonality, which should - # hopefully not destabilize the feedback loop - for i in range(3): - # Error in tesla - dt = self.TIME_DELTA - e = g.MAGNETOMETER.field[i] - # Change in control current - du = e * self.P_CONTROL + self.error_integral[i] * self.I_CONTROL - self.axis_currents[i] += du*dt - - # Update integral - # Add increment - self.error_integral[i] += e*dt - # Clamp range - self.error_integral = np.clip(self.error_integral, -self.I_LIMIT, self.I_LIMIT) - - # Apply new field actuation - self.cage_dev.set_signed_currents(self.axis_currents) - - # Set new progress indicator for UI - self.put_message('progress', (current_time - start_time).seconds / self.SETTLE_TIME) - - # Sleep until next iteration - target_time += self.TIME_DELTA - sleep_time = ((start_time - current_time).total_seconds() + target_time) - if sleep_time > 0: - time.sleep(sleep_time) - current_time = datetime.now() - - coil_constants = np.array([g.CAGE_DEVICE.axes[i].coil_const for i in range(3)]) - for i, axis in zip(range(3), ['x', 'y', 'z']): - raw_experiment_data.append({'axis': axis, - 'cancellation_current': -self.axis_currents[i], - 'ambient_field': -self.axis_currents[i] * coil_constants[i], - 'residual_field': g.MAGNETOMETER.field[i]}) - - results = {'ambient': -self.axis_currents, - 'ambient_ut': -self.axis_currents * coil_constants * 1e6, - 'residual': g.MAGNETOMETER.field, - 'raw_data': raw_experiment_data} - self.put_message('ambient_data', results) - - # Put device into an off and ready state - self.cage_dev.idle() - - def put_message(self, command, arg): - self.view_queue.put({'cmd': command, 'arg': arg}) - - -class CoilConstantCalibration(Thread): - MEASUREMENT_RANGE = 3 # A. Will extend into negative and positive sign - MEASUREMENT_POINTS = 4 # Excludes zero. eg 0.5, 1, 1.5, 2 - SETTLE_TIME = 3 # Time until new measurement is ready after setting current - - def __init__(self, view_queue): - Thread.__init__(self) - self.view_queue = view_queue - - # Hardware checks are done in the init method to allow for exception handling in main thread - # This means the run method should/must be called directly after Thread object creation. - - # Make sure we really have magnetometer data - if not g.MAGNETOMETER.connected: - ui_print("\nError: The magnetometer is not connected. Required for ambient field calibration.") - raise DeviceAccessError("The magnetometer is not connected. Required for ambient field calibration.") - - # Acquire cage device. This resource will only be released after the thread is ended. - try: - self.cage_dev = g.CAGE_DEVICE.request_proxy() - except DeviceBusy: - ui_print("\nError: Failed to acquire coil control. Required for ambient field calibration.") - raise DeviceAccessError("Failed to acquire coil control. Required for ambient field calibration.") - - def run(self): - try: - self.calibration_procedure() - self.put_message('finished', None) - except Exception as e: - self.put_message('failed', e) - finally: - self.cage_dev.close() - - def calibration_procedure(self): - # All generated fields will be compared to this using a simple difference method - ambient_field = g.MAGNETOMETER.field - - # This generates linearly spaced current setpoints and excludes zero - currents = np.linspace(-self.MEASUREMENT_RANGE, self.MEASUREMENT_RANGE, self.MEASUREMENT_POINTS * 2 + 1) - currents = np.delete(currents, self.MEASUREMENT_POINTS) - - # Stores three vectors that correspond to the x,y,z actuation field directions. - axis_field_directions = [] - - # Result variables - coil_constants = np.zeros(3) - k_deviations = np.zeros(3) - raw_experiment_data = [] - - # The coil constant must be determined for every axis - for i in range(3): - k_samples = [] - for c_idx, c in enumerate(currents): - # Set current - c_vec = [0, 0, 0] - c_vec[i] = c - self.cage_dev.set_signed_currents(c_vec) - time.sleep(self.SETTLE_TIME) - - # Get coil constant - field = g.MAGNETOMETER.field - field_diff_mag = np.linalg.norm(field - ambient_field) - sign = 1 if field[i] - ambient_field[i] >= 0 else -1 - k = (field_diff_mag * sign) / c - k_samples.append(k) - - # Save vector as principal coil direction if it is the last sample with the largest positive current - if c_idx == currents.shape[0] - 1: - axis_field_directions.append(g.MAGNETOMETER.field - ambient_field) - - # Set new progress indicator for UI - self.put_message('progress', ((c_idx / (self.MEASUREMENT_POINTS * 2)) + i) / 3) - - # Save into raw data vector - raw_experiment_data.append({'axis': i, - 'I_x': c_vec[0], - 'I_y': c_vec[1], - 'I_z': c_vec[2], - 'delta_mag_B': field_diff_mag, - 'sign': sign, - 'K': k}) - - # Average samples for axis - coil_constants[i] = np.average(k_samples) - k_deviations[i], _ = self.calculate_standard_deviation(k_samples) - - # Put device into an off and ready state - self.cage_dev.idle() - - angles = {'xy': self.angle_between(axis_field_directions[0], axis_field_directions[1]) * 180 / pi, - 'yz': self.angle_between(axis_field_directions[1], axis_field_directions[2]) * 180 / pi, - 'xz': self.angle_between(axis_field_directions[0], axis_field_directions[2]) * 180 / pi} - self.put_message('coil_constant_results', {'k': coil_constants, - 'k_dev': k_deviations, - 'angle': angles, - 'raw_data': raw_experiment_data}) - - @staticmethod - def calculate_standard_deviation(data): - n = len(data) - average = 0 - for datapoint in data: - average += datapoint / n - std_dev = 0 - deviations = [] - for datapoint in data: - std_dev += (datapoint - average) ** 2 - deviations.append(datapoint - average) - std_dev = sqrt(std_dev / n) - return std_dev, deviations - - @staticmethod - def angle_between(v1, v2): - """ Returns the angle in radians between vectors 'v1' and 'v2'""" - v1_u = v1 / np.linalg.norm(v1) - v2_u = v2 / np.linalg.norm(v2) - return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0)) - - def put_message(self, command, arg): - self.view_queue.put({'cmd': command, 'arg': arg}) - - -class MagnetometerCalibration(Thread): - TEST_VECTOR_MAGNITUDE = 100e-6 # In Tesla. Chosen so it can be achieved with a 3A PSU. - - def __init__(self, view_queue, calibration_points, calibration_interval, mgm_to_helmholtz_cos_trans): - Thread.__init__(self) - self.view_queue = view_queue - self.calibration_points = calibration_points - self.calibration_interval = calibration_interval - self.matrix_trans_mgm_to_hh = [[x.get() for x in row] for row in mgm_to_helmholtz_cos_trans] - - # Hardware checks are done in the init method to allow for exception handling in main thread - # This means the run method should/must be called directly after Thread object creation. - - # Make sure we really have magnetometer data - if not g.MAGNETOMETER.connected: - ui_print("\nError: The magnetometer is not connected. Required for ambient field calibration.") - raise DeviceAccessError("The magnetometer is not connected. Required for ambient field calibration.") - - # Acquire cage device. This resource will only be released after the thread is ended. - try: - self.cage_dev = g.CAGE_DEVICE.request_proxy() - except DeviceBusy: - ui_print("\nError: Failed to acquire coil control. Required for ambient field calibration.") - raise DeviceAccessError("Failed to acquire coil control. Required for ambient field calibration.") - - def run(self): - try: - self.calibration_procedure() - self.put_message('finished', None) - except Exception as e: - self.put_message('failed', e) - finally: - self.cage_dev.close() - - def calibration_procedure(self): - # According to method outlined in: - # Zikmund, A. & Janosek, Michal. (2014). Calibration procedure for triaxial magnetometers without a compensating - # system or moving parts. - - # This contains the raw experiment data for exporting - # Each row is a dict containing the applied vector and measured vector - raw_data = [] - - # Find sensor offsets. They must be found prior to applying the chosen calibration algorithm - # This will be accurate if the cage was recently calibrated - self.cage_dev.set_field_compensated([0, 0, 0]) - # Sleep for a certain duration to allow psu to stabilize output and magnetometer to supply readings - time.sleep(self.calibration_interval) - matrix_trans_mgm_to_hh_np = np.array(self.matrix_trans_mgm_to_hh) - # The offsets can easily be read from the magnetometer - offsets = matrix_trans_mgm_to_hh_np.dot(g.MAGNETOMETER.field) - # Save data point to raw_data list - raw_data.append({'applied_x': 0, 'applied_y': 0, 'applied_z': 0, - 'measured_x': offsets[0], 'measured_y': offsets[1], 'measured_z': offsets[2]}) - # Set new progress indicator for UI - self.set_progress(True, 0) - - # Generate our set of test vectors - test_vectors = self.fibonacci_sphere(self.calibration_points) - - # Holds the knowns for each row of our system of equations. These are M, B_x, B_y, B_z - # (B_E is constant for the test and not stored in the array) - # Each sensor axis has its own independent system of equations - samples = [[], [], []] - # Collect sensor data for each test vector - for vec_idx, test_vec in enumerate(test_vectors): - # Command output - applied_vec = test_vec * self.TEST_VECTOR_MAGNITUDE - self.cage_dev.set_field_raw(applied_vec) - - # Sleep for a certain duration to allow psu to stabilize output and magnetometer to supply readings - time.sleep(self.calibration_interval) - - # Read output and save to array for solver later - raw_reading = matrix_trans_mgm_to_hh_np.dot(g.MAGNETOMETER.field) - reading = raw_reading - offsets - for i in range(3): - row = {'m': reading[i], 'b_x': applied_vec[0], 'b_y': applied_vec[1], 'b_z': applied_vec[2]} - # print("[Axis {}] {}".format(i, row)) - samples[i].append(row) - - # Save data point to raw_data list - raw_data.append({'applied_x': applied_vec[0], 'applied_y': applied_vec[1], 'applied_z': applied_vec[2], - 'measured_x': raw_reading[0], 'measured_y': raw_reading[1], 'measured_z': raw_reading[2]}) - - # Set new progress indicator for UI - self.set_progress(True, vec_idx + 1) - - # Put device into an off and ready state - self.cage_dev.idle() - - # Use collected data to build and solve system of equations - sensor_parameters = self.solve_system(samples, offsets) - - # Pass results to UI - self.put_message('calibration_data', {'results': sensor_parameters, 'raw_data': raw_data}) - - def set_progress(self, offset_complete, test_vec_index): - progress = int(offset_complete) * 0.2 + (test_vec_index / self.calibration_points) * 0.8 - self.put_message('progress', progress) - - def solve_system(self, samples, offset_data): - # Calculate magnitude of ambient field - b_e_x = g.CAGE_DEVICE.axes[0].ambient_field - b_e_y = g.CAGE_DEVICE.axes[1].ambient_field - b_e_z = g.CAGE_DEVICE.axes[2].ambient_field - b_e = sqrt(b_e_x**2 + b_e_y**2 + b_e_z**2) - - # Perform least squares optimization on all magnetometer axes - sensor_parameters = [] - for axis, axis_samples in enumerate(samples): - result = scipy.optimize.least_squares(self.residual_function, (1.0, pi/4, pi/4, pi/4), args=(b_e, axis_samples), gtol=1e-13) - s, alpha_e, alpha, beta = result.x - residual = np.max(np.abs(result.fun)) - sensor_parameters.append({'sensitivity': s, - 'offset': offset_data[axis], - 'alpha_e': alpha_e, - 'alpha': alpha, - 'beta': beta, - 'residual': residual}) - - return sensor_parameters - - # Function passed to scipy for the optimization - @staticmethod - def residual_function(x, b_e, samples): - # Unpack vector. These unknown parameters are described in the calibration paper - s, alpha_e, alpha, beta = x - # Residual vector - res = [] - for sample in samples: - # Unpack row coefficients: - m = sample['m'] - b_x = sample['b_x'] - b_y = sample['b_y'] - b_z = sample['b_z'] - res.append(m - s * (b_e*sin(alpha_e) + b_x*cos(alpha)*cos(beta) + b_y*cos(alpha)*sin(beta) + b_z*sin(alpha))) - return res - - @staticmethod - def fibonacci_sphere(samples): - """ - Algorithm to generate roughly equally spaced points on a sphere - From https://stackoverflow.com/a/26127012""" - points = [] - phi = pi * (3.0 - sqrt(5.0)) # golden angle in radians - - for i in range(samples): - y = 1 - (i / float(samples - 1)) * 2 # y goes from 1 to -1 - radius = sqrt(1 - y * y) # radius at y - - theta = phi * i # golden angle increment - - x = cos(theta) * radius - z = sin(theta) * radius - - points.append(np.array([x, y, z])) - - return points - - def put_message(self, command, arg): - self.view_queue.put({'cmd': command, 'arg': arg}) \ No newline at end of file diff --git a/src/user_interface.py b/src/user_interface.py index 059f9cb..c3a5696 100644 --- a/src/user_interface.py +++ b/src/user_interface.py @@ -9,7 +9,10 @@ from tkinter import * from tkinter import ttk from tkinter import messagebox from tkinter import filedialog +import matplotlib as plt +from matplotlib.figure import Figure from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg +from matplotlib.backends.backend_tkagg import NavigationToolbar2Tk # import general packages: import numpy as np @@ -23,8 +26,7 @@ import src.globals as g import src.csv_threading as csv_threading import src.config_handling as config import src.csv_logging as log -from src.calibration_simple import AmbientFieldCalibration, CoilConstantCalibration, MagnetometerCalibration -from src.calibration_complete import AmbientFieldCalibration, CoilConstantCalibration, MagnetometerCalibration +from src.calibration import AmbientFieldCalibration, CoilConstantCalibration, MagnetometerCalibrationSimple, MagnetometerCalibrationComplete from src.exceptions import DeviceAccessError from src.utility import ui_print, save_dict_list_to_csv, load_dict_list_from_csv import src.helmholtz_cage_device as helmholtz_cage_device @@ -1021,17 +1023,17 @@ class CalibrateMagnetometerSimple(Frame): # Centered controls controls_frame = Frame(self.left_column) - controls_frame.grid(row=row_counter, column=0, sticky="sw") + controls_frame.grid(row=row_counter, column=0, sticky="nw") # Number of calibration points calibration_point_nr_label = Label(controls_frame, text="# of calibration points") - calibration_point_nr_label.grid(row=0, column=0, pady=5, sticky="w") + calibration_point_nr_label.grid(row=0, column=0, pady=5, sticky="nw") calibration_point_nr_entry = Entry(controls_frame, textvariable=self.calibration_points_var) - calibration_point_nr_entry.grid(row=0, column=1, pady=5, sticky="w") + calibration_point_nr_entry.grid(row=0, column=1, pady=5, sticky="nw") # Measurement interval calibration_point_nr_label = Label(controls_frame, text="Measurement interval [s]") - calibration_point_nr_label.grid(row=1, column=0, pady=5, sticky="w") + calibration_point_nr_label.grid(row=1, column=0, pady=5, sticky="nw") calibration_point_nr_entry = Entry(controls_frame, textvariable=self.calibration_interval_var) - calibration_point_nr_entry.grid(row=1, column=1, pady=5, sticky="w") + calibration_point_nr_entry.grid(row=1, column=1, pady=5, sticky="nw") # Calibration start buttons start_button_frame = Frame(controls_frame) start_button_frame.grid(row=2, column=0, columnspan=2) @@ -1043,11 +1045,11 @@ class CalibrateMagnetometerSimple(Frame): progress_bar_frame = Frame(controls_frame) progress_bar_frame.grid(row=3, column=0, columnspan=2) calibration_procedure_progress_label = Label(progress_bar_frame, text="Progress:") - calibration_procedure_progress_label.grid(row=0, column=0, padx=10, pady=10) + calibration_procedure_progress_label.grid(row=0, column=0, padx=10, pady=10, sticky="nw") calibration_procedure_progress = ttk.Progressbar(progress_bar_frame, length=240, variable=self.calibration_procedure_progress_var) - calibration_procedure_progress.grid(row=0, column=1, padx=10, pady=10, sticky="we") + calibration_procedure_progress.grid(row=0, column=1, padx=10, pady=10, sticky="nw") row_counter += 1 # CENTER COLUMN @@ -1144,7 +1146,7 @@ class CalibrateMagnetometerSimple(Frame): # Input coordinate system conversion matrix row_counter = 0 input_cos_frame = LabelFrame(self.right_column, text="Input MGM to Helmholtz COS Transformation Matrix") - input_cos_frame.grid(row=row_counter, column=2, padx=(100, 0), pady=20, sticky="nw") + input_cos_frame.grid(row=row_counter, column=0, padx=(100, 0), pady=20, sticky="nw") for i, label in enumerate(['X', 'Y', 'Z']): axis_label = Label(input_cos_frame, text=label) axis_label.grid(row=0, column=i + 1, padx=5, pady=5, sticky="nw") @@ -1300,7 +1302,7 @@ class CalibrateMagnetometerSimple(Frame): try: calibration_points = self.calibration_points_var.get() calibration_interval = self.calibration_interval_var.get() - self.calibration_thread = MagnetometerCalibration(self.view_mpi_queue, + self.calibration_thread = MagnetometerCalibrationSimple(self.view_mpi_queue, calibration_points, calibration_interval, self.mgm_to_helmholtz_cos_trans) @@ -1394,8 +1396,10 @@ class CalibrateMagnetometerComplete(Frame): self.rowconfigure(0, weight=1) self.left_column = Frame(self) self.left_column.grid(row=0, column=0, sticky="nsew") + self.center_column = Frame(self) + self.center_column.grid(row=0, column=1, sticky="nsew") self.right_column = Frame(self) - self.right_column.grid(row=0, column=1, sticky="nsew") + self.right_column.grid(row=0, column=2, sticky="nsew") self.left_column.rowconfigure(3, weight=1) # Thread variables @@ -1470,40 +1474,107 @@ class CalibrateMagnetometerComplete(Frame): # Centered controls controls_frame = Frame(self.left_column) - controls_frame.grid(row=row_counter, column=0, sticky="sw") + controls_frame.grid(row=row_counter, column=0, sticky="nw") # Number of calibration points calibration_point_nr_label = Label(controls_frame, text="# of calibration points") - calibration_point_nr_label.grid(row=0, column=0, pady=5, sticky="w") + calibration_point_nr_label.grid(row=0, column=0, pady=5, sticky="nw") calibration_point_nr_entry = Entry(controls_frame, textvariable=self.calibration_points_var) - calibration_point_nr_entry.grid(row=0, column=1, pady=5, sticky="w") + calibration_point_nr_entry.grid(row=0, column=1, pady=5, sticky="nw") # Measurement interval calibration_point_nr_label = Label(controls_frame, text="Measurement interval [s]") - calibration_point_nr_label.grid(row=1, column=0, pady=5, sticky="w") + calibration_point_nr_label.grid(row=1, column=0, pady=5, sticky="nw") calibration_point_nr_entry = Entry(controls_frame, textvariable=self.calibration_interval_var) - calibration_point_nr_entry.grid(row=1, column=1, pady=5, sticky="w") + calibration_point_nr_entry.grid(row=1, column=1, pady=5, sticky="nw") # Calibration start buttons start_button_frame = Frame(controls_frame) - start_button_frame.grid(row=2, column=0, columnspan=2) + start_button_frame.grid(row=2, column=0, columnspan=2, sticky="nw") self.start_calibration_button = Button(start_button_frame, text="Start Calibration", command=self.start_calibration_procedure, pady=5, padx=5, font=SMALL_BUTTON_FONT) - self.start_calibration_button.grid(row=0, column=0, padx=10, pady=(30, 10)) + self.start_calibration_button.grid(row=0, column=0, padx=10, pady=(30, 10), sticky="we") # Calibration progress bar progress_bar_frame = Frame(controls_frame) progress_bar_frame.grid(row=3, column=0, columnspan=2) calibration_procedure_progress_label = Label(progress_bar_frame, text="Progress:") - calibration_procedure_progress_label.grid(row=0, column=0, padx=10, pady=10) + calibration_procedure_progress_label.grid(row=0, column=0, padx=10, pady=10, sticky="nw") calibration_procedure_progress = ttk.Progressbar(progress_bar_frame, length=240, variable=self.calibration_procedure_progress_var) - calibration_procedure_progress.grid(row=0, column=1, padx=10, pady=10, sticky="we") + calibration_procedure_progress.grid(row=0, column=1, padx=10, pady=10, sticky="nw") row_counter += 1 # CENTER COLUMN - # Magnetometer calibration results row_counter = 0 - calibration_results_frame = LabelFrame(self.right_column, text="Magnetometer Results") - calibration_results_frame.grid(row=row_counter, column=1, padx=(100, 0), pady=20, sticky="nw") + # Input coordinate system conversion matrix + input_cos_frame = LabelFrame(self.center_column, text="Input MGM to Helmholtz COS Transformation Matrix") + input_cos_frame.grid(row=row_counter, column=0, padx=(100, 0), pady=20, sticky="nw") + for i, label in enumerate(['X', 'Y', 'Z']): + axis_label = Label(input_cos_frame, text=label) + axis_label.grid(row=row_counter, column=i + 1, padx=5, pady=5, sticky="nw") + row_counter += 1 + # Coordinate transformation box + cos_text_label = Label(input_cos_frame, text="X") + cos_text_label.grid(row=row_counter, column=0, padx=5, pady=5, sticky="nw") + for i in range(3): + axis_data = Entry(input_cos_frame, + textvariable=self.mgm_to_helmholtz_cos_trans[0][i], + width=15) + axis_data.grid(row=row_counter, column=i + 1, padx=5, pady=5, sticky="nw") + cos_unit = Label(input_cos_frame, text="-") + cos_unit.grid(row=row_counter, column=4, padx=5, pady=5, sticky="nw") + row_counter += 1 + cos_text_label = Label(input_cos_frame, text="Y") + cos_text_label.grid(row=row_counter, column=0, padx=5, pady=5, sticky="nw") + for i in range(3): + axis_data = Entry(input_cos_frame, + textvariable=self.mgm_to_helmholtz_cos_trans[1][i], + width=15) + axis_data.grid(row=row_counter, column=i + 1, padx=5, pady=5, sticky="nw") + cos_unit = Label(input_cos_frame, text="-") + cos_unit.grid(row=row_counter, column=4, padx=5, pady=5, sticky="nw") + row_counter += 1 + cos_text_label = Label(input_cos_frame, text="Z") + cos_text_label.grid(row=row_counter, column=0, padx=5, pady=5, sticky="nw") + for i in range(3): + axis_data = Entry(input_cos_frame, + textvariable=self.mgm_to_helmholtz_cos_trans[2][i], + width=15) + axis_data.grid(row=row_counter, column=i + 1, padx=5, pady=5, sticky="nw") + cos_unit = Label(input_cos_frame, text="-") + cos_unit.grid(row=row_counter, column=4, padx=5, pady=5, sticky="nw") + row_counter += 1 + # Note on input + label = "Note:" + axis_note = Label(input_cos_frame, text=label) + axis_note.grid(row=row_counter, column=0, padx=5, pady=5, sticky="nw") + label = "-Transfers fields value of Helmholtz cage to COS of MGM\n-B_mgm = mgm_T_hh * B_hh" + axis_note = Label(input_cos_frame, anchor='w', justify='left', text=label) + axis_note.grid(row=row_counter, column=1, padx=5, pady=5, columnspan=4, sticky="nw") + row_counter += 1 + # Save calibration buttons + save_input_cos_frame = Frame(input_cos_frame) + save_input_cos_frame.grid(row=row_counter, column=0, columnspan=5) + # Save and apply + self.export_cos_trans_button = Button(save_input_cos_frame, text="Export to CSV", + command=self.export_csv_cos_trans_matrix, + state="normal", + pady=5, padx=5) + self.export_cos_trans_button.grid(row=0, column=0, padx=5, pady=5) + self.copy_cos_trans_matrix_button = Button(save_input_cos_frame, text="Copy to clipboard", + command=self.copy_to_clipboard_cos_trans_matrix, + state="normal", + pady=5, padx=5) + self.copy_cos_trans_matrix_button.grid(row=0, column=1, padx=5, pady=5) + self.normalize_matrix_button = Button(save_input_cos_frame, text="Orthonormalize matrix", + command=self.matrix_normalize, + state="normal", + pady=5, padx=5) + self.normalize_matrix_button.grid(row=0, column=2, padx=5, pady=5) + row_counter += 1 + + # Magnetometer calibration results + calibration_results_frame = LabelFrame(self.center_column, text="Magnetometer Results") + calibration_results_frame.grid(row=row_counter, column=0, padx=(100, 0), pady=20, sticky="nw") row_counter += 1 # A_mat_inv results_label_a_mat_inv = Label(calibration_results_frame, text="A^-1 =") @@ -1514,8 +1585,9 @@ class CalibrateMagnetometerComplete(Frame): textvariable=self.a_mat_inv_result_vars[row][column], width=15, state='readonly') - axis_data.grid(row=row_counter+row, column=i + column, padx=5, pady=5, sticky="nw") + axis_data.grid(row=row_counter+row, column=1 + column, padx=5, pady=5, sticky="nw") row_counter += 3 + """ # A_mat results_label_a_mat_inv = Label(calibration_results_frame, text="A =") results_label_a_mat_inv.grid(row=row_counter+1, column=0, padx=5, pady=5, sticky="nw") @@ -1525,17 +1597,18 @@ class CalibrateMagnetometerComplete(Frame): textvariable=self.a_mat_result_vars[row][column], width=15, state='readonly') - axis_data.grid(row=row_counter+row, column=i + column, padx=5, pady=5, sticky="nw") + axis_data.grid(row=row_counter+row, column=1 + column, padx=5, pady=5, sticky="nw") row_counter += 3 + """ # b results_label_a_mat_inv = Label(calibration_results_frame, text="b^T =") results_label_a_mat_inv.grid(row=row_counter, column=0, padx=5, pady=5, sticky="nw") for row in range(3): axis_data = Entry(calibration_results_frame, - textvariable=self.b_result_vars[row], #FLAG _result_vars does not compule + textvariable=self.b_result_vars[row], #FLAG _result_vars does not compute width=15, state='readonly') - axis_data.grid(row=row_counter, column=i + row, padx=5, pady=5, sticky="nw") + axis_data.grid(row=row_counter, column=1 + row, padx=5, pady=5, sticky="nw") row_counter += 1 # Save calibration buttons @@ -1558,80 +1631,13 @@ class CalibrateMagnetometerComplete(Frame): pady=5, padx=5) self.import_calibration_data_button.grid(row=0, column=2, padx=5, pady=5) + # RIGHT COLUMN # Notes on the calibration method calibration_method_notes_frame = LabelFrame(self.right_column, text="Calibration method notes:") - calibration_method_notes_frame.grid(row=row_counter, column=1, padx=(100, 0), pady=20, sticky="nw") + calibration_method_notes_frame.grid(row=0, column=0, padx=(100, 0), pady=20, sticky="nw") label = "-Implementation of calibration according to Kok et al. [ISBN: 978-0-9824438-5-9]\n-Implementation of ellipsoid fit according to Li et al. [DOI: 10.1109/GMAP.2004.1290055]\n-Points created by Fibonacci sphere\n-Accounts for soft-iron (A matrix) and hard-iron (b offset vector) effects!\n-MEasured to calibrated field function: h=A^-1 (h_m-b)" calibration_method_notes = Label(calibration_method_notes_frame, anchor='w', justify='left', text=label) - calibration_method_notes.grid(row=1, column=0, padx=5, pady=5, sticky="nw") - #FLAG - - # RIGHT COLUMN - # Input coordinate system conversion matrix - row_counter = 0 - input_cos_frame = LabelFrame(self.right_column, text="Input MGM to Helmholtz COS Transformation Matrix") - input_cos_frame.grid(row=row_counter, column=2, padx=(100, 0), pady=20, sticky="nw") - for i, label in enumerate(['X', 'Y', 'Z']): - axis_label = Label(input_cos_frame, text=label) - axis_label.grid(row=0, column=i + 1, padx=5, pady=5, sticky="nw") - # Axis sensitivities - sensitivity_results_label = Label(input_cos_frame, text="X") - sensitivity_results_label.grid(row=1, column=0, padx=5, pady=5, sticky="nw") - for i in range(3): - axis_data = Entry(input_cos_frame, - textvariable=self.mgm_to_helmholtz_cos_trans[0][i], - width=15) - axis_data.grid(row=1, column=i + 1, padx=5, pady=5, sticky="nw") - sensitivity_results_unit = Label(input_cos_frame, text="-") - sensitivity_results_unit.grid(row=1, column=4, padx=5, pady=5, sticky="nw") - # Axis offsets - offset_results_label = Label(input_cos_frame, text="Y") - offset_results_label.grid(row=2, column=0, padx=5, pady=5, sticky="nw") - for i in range(3): - axis_data = Entry(input_cos_frame, - textvariable=self.mgm_to_helmholtz_cos_trans[1][i], - width=15) - axis_data.grid(row=2, column=i + 1, padx=5, pady=5, sticky="nw") - offset_results_unit = Label(input_cos_frame, text="-") - offset_results_unit.grid(row=2, column=4, padx=5, pady=5, sticky="nw") - # Angle to XY coil plane - angle_to_plane_label = Label(input_cos_frame, text="Z") - angle_to_plane_label.grid(row=3, column=0, padx=5, pady=5, sticky="nw") - for i in range(3): - axis_data = Entry(input_cos_frame, - textvariable=self.mgm_to_helmholtz_cos_trans[2][i], - width=15) - axis_data.grid(row=3, column=i + 1, padx=5, pady=5, sticky="nw") - angle_to_plane_unit = Label(input_cos_frame, text="-") - angle_to_plane_unit.grid(row=3, column=4, padx=5, pady=5, sticky="nw") - # Note on input - label = "Note:" - axis_note = Label(input_cos_frame, text=label) - axis_note.grid(row=4, column=0, padx=5, pady=5, sticky="nw") - label = "-Transfers fields value of Helmholtz cage to COS of MGM\n-B_mgm = mgm_T_hh * B_hh" - axis_note = Label(input_cos_frame, anchor='w', justify='left', text=label) - axis_note.grid(row=4, column=1, padx=5, pady=5, columnspan=4, sticky="nw") - # Save calibration buttons - save_input_cos_frame = Frame(input_cos_frame) - save_input_cos_frame.grid(row=6, column=0, columnspan=5) - # Save and apply - self.export_cos_trans_button = Button(save_input_cos_frame, text="Export to CSV", - command=self.export_csv_cos_trans_matrix, - state="normal", - pady=5, padx=5) - self.export_cos_trans_button.grid(row=0, column=0, padx=5, pady=5) - self.copy_cos_trans_matrix_button = Button(save_input_cos_frame, text="Copy to clipboard", - command=self.copy_to_clipboard_cos_trans_matrix, - state="normal", - pady=5, padx=5) - self.copy_cos_trans_matrix_button.grid(row=0, column=1, padx=5, pady=5) - self.normalize_matrix_button = Button(save_input_cos_frame, text="Orthonormalize matrix", - command=self.matrix_normalize, - state="normal", - pady=5, padx=5) - self.normalize_matrix_button.grid(row=0, column=2, padx=5, pady=5) - - row_counter += 1 + calibration_method_notes.grid(row=0, column=0, padx=5, pady=5, sticky="nw") # This starts an endless polling loop self.update_view() @@ -1689,51 +1695,56 @@ class CalibrateMagnetometerComplete(Frame): self.calibration_raw_results = results['raw_data'] # Unpack the dict - results = results['results'] + result = results['results'] # Flag: 'results' replaced with 'raw_data' # Display calibration in GUI - self.mag_amp_avg_set_result_vars.set("{:.3f}".format(results['mag_amp_avg_set'])) - self.total_error_result_vars.set("{:.3f}".format(results['total_error'])) + self.mag_amp_avg_set_result_vars.set("{:.3e}".format(result[0]['mag_amp_avg_set'])) + self.total_error_result_vars.set("{:.3e}".format(result[0]['total_error'])) for row in range(3): - self.b_result_vars[row].set("{:.3f}".format(results[row][column]['b'])) - self.n_result_vars[row].set("{:.3f}".format(results[row][column]['b'])) + self.b_result_vars[row].set("{:.3e}".format(result[0]['b'][row][0])) + self.n_result_vars[row].set("{:.3e}".format(result[0]['n'][row][0])) for column in range(3): #FLAG - self.a_mat_result_vars[row][column].set("{:.3f}".format(results[row][column]['a_mat'])) - self.a_mat_inv_result_vars[row][column].set("{:.3f}".format(results[row][column]['a_mat_inv'])) - self.q_mat_result_vars[row][column].set("{:.3f}".format(results[row][column]['q_mat'])) + self.a_mat_result_vars[row][column].set("{:.6f}".format(result[0]['a_mat'][row][column])) + self.a_mat_inv_result_vars[row][column].set("{:.6f}".format(result[0]['a_mat_inv'][row][column])) + self.q_mat_result_vars[row][column].set("{:.6f}".format(result[0]['q_mat'][row][column])) - # Populate clipboard string #FLAG!!! - self.clipboard = "Not implemented yet" - """ - self.clipboard = "\tX\tY\tZ\n" - self.clipboard += "A_mat [-]" - for i in range(3): - self.clipboard += "\t{:.3f}".format(results[i]['sensitivity']) - self.clipboard += "\nOffset [uT]" - for i in range(3): - self.clipboard += "\t{:.3f}".format(results[i]['offset'] * 1e6) - self.clipboard += "\nAngle to XY Plane [deg]" - for i in range(3): - self.clipboard += "\t{:.3f}".format(results[i]['alpha'] * 180 / pi) - self.clipboard += "\nAngle in XY Plane [deg]" - for i in range(3): - self.clipboard += "\t{:.3f}".format(results[i]['beta'] * 180 / pi) - self.clipboard += "\nResidual [uT]" - for i in range(3): - self.clipboard += "\t{:.3e}".format(results[i]['residual'] * 1e6) - """ + # Populate clipboard string + self.clipboard += "A_mat [-]\n" + for row in range(3): + for column in range(3): + self.clipboard += "{:.6e}\t".format(result[0]['a_mat'][row][column]) + self.clipboard += "\n" + self.clipboard += "A_mat_inv [-]\n" + for row in range(3): + for column in range(3): + self.clipboard += "{:.6e}\t".format(result[0]['a_mat_inv'][row][column]) + self.clipboard += "\n" + self.clipboard += "b [T]\n" + for row in range(3): + self.clipboard += "{:.6e}\t".format(result[0]['b'][row][0]) + self.clipboard += "\nMag_amp_avg_set [T]\n" + self.clipboard += "{:.6e}\n".format(result[0]['mag_amp_avg_set']) + self.clipboard += "Total error [T]\n" + self.clipboard += "{:.6e}\n".format(result[0]['total_error']) + self.clipboard += "Q_mat [-]\n" + for row in range(3): + for column in range(3): + self.clipboard += "{:.6e}\t".format(result[0]['q_mat'][row][column]) + self.clipboard += "\n" + self.clipboard += "n [-]\n" + for row in range(3): + self.clipboard += "{:.6e}\t".format(result[0]['n'][row][0]) # Enable save buttons self.export_calibration_button.configure(state="normal") self.copy_calibration_button.configure(state="normal") - # self.export_mgm_button.configure(state="normal") def start_calibration_procedure(self): try: calibration_points = self.calibration_points_var.get() calibration_interval = self.calibration_interval_var.get() - self.calibration_thread = MagnetometerCalibration(self.view_mpi_queue, + self.calibration_thread = MagnetometerCalibrationComplete(self.view_mpi_queue, calibration_points, calibration_interval, self.mgm_to_helmholtz_cos_trans) @@ -1750,9 +1761,24 @@ class CalibrateMagnetometerComplete(Frame): ui_print("Saved calibration results to magnetometer_calibration.csv.") def import_csv_calibration_raw_data(self): - self.calibration_raw_results, filename = load_dict_list_from_csv('magnetometer_calibration.csv',query_path=True) - print(self.calibration_raw_results) + data, raw_data, filename = load_dict_list_from_csv('magnetometer_calibration.csv', query_path=True) + if data is None or filename is None: + ui_print("File could not be loaded! Read file name is {]".format(filename)) + return ui_print("Loaded calibration results from {}".format(filename)) + ui_print("Size of read data is {}.".format(data.shape)) + if data.shape[1] != 6: + ui_print("File does not have 6 columns instead of {}!".format(data.shape)) + ui_print("[h_x_set|h_y_set|h_z_set|h_x_m|h_y_m|h_z_m|") + return + self.calibration_raw_results = raw_data + + # Execute calibration function and display results + sensor_parameters, mag_x_set, mag_y_set, mag_z_set, mag_x_m, mag_y_m, mag_z_m, cal_x, cal_y, cal_z, mag_amp_avg_set = MagnetometerCalibrationComplete.solve_system(self.view_mpi_queue, raw_data, self.mgm_to_helmholtz_cos_trans) + MagnetometerCalibrationComplete.plot_magnetometer_calibration(self, mag_x_set, mag_y_set, mag_z_set, mag_x_m, mag_y_m, mag_z_m, cal_x, cal_y, + cal_z, mag_amp_avg_set) + # Pass results to UI + self.put_message('calibration_data', {'results': sensor_parameters, 'raw_data': raw_data}) def export_csv_cos_trans_matrix(self): cos_trans_matrix = [ @@ -1772,6 +1798,9 @@ class CalibrateMagnetometerComplete(Frame): save_dict_list_to_csv('magnetometer_cos_trans_matrix.csv', cos_trans_matrix, query_path=True) ui_print("Saved MGM to Helmholtz coordinate transformation matrix to magnetometer_cos_trans_matrix.csv.") + def put_message(self, command, arg): + self.view_mpi_queue.put({'cmd': command, 'arg': arg}) #FLAG not used? + def copy_to_clipboard(self): self.clipboard_clear() self.clipboard_append(self.clipboard) diff --git a/src/utility.py b/src/utility.py index d29a48b..88d707f 100644 --- a/src/utility.py +++ b/src/utility.py @@ -44,4 +44,9 @@ def load_dict_list_from_csv(filename, query_path=False): data = np.genfromtxt(filename, dtype=float, delimiter=',') data = data[1:len(data), :] # remove header - return data, filename + # Save data point to raw_data list + raw_data = [] + for i in range(data.shape[0]): + raw_data.append({'applied_x': data[i][0], 'applied_y': data[i][1], 'applied_z': data[i][2], + 'measured_x': data[i][3], 'measured_y': data[i][4], 'measured_z': data[i][5]}) + return data, raw_data, filename