eive/Helmholtz_Test_Bench
16
0
Fork 0
forked from zietzm/Helmholtz_Test_Bench
Code Issues Pull Requests Projects Releases 1 Wiki Activity

Trying to plot directly after gathering of data (untested!)

Browse Source
This commit is contained in:
Markus Koller
2023-01-29 15:35:42 +01:00
parent e107d15cf5
commit dbd393e661
2 changed files with 123 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/calibration.py
+82 -11
View File
@@ -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
src/user_interface.py
+41
View File
@@ -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: