diff --git a/src/calibration.py b/src/calibration.py index 06d660f..711cbd3 100644 --- a/src/calibration.py +++ b/src/calibration.py @@ -3,12 +3,10 @@ import time from datetime import datetime from threading import Thread import numpy as np -from numpy.lib.scimath import sqrt as csqrt import matplotlib.pyplot as plt 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 @@ -434,8 +432,9 @@ class MagnetometerCalibrationComplete(Thread): def run(self): try: - self.calibration_procedure() + raw_data = self.calibration_procedure() self.put_message('finished', None) + return raw_data except Exception as e: self.put_message('failed', e) finally: @@ -496,14 +495,7 @@ class MagnetometerCalibrationComplete(Thread): # 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(raw_data) # FLAG: untested! - 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( - raw_data, self.matrix_trans_mgm_to_hh) - - # Pass results to UI - self.put_message('calibration_data', {'results': sensor_parameters, 'raw_data': raw_data}) + return 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 @@ -580,7 +572,11 @@ class MagnetometerCalibrationComplete(Thread): # Retrieve calibration parameters q_mat_inv = np.linalg.inv(q_mat) b = -np.dot(q_mat_inv, n) +<<<<<<< Updated upstream 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_inv = np.real(mag_amp_avg_set / np.sqrt(np.dot(n.T, np.dot(q_mat_inv, n)) - d) * scipy.linalg.sqrtm(q_mat)) +>>>>>>> Stashed changes a_mat = np.linalg.inv(a_mat_inv) # Calculate error cal_x = np.zeros(mag_x_m.shape) @@ -753,8 +749,83 @@ class MagnetometerCalibrationComplete(Thread): return q_mat, n, d @staticmethod +<<<<<<< Updated upstream def plot_magnetometer_calibration(target_column, 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): +======= + def fit_ellipsoid(mag_x_m, mag_y_m, mag_z_m): + """ Estimate ellipsoid parameters from a set of points. + Parameters + ---------- + mag_x_m, mag_y_m, mag_z_m : array_like, array_like, array_like + The samples (M,N) where M=3 (x,y,z) and N=number of samples. + Returns + ------- + s : array_like + The samples (M,N) where M=3 (x,y,z) and N=number of samples. + Returns + ------- + M, n, d : array_like, array_like, float + The ellipsoid parameters M, n, d. + References + ---------- + .. [1] Qingde Li; Griffiths, J.G., "Least squares ellipsoid specific + fitting," in Geometric Modeling and Processing, 2004. + Proceedings, vol., no., pp.335-340, 2004 + .. https://github.com/nliaudat/magnetometer_calibration/blob/main/calibrate.py + """ + + # Converts to samples (M,N) where M=3 (x,y,z) and N=number of samples. + s = np.array([mag_x_m, mag_y_m, mag_z_m]) + + # d (samples) + d = np.array([s[0] ** 2., s[1] ** 2., s[2] ** 2., + 2. * s[1] * s[2], 2. * s[0] * s[2], 2. * s[0] * s[1], + 2. * s[0], 2. * s[1], 2. * s[2], np.ones_like(s[0])]) + + # s, s_11, s_12, s_21, s_22 (eq. 11) + s = np.dot(d, d.T) + s_11 = s[:6, :6] + s_12 = s[:6, 6:] + s_21 = s[6:, :6] + s_22 = s[6:, 6:] + + # c (Eq. 8, k=4) + c = np.array([[-1, 1, 1, 0, 0, 0], + [1, -1, 1, 0, 0, 0], + [1, 1, -1, 0, 0, 0], + [0, 0, 0, -4, 0, 0], + [0, 0, 0, 0, -4, 0], + [0, 0, 0, 0, 0, -4]]) + + # v_1 (eq. 15, solution) + e = np.dot(np.linalg.inv(c), + s_11 - np.dot(s_12, np.dot(np.linalg.inv(s_22), s_21))) + + e_w, e_v = np.linalg.eig(e) + + v_1 = e_v[:, np.argmax(e_w)] + if v_1[0] < 0: + v_1 = -v_1 + + # v_2 (eq. 13, solution) + v_2 = np.dot(np.dot(-np.linalg.inv(s_22), s_21), v_1) + + # Quadratic-form parameters + m = np.array([[v_1[0], v_1[3], v_1[4]], + [v_1[3], v_1[1], v_1[5]], + [v_1[4], v_1[5], v_1[2]]]) + n = np.array([[v_2[0]], + [v_2[1]], + [v_2[2]]]) + d = v_2[3] + + return m, n, d + + @staticmethod + def plot_magnetometer_calibration(target_column, 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): +>>>>>>> Stashed changes plot_fontsize = 5 ax_width = 0.2 diff --git a/src/user_interface.py b/src/user_interface.py index 25c7ec1..1f43590 100644 --- a/src/user_interface.py +++ b/src/user_interface.py @@ -1611,6 +1611,11 @@ class CalibrateMagnetometerComplete(Frame): width=15, state='readonly') axis_data.grid(row=row_counter + row, column=1 + column, padx=5, pady=5, sticky="nw") +<<<<<<< Updated upstream +======= + results_label_unit = Label(calibration_results_frame, text="-") + results_label_unit.grid(row=row_counter + row, column=1 + 3, padx=5, pady=5, sticky="nw") +>>>>>>> Stashed changes row_counter += 3 """ # A_mat @@ -1634,6 +1639,11 @@ class CalibrateMagnetometerComplete(Frame): width=15, state='readonly') axis_data.grid(row=row_counter, column=1 + row, padx=5, pady=5, sticky="nw") +<<<<<<< Updated upstream +======= + results_label_unit = Label(calibration_results_frame, text="T") + results_label_unit.grid(row=row_counter, column=1 + row + 1, padx=5, pady=5, sticky="nw") +>>>>>>> Stashed changes row_counter += 1 # Save calibration buttons @@ -1793,6 +1803,7 @@ class CalibrateMagnetometerComplete(Frame): try: calibration_points = self.calibration_points_var.get() calibration_interval = self.calibration_interval_var.get() +<<<<<<< Updated upstream self.calibration_thread = MagnetometerCalibrationComplete(self.view_mpi_queue, calibration_points, calibration_interval, @@ -1800,6 +1811,28 @@ class CalibrateMagnetometerComplete(Frame): self.right_column) self.calibration_thread.start() self.deactivate_buttons() +======= + calibration_mag_field = self.mag_field_magnitude_var.get() * 1e-6 # converted to micro Tesla + if calibration_mag_field <= 0 or calibration_mag_field > g.MAG_MAG_FIELD: + raise MagFieldOutOfBounds + [self.calibration_thread, raw_data] = MagnetometerCalibrationComplete(self.view_mpi_queue, + calibration_points, + calibration_interval, + calibration_mag_field, + self.right_column) + self.calibration_thread.start() + self.deactivate_buttons() + # Use collected data to build and solve system of equations + 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(raw_data, [[1, 0, 0], [0, 1, 0], [0, 0, 1]]) + # Pass results to UI + self.put_message('calibration_data', {'results': sensor_parameters, 'raw_data': raw_data}) + MagnetometerCalibrationComplete.plot_magnetometer_calibration(self.right_column, + 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 MagFieldOutOfBounds as e: + messagebox.showwarning("Calibration failed", "\n{}".format(e)) +>>>>>>> Stashed changes except (DeviceAccessError, TclError) as e: messagebox.showwarning("Calibration failed", "Failed to start calibration:\n{}".format(e)) @@ -1826,11 +1859,19 @@ class CalibrateMagnetometerComplete(Frame): # 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( +<<<<<<< Updated upstream raw_data, self.mgm_to_helmholtz_cos_trans) MagnetometerCalibrationComplete.plot_magnetometer_calibration(self.right_column, 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) +======= + raw_data, mgm_to_helmholtz_cos_trans) + MagnetometerCalibrationComplete.plot_magnetometer_calibration(self.right_column, + 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) +>>>>>>> Stashed changes # Pass results to UI self.put_message('calibration_data', {'results': sensor_parameters, 'raw_data': raw_data}) except TypeError: