138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245 | def magyc_nls(magnetic_field: Union[np.ndarray, list], angular_rate: Union[np.ndarray, list],
time: Union[np.ndarray, list]) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
"""
Proposed method for the full calibration of a three-axis magnetometer
and a three-axis gyroscope using magnetic field and angular rate measurements.
This particular approach is based on a least squares optimization and poses
the probems as a non-linear least squares optimization problem.
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.
gyro_bias (numpy.ndarray): Gyroscope bias.
calibrated_magnetic_field (numpy.ndarray): Calibrated magnetic field measurements.
calibrated_angular_rate (numpy.ndarray): Calibrated angular rate 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 = npj.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.vstack([np.zeros((1, 3)), magnetic_field_derivative]).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, 0.0, 0.0, 0.0])
# Optimization
res = scipy.optimize.least_squares(
_magyc_nls_cost_function,
x0,
method="dogbox",
jac=_compute_jacobian_nls_jax,
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, HI and Wb
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:8].reshape(3, 1)
gyro_bias = x[8:].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
# Calibrated gyroscope measurements
calibrated_angular_rate = angular_rate - gyro_bias.flatten()
return hard_iron.flatten(), soft_iron, gyro_bias.flatten(), calibrated_magnetic_field, calibrated_angular_rate
|