Magnetometer Calibration Gyro Aided

The linear least squares for sensor bias calibration is seeks to minimize the sum of squared residuals

\[\sum_{i=1}^{n} \frac{1}{\sigma_i^2} ||\dot{x}_i + \omega_i \times (x_i - b)||^2\]

Where \(x(t)\) is the measured magnetic field, \(\dot{x(t)}\) is the measured magnetic field differentiated with respect to time, \(\omega(t)\) is the measured angular-rate in instrument coordinates, \(b\) is the hard-iron, and \(\times\) is the standard cross product operator.

This optimization problem can be solved in an analytical way. For further information refer to section IV.A in Troni, G. and Whitcomb, L. L. (2019). Field sensor bias calibration with angular-rate sensors: Theory and experimental evaluation with application to magnetometer calibration. IEEE/ASME Transactions on Mechatronics, 24(4):1698--1710.

Parameters:

Name Type Description Default
magnetic_field ndarray or list

Magnetic field measurements in a 3xN or Nx3 numpy array or list.

 required
angular_rate ndarray or list

Angular rate measurements in a 3xN or Nx3 numpy array or list.

 required
time ndarray or list

Time measurements in a 1D numpy array or list.

 required

Returns:

Name Type Description
hard_iron ndarray

Hard iron bias.
calibrated_magnetic_field ndarray

Calibrated magnetic field measurements

Raises:

Type Description
TypeError

If the magnetic field, angular rate, or time are not numpy arrays or lists.
ValueError

If the magnetic field, angular rate, or time are not 3xN or Nx3 numpy arrays.
ValueError

If the magnetic field, angular rate, and time do not have the same number of samples.
Source code in magyc/benchmark_methods/sar.py 
 22
 23
 24
 25
 26
 27
 28
 29
 30
 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
def sar_ls(magnetic_field: Union[np.ndarray, list], angular_rate: Union[np.ndarray, list],
           time: Union[np.ndarray, list]) -> Tuple[np.ndarray, np.ndarray]:
    """
    The linear least squares for sensor bias calibration is seeks to minimize the
    sum of squared residuals

    $$\\sum_{i=1}^{n} \\frac{1}{\\sigma_i^2} ||\\dot{x}_i + \\omega_i \\times (x_i - b)||^2$$

    Where $x(t)$ is the measured magnetic field, $\\dot{x(t)}$ is the measured
    magnetic field differentiated with respect to time, $\\omega(t)$ is the
    measured angular-rate in instrument coordinates, $b$ is the hard-iron, and
    $\\times$ is the standard cross product operator.

    This optimization problem can be solved in an analytical way. For further
    information refer to section IV.A in Troni, G. and Whitcomb, L. L. (2019).
    Field sensor bias calibration with angular-rate sensors: Theory and experimental
    evaluation with application to magnetometer calibration. IEEE/ASME Transactions
    on Mechatronics, 24(4):1698--1710.

    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 measurements in a 1D numpy array or list.

    Returns:
        hard_iron (numpy.ndarray): Hard iron bias.
        calibrated_magnetic_field (numpy.ndarray): Calibrated magnetic field measurements

    Raises:
        TypeError: If the magnetic field, angular rate, or time are not numpy arrays or lists.
        ValueError: If the magnetic field, angular rate, or time are not 3xN or Nx3 numpy arrays.
        ValueError: If the magnetic field, angular rate, and time do not have the same number of samples.
    """
    # 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 magnetic calibration
    # Get the data variance
    magnetic_field_variance = _get_sigma_noise(magnetic_field)
    sigma_i = np.linalg.norm(magnetic_field_variance).reshape(-1, 1, 1)

    # Compute the skew-symmetric matrix of the angular rate.
    skew_symmetric_angular_rate = np.apply_along_axis(nm.vec_to_so3, 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)

    # Estimate b
    b1_inv = np.linalg.inv(np.einsum("ijk->jk", (skew_symmetric_angular_rate ** 2) * (1 / sigma_i)))

    yi = np.einsum(
        "ijk->ikj",
        np.cross(angular_rate.reshape(-1, 1, 3), magnetic_field.reshape(-1, 1, 3))
        + magnetic_field_derivative.reshape(-1, 1, 3),
    )
    b2 = np.einsum("ijk->jk", (skew_symmetric_angular_rate @ yi) * (1 / sigma_i))

    hard_iron = b1_inv @ b2

    # Calibrate magnetic field
    calibrated_magnetic_field = magnetic_field - hard_iron.flatten()

    return hard_iron.flatten(), calibrated_magnetic_field

The Kalman filter for sensor bias calibration uses the system model with a discretization of the continuous-time system the sensor bias estimation can be solved with a standard discrete-time Kalman filter implementation that does not require differentiation.

\[\dot{x}_i = -\omega_i \times (x_i - b)\]

Where \(x(t)\) is the measured magnetic field, \(\dot{x(t)}\) is the measured magnetic field differentiated with respect to time, \(\omega(t)\) is the measured angular-rate in instrument coordinates, and \(b\) is the hard-iron.

For further information refer to section IV.B in Troni, G. and Whitcomb, L. L. (2019). Field sensor bias calibration with angular-rate sensors: Theory and experimental evaluation with application to magnetometer calibration. IEEE/ASME Transactions on Mechatronics, 24(4):1698--1710.

Parameters:

Name Type Description Default
magnetic_field ndarray or list

Magnetic field measurements in a 3xN or Nx3 numpy array or list.

 required
angular_rate ndarray or list

Angular rate measurements in a 3xN or Nx3 numpy array or list.

 required
time ndarray or list

Time measurements in a 1D numpy array or list.

 required
gains tuple

Kalman filter gains.

 (1.0, 1.0)
f_normalize bool

Whether the k2 gain should be scaled by and adaptive constant computed as the reciprocal of the norm of the gyroscope measurement for that step, by default False.

 False

Returns:

Name Type Description
hard_iron ndarray

Hard iron bias.
calibrated_magnetic_field ndarray

Calibrated magnetic field measurements
calibrated_filteres_magnetic_field ndarray

Calibrated and filtered magnetic field measurements

Raises:

Type Description
TypeError

If the magnetic field, angular rate, or time are not numpy arrays or lists.
ValueError

If the magnetic field, angular rate, or time are not 3xN or Nx3 numpy arrays.
ValueError

If the magnetic field, angular rate, and time do not have the same number of samples.
TypeError

If the gains are not a tuple of floats.
TypeError

If the f_normalize is not a boolean.
Source code in magyc/benchmark_methods/sar.py 
124
125
126
127
128
129
130
131
132
133
134
135
136
137
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
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
def sar_kf(magnetic_field: Union[np.ndarray, list], angular_rate: Union[np.ndarray, list],
           time: Union[np.ndarray, list], gains: Tuple[float, float] = (1.0, 1.0),
           f_normalize: bool = False) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    The Kalman filter for sensor bias calibration uses the system model with  a
    discretization of the continuous-time system the sensor bias estimation can
    be solved with a standard discrete-time Kalman filter implementation that
    does not require differentiation.

    $$\\dot{x}_i = -\\omega_i \\times (x_i - b)$$

    Where $x(t)$ is the measured magnetic field, $\\dot{x(t)}$ is the measured
    magnetic field differentiated with respect to time, $\\omega(t)$ is the
    measured angular-rate in instrument coordinates, and $b$ is the hard-iron.

    For further information refer to section IV.B in Troni, G. and Whitcomb, L. L.
    (2019). Field sensor bias calibration with angular-rate sensors: Theory and
    experimental evaluation with application to magnetometer calibration. IEEE/ASME
    Transactions on Mechatronics, 24(4):1698--1710.

    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 measurements in a 1D numpy array or list.
        gains (tuple): Kalman filter gains.
        f_normalize (bool): Whether the k2 gain should be scaled by and adaptive
            constant computed as the reciprocal of the norm of the gyroscope measurement
            for that step, by default False.

    Returns:
        hard_iron (numpy.ndarray): Hard iron bias.
        calibrated_magnetic_field (numpy.ndarray): Calibrated magnetic field measurements
        calibrated_filteres_magnetic_field (numpy.ndarray): Calibrated and filtered magnetic field measurements

    Raises:
        TypeError: If the magnetic field, angular rate, or time are not numpy arrays or lists.
        ValueError: If the magnetic field, angular rate, or time are not 3xN or Nx3 numpy arrays.
        ValueError: If the magnetic field, angular rate, and time do not have the same number of samples.
        TypeError: If the gains are not a tuple of floats.
        TypeError: If the f_normalize is not a boolean.
    """
    # 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.")

    # Check that the gains are a tuple of floats
    if not isinstance(gains, tuple):
        raise TypeError("The gains must be a tuple of floats.")
    if not all(isinstance(gain, float) for gain in gains):
        raise TypeError("The gains must be a tuple of floats.")

    # Check that the f_normalize is a boolean
    if not isinstance(f_normalize, bool):
        raise TypeError("The f_normalize must be a boolean.")

    # Compute the magnetic calibration
    # Initial parameters
    b0 = np.zeros((3, ))
    k1a = gains[0]
    k1b = gains[1] if len(gains) >= 2 else gains[0]
    k2 = gains[2] if len(gains) >= 3 else gains[1]
    mf = magnetic_field.reshape(3, -1)
    w = angular_rate.reshape(3, -1)
    dt = np.diff(time)
    dt_vec = np.concatenate([np.array([dt[0]]), dt])

    # Kalman Model
    Bc = np.zeros([6, 1])
    # Measurement model
    H1 = np.hstack([np.eye(3), np.zeros([3, 3])])
    # Process noise covariance
    Qc = np.diag([k1a, k1a, k1a, k1b, k1b, k1b])
    # Variance in the measurements
    R = np.diag([k2, k2, k2])

    # KF
    F1 = _kf_transition_matrix([0, 0, 0])
    n = F1.shape[0]
    m = F1.shape[1]
    MM = np.zeros([n, mf.shape[1]])
    PP = np.zeros([n, m, mf.shape[1]])
    AA = np.zeros([n, m, mf.shape[1]])
    QQ = np.zeros([n, m, mf.shape[1]])
    KK = np.zeros([n, H1.shape[0], mf.shape[1]])

    # Initial guesses for the state mean and covariance.
    x = np.hstack([mf[:, 0], b0])
    p01 = 0.001  # P0 gyro
    p02 = 0.001  # P0 bias
    P0 = np.diag([p01, p01, p01, p02, p02, p02])
    P = P0

    # Filtering steps.
    for ix in range(mf.shape[1]):
        # Discretization of the continous-time system (dtk)
        dtk = dt_vec[ix]
        u = w[:, ix]

        [Ak, Bk, Qk] = _kf_lti_discretize(_kf_transition_matrix(u), Bc, Qc, dtk)

        AA[:, :, ix] = Ak
        QQ[:, :, ix] = Qk

        # Prediction
        [x, P] = _kf_predict(x, P, Ak, Qk)
        [x, P, K, dy, S] = _kf_update(x, P, mf[:, ix], H1, R)

        MM[:, ix] = x
        PP[:, :, ix] = P
        KK[:, :, ix] = K

    # Final Bias averaging last 20%
    hard_iron = np.mean(MM[3:, -int(np.round(mf.shape[1]*0.2)):], axis=1, keepdims=True)

    # Calibrate magnetic field
    calibrated_magnetic_field = magnetic_field - hard_iron.flatten()
    calibrated_filteres_magnetic_field = MM[:3, :].T

    return hard_iron.flatten(), calibrated_magnetic_field, calibrated_filteres_magnetic_field

The adaptive identification for sensor bias calibration proposes that the unknown sensor bias, \(b\), can be estimated on-line with a novel adaptive identification algorithm. The possible advantages of this adaptive approach are that (i) it does not require numerical differentiation of the sensor measurement \(x(t)\), (ii) it is less computationally expensive than the SAR-KF, and (iii) it could be combined with other nonlinear observer methods.

For further information refer to section IV.C in Troni, G. and Whitcomb, L. L. (2019). Field sensor bias calibration with angular-rate sensors: Theory and experimental evaluation with application to magnetometer calibration. IEEE/ASME Transactions on Mechatronics, 24(4):1698--1710.

Parameters:

Name Type Description Default
magnetic_field ndarray or list

Magnetic field measurements in a 3xN or Nx3 numpy array or list.

 required
angular_rate ndarray or list

Angular rate measurements in a 3xN or Nx3 numpy array or list.

 required
time ndarray or list

Time measurements in a 1D numpy array or list.

 required
gains tuple

Gains defined in the set of equations (5) of the proposed method as a tuple of floats, by default (1.0, 1.0)

 (1.0, 1.0)
f_normalize bool

Whether the k2 gain should be scaled by and adaptive constant computed as the reciprocal of the norm of the gyroscope measurement for that step, by default False.

 False

Returns:

Name Type Description
hard_iron ndarray

Hard iron bias.
calibrated_magnetic_field ndarray

Calibrated magnetic field measurements
calibrated_filtered_magnetic_field ndarray

Calibrated and filtered magnetic field measurements

Raises:

Type Description
TypeError

If the magnetic field, angular rate, or time are not numpy arrays or lists.
ValueError

If the magnetic field, angular rate, or time are not 3xN or Nx3 numpy arrays.
ValueError

If the magnetic field, angular rate, and time do not have the same number of samples.
TypeError

If the gains are not a tuple of floats.
TypeError

If the f_normalize is not a boolean.
Source code in magyc/benchmark_methods/sar.py 
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
def sar_aid(magnetic_field: Union[np.ndarray, list], angular_rate: Union[np.ndarray, list],
            time: Union[np.ndarray, list], gains: Tuple[float, float] = (1.0, 1.0),
            f_normalize: bool = False) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    The adaptive identification for sensor bias calibration proposes that the
    unknown sensor bias, $b$, can be estimated on-line with a novel adaptive
    identification algorithm. The possible advantages of this adaptive approach
    are that (i) it does not require numerical differentiation of the sensor
    measurement $x(t)$, (ii) it is less computationally expensive than the SAR-KF,
    and (iii) it could be combined with other nonlinear observer methods.

    For further information refer to section IV.C in Troni, G. and Whitcomb, L. L.
    (2019). Field sensor bias calibration with angular-rate sensors: Theory and
    experimental evaluation with application to magnetometer calibration. IEEE/ASME
    Transactions on Mechatronics, 24(4):1698--1710.

    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 measurements in a 1D numpy array or list.
        gains (tuple): Gains defined in the set of equations (5) of the proposed method as
            a tuple of floats, by default (1.0, 1.0)
        f_normalize (bool): Whether the k2 gain should be scaled by and adaptive
            constant computed as the reciprocal of the norm of the gyroscope measurement
            for that step, by default False.

    Returns:
        hard_iron (numpy.ndarray): Hard iron bias.
        calibrated_magnetic_field (numpy.ndarray): Calibrated magnetic field measurements
        calibrated_filtered_magnetic_field (numpy.ndarray): Calibrated and filtered magnetic field measurements

    Raises:
        TypeError: If the magnetic field, angular rate, or time are not numpy arrays or lists.
        ValueError: If the magnetic field, angular rate, or time are not 3xN or Nx3 numpy arrays.
        ValueError: If the magnetic field, angular rate, and time do not have the same number of samples.
        TypeError: If the gains are not a tuple of floats.
        TypeError: If the f_normalize is not a boolean.
    """
    # 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.")

    # Check that the gains are a tuple of floats
    if not isinstance(gains, tuple):
        raise TypeError("The gains must be a tuple of floats.")
    if not all(isinstance(gain, float) for gain in gains):
        raise TypeError("The gains must be a tuple of floats.")

    # Check that the f_normalize is a boolean
    if not isinstance(f_normalize, bool):
        raise TypeError("The f_normalize must be a boolean.")

    # Compute the magnetic calibration
    # Initial parameters
    b0 = np.zeros((3, ))
    k1 = gains[0]
    k2 = gains[1]
    mf = magnetic_field.reshape(3, -1)
    w = angular_rate.reshape(3, -1)
    dt = np.diff(time)
    dt_vec = np.concatenate([np.array([dt[0]]), dt])

    # Compute the skew-symmetric matrix of the angular rate.
    skew_symmetric_angular_rate = np.apply_along_axis(nm.vec_to_so3, 1, angular_rate)

    # Adaptive ID system
    mh = np.zeros((3, mf.shape[1] + 1))
    mhd = np.zeros((3, mf.shape[1]))
    bh = np.zeros((3, mf.shape[1] + 1))
    bhd = np.zeros((3, mf.shape[1]))
    mh[:, 0] = magnetic_field[0, :]
    bh[:, 0] = b0

    for ix in range(mf.shape[1]):
        mhd[:, ix] = (
            - skew_symmetric_angular_rate[ix, :, :] @ mh[:, ix]
            + skew_symmetric_angular_rate[ix, :, :] @ bh[:, ix]
            - k1 * (mh[:, ix] - mf[:, ix])
        )

        if (np.linalg.norm(w[:, ix]) > 0.01) and f_normalize:
            k_adap = 1 / np.linalg.norm(w[:, ix])
            bhd[:, ix] = -k_adap * k2 * skew_symmetric_angular_rate[ix, :, :] @ (mh[:, ix] - mf[:, ix])
        else:
            bhd[:, ix] = -k2 * skew_symmetric_angular_rate[ix, :, :].T @ (mh[:, ix] - mf[:, ix])

        mh[:, ix + 1] = mh[:, ix] + dt_vec[ix] * mhd[:, ix]
        bh[:, ix + 1] = bh[:, ix] + dt_vec[ix] * bhd[:, ix]

    # Final Bias averaging last 20%
    hard_iron = np.mean(bh[:, -int(np.round(mf.shape[1] * 0.2)):], axis=1, keepdims=True)

    # Calibrate magnetic field
    calibrated_magnetic_field = magnetic_field - hard_iron.flatten()
    calibrated_filtered_magnetic_field = mh[:, :-1].T

    return hard_iron.flatten(), calibrated_magnetic_field, calibrated_filtered_magnetic_field