31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135 | def magyc_ls(magnetic_field: Union[np.ndarray, list], angular_rate: Union[np.ndarray, list],
time: Union[np.ndarray, list]) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
"""
Proposed method for the full calibration of a three-axis magnetometer
using magnetic field and angular rate measurements. This particular approach
is based on a least squares optimization and poses the probems as a linear
least squares optimization problem.
Even tough a closed solution can be computed, it is a ill-conditioned problem
and the optimization is preferred.
Args:
magnetic_field (numpy.ndarray or list): Magnetic field measurements in a
3xN or Nx3 numpy array or list.
angular_rate (numpy.ndarray or list): Angular rate measurements in a 3xN or
Nx3 numpy array or list.
time (numpy.ndarray or list): Time stamps of the measurements.
Returns:
hard_iron (numpy.ndarray): Hard iron bias.
soft_iron (numpy.ndarray): Soft iron matrix.
calibrated_magnetic_field (numpy.ndarray): Calibrated magnetic field measurements.
Raises:
TypeError: If the magnetic field, angular rate, and time are not numpy arrays or lists.
ValueError: If the magnetic field and angular rate are not 3xN or Nx3 numpy
arrays, or if the time is not a 1D numpy array.
"""
# Check if the magnetic_field, angular_rate, and time are lists and convert them to numpy arrays
if isinstance(magnetic_field, list):
magnetic_field = np.array(magnetic_field)
if isinstance(angular_rate, list):
angular_rate = np.array(angular_rate)
if isinstance(time, list):
time = np.array(time)
# Check if the magnetic_field, angular_rate, and time are numpy arrays
if not isinstance(magnetic_field, np.ndarray):
raise TypeError("The magnetic field must be a numpy array or a list.")
if not isinstance(angular_rate, np.ndarray):
raise TypeError("The angular rate must be a numpy array or a list.")
if not isinstance(time, np.ndarray):
raise TypeError("The time must be a numpy array or a list.")
# Check if the magnetic_field and angular_rate are 3xN or Nx3 numpy arrays
if magnetic_field.ndim != 2 or (magnetic_field.shape[0] != 3 and magnetic_field.shape[1] != 3):
raise ValueError("The magnetic field must be a 3xN or Nx3 numpy array.")
if angular_rate.ndim != 2 or (angular_rate.shape[0] != 3 and angular_rate.shape[1] != 3):
raise ValueError("The angular rate must be a 3xN or Nx3 numpy array.")
# Check if the time is a 1D numpy array
time = time.flatten()
if time.ndim != 1:
raise ValueError("The time must be a (n, ), (n, 1) or (1, n) numpy array.")
# Force the magnetic_field and angular_rate to be Nx3 numpy arrays
if magnetic_field.shape[0] == 3:
magnetic_field = magnetic_field.T
if angular_rate.shape[0] == 3:
angular_rate = angular_rate.T
# Check if the magnetic_field, angular_rate, and time have the same number of samples
if magnetic_field.shape[0] != angular_rate.shape[0] or magnetic_field.shape[0] != time.shape[0]:
raise ValueError("The magnetic field, angular rate, and time must have the same number of samples.")
# Compute the skew symmetric matrix of the angular rate
skew_symmetric_angular_rate = np.apply_along_axis(_vec_to_so3_jax, 1, angular_rate)
# Compute the magnetic field derivative
magnetic_field_derivative = np.diff(magnetic_field, axis=0) / np.diff(time).reshape(-1, 1)
magnetic_field_derivative = np.concatenate([np.zeros((1, 3)), magnetic_field_derivative], axis=0).reshape(-1, 3, 1)
# Reshape magnetic field
magnetic_field_3d = magnetic_field.reshape(-1, 3, 1)
# Compute the magnetic calibration
# Least Squares Initial Guess and Constraints
x0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
# Optimization
res = scipy.optimize.least_squares(
_magyc_ls_cost_function,
x0,
jac=_magyc_ls_jacobian,
method="dogbox",
verbose=0,
loss="linear",
max_nfev=1000,
ftol=1.00e-06,
gtol=None,
xtol=None,
x_scale="jac",
args=(magnetic_field_3d, magnetic_field_derivative, skew_symmetric_angular_rate)
)
# Compute SI and HI
x = res["x"]
lower_triangular_matrix = np.array([[exp(x[0]), 0, 0], [x[1], exp(x[2]), 0], [x[3], x[4], 1 / exp(x[0] + x[2])]])
soft_iron = np.linalg.inv(lower_triangular_matrix @ lower_triangular_matrix.T)
hard_iron = soft_iron @ x[5:].reshape(3, 1)
# Calibrate magnetic field
calibrated_magnetic_field = (np.linalg.inv(soft_iron) @ (magnetic_field.reshape(3, -1) - hard_iron.reshape(3, 1))).T
return hard_iron.flatten(), soft_iron, calibrated_magnetic_field
|