def MagnetPosError_5d(magnet_pos_5d, Bt, sensor_param, Measured_data):
"""
The function provide the error of one magnet_pos
:param magnet_pos: 1*5 magnet position and orientation vector (x, y, z, theta, phi)
:param Bt:
:param sensor_param: 16*6 sensor position and orientation matrix
:param Measured_data: 1*48 data
:return:
"""
Measured_data = Measured_data.reshape((16, 3))
sensor_pos = sensor_param[:, 0:3]
sensor_rotation = sensor_param[:, 3:6]
sensor_rotation_matrix = []
for r_v in sensor_rotation:
sensor_rotation_matrix.append(
np.dot(yaw_matrix(r_v[2]), pitch_matrix(r_v[1]),
roll_matrix(r_v[0])))
sensor_rotation_matrix = np.array(sensor_rotation_matrix)
magnet_pos = magnetPos_from_setup(magnet_pos_5d)[0]
theo_data_init = M_field_value_model(magnet_pos, sensor_pos)
Theorectical_data = []
for i in range(theo_data_init.shape[0]):
Theorectical_data.append(
np.dot(theo_data_init[i], sensor_rotation_matrix[i]).tolist())
Theorectical_data = 1e7 * Bt * np.array(
Theorectical_data) #change to mG unit
E_Matrix = (Measured_data - Theorectical_data).transpose()
Error = sum(E_Matrix[0]**2) + sum(E_Matrix[1]**2) + sum(E_Matrix[2]**2)
return Error
def MagnetPosError_5d_jac(magnet_pos_5d, Bt, sensor_param, Measured_data):
theta = magnet_pos_5d[3]
phi = magnet_pos_5d[4]
'''Theoretical magnetic value'''
Measured_data = Measured_data.reshape((16, 3))
sensor_pos = sensor_param[:, 0:3]
sensor_rotation = sensor_param[:, 3:6]
sensor_rotation_matrix = []
for r_v in sensor_rotation:
sensor_rotation_matrix.append(
np.dot(yaw_matrix(r_v[2]), pitch_matrix(r_v[1]),
roll_matrix(r_v[0])))
sensor_rotation_matrix = np.array(sensor_rotation_matrix)
magnet_pos = magnetPos_from_setup(magnet_pos_5d)[0]
theo_data_init = M_field_value_model(magnet_pos, sensor_pos)
Theorectical_data = []
for i in range(theo_data_init.shape[0]):
Theorectical_data.append(
np.dot(theo_data_init[i], sensor_rotation_matrix[i]).tolist())
Theorectical_data = 1e7 * Bt * np.array(
Theorectical_data) # change to mG unit
Error_matrix = Theorectical_data - Measured_data
'''derivative part'''
a = sensor_pos[:, 0] - magnet_pos[0]
b = sensor_pos[:, 1] - magnet_pos[1]
c = sensor_pos[:, 2] - magnet_pos[2]
m, n, p = magnet_pos[3], magnet_pos[4], magnet_pos[5]
Blx_a = (9 * m * a + 3 * n * b + 3 * p * c) * (a**2 + b**2 + c**2)**(
-5 / 2) - 5 * a * (3 * m * a**2 + 3 * n * b * c +
3 * p * c * a) * (a**2 + b**2 + c**2)**(-7 / 2)
Bly_a = (3 * m * b + 3 * n * a) * (a**2 + b**2 + c**2)**(
-5 / 2) - 5 * a * (3 * m * a * b + 3 * n * b**2 +
3 * p * c * b) * (a**2 + b**2 + c**2)**(-7 / 2)
Blz_a = (3 * m * c + 3 * p * a) * (a**2 + b**2 + c**2)**(
-5 / 2) - 5 * a * (3 * m * a * c + 3 * n * b * c +
3 * p * c**2) * (a**2 + b**2 + c**2)**(-7 / 2)
Blx_b = (3 * m * a + 3 * m * b) * (a**2 + b**2 + c**2)**(
-5 / 2) - 5 * b * (3 * m * a**2 + 3 * n * b * c +
3 * p * a * c) * (a**2 + b**2 + c**2)**(-7 / 2)
Bly_b = (3 * m * a + 9 * n * b + 3 * p * c) * (a**2 + b**2 + c**2)**(
-5 / 2) - 5 * b * (3 * m * a * b + 3 * n * b**2 +
3 * p * c * b) * (a**2 + b**2 + c**2)**(-7 / 2)
Blz_b = (3 * n * c + 3 * p * b) * (a**2 + b**2 + c**2)**(
-5 / 2) - 5 * b * (3 * m * a * c + 3 * n * b * c +
3 * p * c**2) * (a**2 + b**2 + c**2)**(-7 / 2)
Blx_c = (3 * p * a + 3 * m * c) * (a**2 + b**2 + c**2)**(
-5 / 2) - 5 * c * (3 * m * a**2 + 3 * n * b * a +
3 * p * c * a) * (a**2 + b**2 + c**2)**(-7 / 2)
Bly_c = (3 * p * b + 3 * m * c) * (a**2 + b**2 + c**2)**(
-5 / 2) - 5 * c * (3 * m * a * b + 3 * n * b**2 +
3 * p * c * b) * (a**2 + b**2 + c**2)**(-7 / 2)
Blz_c = (3 * m * a + 3 * n * b + 9 * p * c) * (a**2 + b**2 + c**2)**(
-5 / 2) - 5 * c * (3 * m * a * c + 3 * n * b * c +
3 * p * c**2) * (a**2 + b**2 + c**2)**(-7 / 2)
Blx_m = (2 * a**2 - b**2 - c**2) * (a**2 + b**2 + c**2)**(-5 / 2)
Bly_m = (3 * a * b) * (a**2 + b**2 + c**2)**(-5 / 2)
Blz_m = (3 * a * c) * (a**2 + b**2 + c**2)**(-5 / 2)
Blx_n = (3 * a * b) * (a**2 + b**2 + c**2)**(-5 / 2)
Bly_n = (2 * b**2 - a**2 - c**2) * (a**2 + b**2 + c**2)**(-5 / 2)
Blz_n = (3 * b * c) * (a**2 + b**2 + c**2)**(-5 / 2)
Blx_p = (3 * a * c) * (a**2 + b**2 + c**2)**(-5 / 2)
Bly_p = (3 * b * c) * (a**2 + b**2 + c**2)**(-5 / 2)
Blz_p = (2 * c**2 - b**2 - a**2) * (a**2 + b**2 + c**2)**(-5 / 2)
a_x = -1
b_y = -1
c_z = -1
m_theta = math.cos(phi) * math.cos(theta)
n_theta = math.sin(phi) * math.cos(theta)
p_theta = -math.sin(theta)
m_phi = -math.sin(theta) * math.sin(phi)
n_phi = math.sin(theta) * math.cos(phi)
p_phi = 0
B_x = Bt * np.vstack((Blx_a * a_x, Bly_a * a_x, Blz_a * a_x)).transpose()
B_y = Bt * np.vstack((Blx_b * b_y, Bly_b * b_y, Blz_b * b_y)).transpose()
B_z = Bt * np.vstack((Blx_c * c_z, Bly_c * c_z, Blz_c * c_z)).transpose()
B_theta = Bt * np.vstack(
(Blx_m * m_theta + Blx_n * n_theta + Blx_p * p_theta,
Bly_m * m_theta + Bly_n * n_theta + Bly_p * p_theta,
Blz_m * m_theta + Blz_n * n_theta + Blz_p * p_theta)).transpose()
B_phi = Bt * np.vstack(
(Blx_m * m_phi + Blx_n * n_phi + Blx_p * p_phi,
Bly_m * m_phi + Bly_n * n_phi + Bly_p * p_phi,
Blz_m * m_phi + Blz_n * n_phi + Blz_p * p_phi)).transpose()
Error_x = sum(sum(2 * Error_matrix * B_x))
Error_y = sum(sum(2 * Error_matrix * B_y))
Error_z = sum(sum(2 * Error_matrix * B_z))
Error_theta = sum(sum(2 * Error_matrix * B_theta))
Error_phi = sum(sum(2 * Error_matrix * B_phi))
return np.array([0., 0., 0., Error_theta, Error_phi])
magnetPos6d_init = np.array([0., 0., 0., m_opt, n_opt, p_opt])
print(magnetPos6d_init)
bounds = ([-190e-3, -120e-3, -140e-3, -1., -1.,
-1.], [190e-3, 120e-3, 140e-3, 1., 1., 1.])
res_final = least_squares(MagnetPosError_6d,
magnetPos6d_init,
verbose=0,
bounds=bounds,
jac=MagnetPosError_6d_jac,
ftol=1e-8,
xtol=1e-10,
method='trf',
args=(Bt, sensorParam, data[i]))
x_opt, y_opt, z_opt = res_final.x[0], res_final.x[1], res_final.x[2]
print("optimization results")
print(res_final.x)
magnetPos6d_init = np.array(
[0., 0., 0.,
math.sqrt(1 / 3),
math.sqrt(1 / 3),
math.sqrt(1 / 3)])
print("original position")
print(magnetPos_from_setup(p_m_original[Idx[0]])[i])
print("\n")
from lib.PreProcessingParam import *
from lib.util import *
from lib.magneticModel import *
sensor_param = sio.loadmat("../LabParam0222.mat")['sensorParam']
Bt = sio.loadmat("../LabParam0222.mat")['Bt']
magnet_pos = np.array([0., 0., 0., 0., 0.])
x = magnet_pos[0]
y = magnet_pos[1]
z = magnet_pos[2]
theta = magnet_pos[3]
phi = magnet_pos[4]
magnetPos = magnetPos_from_setup(magnet_pos)
sensorPos = sensor_param[:, 0:3]
a = sensorPos[:, 0] - magnetPos[0, 0]
b = sensorPos[:, 1] - magnetPos[0, 1]
c = sensorPos[:, 2] - magnetPos[0, 2]
m, n, p = magnetPos[0, 3], magnetPos[0, 4], magnetPos[0, 5]
Blx_a = (9 * m * a + 3 * n * b + 3 * p * c) * (a**2 + b**2 + c**2)**(
-5 / 2) - 5 * a * (3 * m * a**2 + 3 * n * b * c +
3 * p * c * a) * (a**2 + b**2 + c**2)**(-7 / 2)
Bly_a = (3 * m * b + 3 * n * a) * (a**2 + b**2 + c**2)**(-5 / 2) - 5 * a * (
3 * m * a * b + 3 * n * b**2 + 3 * p * c * b) * (a**2 + b**2 + c**2)**(-7 /
2)
(p_xy['c_phi90'].shape[0], 1)))),
14:
np.hstack((p_xy['c_phi0'], -h * np.zeros(
(p_xy['c_phi0'].shape[0], 1)), 90 * np.ones(
(p_xy['c_phi0'].shape[0], 1)),
np.zeros((p_xy['c_phi0'].shape[0], 1)))),
15:
np.hstack((p_xy['c_phi90'], -h * np.zeros(
(p_xy['c_phi90'].shape[0], 1)), 90 * np.ones(
(p_xy['c_phi90'].shape[0], 1)), -90 * np.ones(
(p_xy['c_phi90'].shape[0], 1)))),
}
p_m_actual = {}
for i in range(16):
p_m_actual[i] = magnetPos_from_setup(p_m_original[i])
'''calibration Bt'''
Idx = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
#Idx = [0, 3, 6, 7, 8, 9, 10, 11]
#Idx = [12, 13, 14, 15]
#Idx = [7, 8, 9, 10, 11]
#Idx = [0, 3]
#Idx = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
magnetPos = np.vstack((
p_m_actual[Idx[0]],
p_m_actual[Idx[1]],
p_m_actual[Idx[2]],
p_m_actual[Idx[3]],
p_m_actual[Idx[4]],
p_m_actual[Idx[5]],