TWOSTEP

The TWOSTEP method proposes a fast, robust algorithm for estimating magnetometer biases when the attitude is unknown. This algorithm combines the convergence in a single step of a heuristic algorithm currently in use with the correct treatment of the statistics of the measurements and does without discarding data.

This algorithm was the in a first publication developed for the estimation of the hard-iron (Alonso, R. Shuster, M.D. (2002a). TWOSTEP: A fast, robust algorithm for attitude-independent magnetometer-bias determination. Journal of the Astronautical Sciences, 50(4):433-452.

Parameters:

Name Type Description Default
magnetic_field ndarray or list

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

 required
reference_magnetic_field ndarray or list

Reference magnetic field measurements in a 3, or 1x3, or 3x1 numpy array or list.

 required
max_iterations int

Maximum number of iterations for the second step.

 2000
measurement_noise_std float

Standard deviation that characterizes the measurements' noise, by default 0.001 G.

 0.001

Returns:

Name Type Description
hard_iron ndarray

Estimated hard-iron bias as a 3x1 numpy array.
calibrated_magnetic_field ndarray

Calibrated magnetic field as a 3xN numpy array.

Raises:

Type Description
TypeError

If the magnetic field or reference magnetic field inputs are not numpy arrays or lists.
ValueError

If the magnetic field input is not a 3xN or Nx3 numpy array, if the reference magnetic field input is not a 3, or 1x3, or 3x1 numpy array, if the maximum number of iterations is not a positive integer, or if the measurement noise standard deviation is not a positive float.
Source code in magyc/benchmark_methods/twostep.py 
 19
 20
 21
 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
122
123
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
def twostep_hi(magnetic_field: Union[np.ndarray, list], reference_magnetic_field: Union[np.ndarray, list],
               max_iterations: int = 2000, measurement_noise_std: float = 1e-3) -> Tuple[np.ndarray, np.ndarray]:
    """
    The TWOSTEP method proposes a fast, robust algorithm for estimating magnetometer
    biases when the attitude is unknown. This algorithm combines the convergence
    in a single step of a heuristic algorithm currently in use with the correct
    treatment of the statistics of the measurements and does without discarding
    data.

    This algorithm was the in a first publication developed for the estimation of
    the hard-iron (Alonso, R. Shuster, M.D. (2002a). TWOSTEP: A fast, robust
    algorithm for attitude-independent magnetometer-bias determination. Journal
    of the Astronautical Sciences, 50(4):433-452.

    Args:
        magnetic_field (numpy.ndarray or list): Magnetic field measurements in a
            3xN or Nx3 numpy array or list.
        reference_magnetic_field (numpy.ndarray or list): Reference magnetic field
            measurements in a 3, or 1x3, or 3x1 numpy array or list.
        max_iterations (int): Maximum number of iterations for the second step.
        measurement_noise_std (float): Standard deviation that characterizes the
            measurements' noise, by default 0.001 G.

    Returns:
        hard_iron (numpy.ndarray): Estimated hard-iron bias as a 3x1 numpy array.
        calibrated_magnetic_field (numpy.ndarray): Calibrated magnetic field as a
            3xN numpy array.

    Raises:
        TypeError: If the magnetic field or reference magnetic field inputs are not
            numpy arrays or lists.
        ValueError: If the magnetic field input is not a 3xN or Nx3 numpy array, if
            the reference magnetic field input is not a 3, or 1x3, or 3x1 numpy array,
            if the maximum number of iterations is not a positive integer, or if the
            measurement noise standard deviation is not a positive float.
    """
    # Check if the magnetic field input is a list and convert it to a numpy array
    if isinstance(magnetic_field, list):
        magnetic_field = np.array(magnetic_field)

    # Check if the reference magnetic field input is a list and convert it to a numpy array
    if isinstance(reference_magnetic_field, list):
        reference_magnetic_field = np.array(reference_magnetic_field).flatten()

    # Check if the magnetic field input is a numpy array
    if not isinstance(magnetic_field, np.ndarray):
        raise TypeError("The magnetic field input must be a numpy array or a list.")

    # Check if the reference magnetic field input is a numpy array
    if not isinstance(reference_magnetic_field, np.ndarray):
        raise TypeError("The reference magnetic field input must be a numpy array or a list.")

    # Check if the magnetic field input is a 3xN or Nx3 numpy array
    if magnetic_field.ndim != 2 or (magnetic_field.shape[0] != 3 and magnetic_field.shape[1] != 3):
        raise ValueError("The magnetic field input must be a 3xN or Nx3 numpy array.")

    # Check if the reference magnetic field input is a 3, numpy array
    if reference_magnetic_field.ndim != 1 and reference_magnetic_field.size != 3:
        raise ValueError("The reference magnetic field input must be a 3, or 1x3, or 3x1 numpy array.")

    # Force the magnetic field array to be a Nx3 numpy array
    if magnetic_field.shape[0] == 3:
        magnetic_field = magnetic_field.T

    # Check that the maximum number of iterations is a positive integer
    if not isinstance(max_iterations, int) or max_iterations <= 0:
        raise ValueError("The maximum number of iterations must be a positive integer.")

    # Check that the measurement noise standard deviation is a positive float
    if not isinstance(measurement_noise_std, float) or measurement_noise_std <= 0:
        raise ValueError("The measurement noise standard deviation must be a positive float.")

    # Compute magnetic field calibration
    mf = magnetic_field

    # First step
    b0 = np.zeros((3, 1))

    # Effective measurement from paper equation (3a)
    b_matrix = np.ones((mf.shape)) * reference_magnetic_field
    z_k = (np.square(np.linalg.norm(mf, axis=1)) - np.square(np.linalg.norm(b_matrix, axis=1))).reshape(-1, 1)

    # Sensor measurements noise modeled as white gaussian with standard deviation epsilon_k
    epsilon_sq_k = np.ones(mf.shape) * (measurement_noise_std**2)

    # Sensor error scalar measurement noise characterization as gaussian.
    # Gaussian distribution mean, equation (7a)
    mu_k = -np.sum(epsilon_sq_k, axis=1, keepdims=True)

    # Gaussian distribution variance, equation (5.15)
    sigma_sq_k = (
        4
        * (
            (mf.reshape(-1, 1, 3) - b0.reshape(1, 3))
            @ np.apply_along_axis(np.diag, 1, epsilon_sq_k)
            @ (mf.reshape(-1, 3, 1) - b0)
        )
        + 2 * np.apply_along_axis(lambda x: np.square(np.trace(np.diag(x))), 1, epsilon_sq_k).reshape(-1, 1, 1)
    ).reshape(-1, 1)

    # Calculate  centered sigma squared, equation (14)
    sigma_sq_bar = 1 / np.sum(1 / sigma_sq_k)

    # Center  the  data
    mu_bar, mu_k_tilde = _center_data(mu_k, sigma_sq_k, sigma_sq_bar)
    z_bar, z_k_tilde = _center_data(z_k, sigma_sq_k, sigma_sq_bar)
    b_bar, b_k_tilde = _center_data(b_matrix, sigma_sq_k, sigma_sq_bar)

    # Offset and error covariance matrix calculation from paper equations (33) and (34)
    F_bb_tilde = np.einsum(
        "ijk->jk",
        (4 / sigma_sq_k.reshape(-1, 1, 1)) * (b_k_tilde.reshape(-1, 3, 1) @ b_k_tilde.reshape(-1, 1, 3)),
    )
    F_zb = np.einsum(
        "ijk->jk",
        ((z_k_tilde - mu_k_tilde) * (2 / sigma_sq_k)).reshape(-1, 1, 1) * b_k_tilde.reshape(-1, 3, 1),
    )
    b = np.linalg.inv(F_bb_tilde) @ F_zb

    # Second Step: Iterative
    F_bb_bar = (4 / sigma_sq_bar) * (b_bar.reshape(-1, 1) - b) @ (b_bar.reshape(-1, 1) - b).T
    b_asterisk = np.copy(b)

    if np.max(np.diag(F_bb_bar) / np.diag(F_bb_tilde)) > 0.001:
        F_bb = F_bb_tilde + F_bb_bar
        gg = (F_bb_tilde @ (b - b_asterisk)) - (1 / sigma_sq_bar) * (
            z_bar - 2 * (b_bar @ b) + np.linalg.norm(b) ** 2 - mu_bar
        ) * 2 * (b_bar.reshape(-1, 1) - b)
        bn = b - np.linalg.inv(F_bb) @ gg

        iter = 1
        while ((bn - b).T @ F_bb @ (bn - b)) > 0.001:
            b = np.copy(bn)
            gg = (F_bb_tilde @ (b - b_asterisk)) - (1 / sigma_sq_bar) * (
                z_bar - 2 * (b_bar @ b) + np.linalg.norm(b) ** 2 - mu_bar
            ) * 2 * (b_bar.reshape(-1, 1) - b)
            F_bb_bar = (4 / sigma_sq_bar) * (b_bar.reshape(-1, 1) - b) @ (b_bar.reshape(-1, 1) - b).T
            F_bb = F_bb_tilde + F_bb_bar
            bn = b - np.linalg.inv(F_bb) @ gg

            iter += 1
            if iter > max_iterations:
                warnings.warn("Second step: Maximum number of iterations reached.", RuntimeWarning)
                break

    hard_iron = bn.reshape(3, 1)

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

    return hard_iron.flatten(), calibrated_magnetic_field

The TWOSTEP method proposes a fast, robust algorithm for estimating magnetometer biases when the attitude is unknown. This algorithm combines the convergence in a single step of a heuristic algorithm currently in use with the correct treatment of the statistics of the measurements and does without discarding data.

This algorithm was extended in a second iteration to compute also the soft-iron (Alonso, R. Shuster, M.D. (2002b). Complete linear attitude-independent magnetometer calibration. Journal of the Astronautical Science, 50(4):477-490).

Parameters:

Name Type Description Default
magnetic_field ndarray or list

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

 required
reference_magnetic_field ndarray or list

Reference magnetic field measurements in a 3, or 1x3, or 3x1 numpy array or list.

 required
max_iterations int

Maximum number of iterations for the second step.

 2000
measurement_noise_std float

Standard deviation that characterizes the measurements' noise, by default 0.001 G.

 0.001

Returns:

Name Type Description
hard_iron ndarray

Estimated hard-iron bias as a 3x1 numpy array.
calibrated_magnetic_field ndarray

Calibrated magnetic field as a 3xN numpy array.

Raises:

Type Description
TypeError

If the magnetic field or reference magnetic field inputs are not numpy arrays or lists.
ValueError

If the magnetic field input is not a 3xN or Nx3 numpy array, if the reference magnetic field input is not a 3, or 1x3, or 3x1 numpy array, if the maximum number of iterations is not a positive integer, or if the measurement noise standard deviation is not a positive float.
Source code in magyc/benchmark_methods/twostep.py 
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
278
279
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
def twostep_hsi(magnetic_field: Union[np.ndarray, list], reference_magnetic_field: Union[np.ndarray, list],
                max_iterations: int = 2000, measurement_noise_std: float = 1e-3) -> Tuple[np.ndarray, np.ndarray]:
    """
    The TWOSTEP method proposes a fast, robust algorithm for estimating magnetometer
    biases when the attitude is unknown. This algorithm combines the convergence
    in a single step of a heuristic algorithm currently in use with the correct
    treatment of the statistics of the measurements and does without discarding
    data.

    This algorithm was extended in a second iteration to compute also the soft-iron
    (Alonso, R. Shuster, M.D. (2002b). Complete linear attitude-independent
    magnetometer calibration. Journal of the Astronautical Science, 50(4):477-490).

    Args:
        magnetic_field (numpy.ndarray or list): Magnetic field measurements in a
            3xN or Nx3 numpy array or list.
        reference_magnetic_field (numpy.ndarray or list): Reference magnetic field
            measurements in a 3, or 1x3, or 3x1 numpy array or list.
        max_iterations (int): Maximum number of iterations for the second step.
        measurement_noise_std (float): Standard deviation that characterizes the
            measurements' noise, by default 0.001 G.

    Returns:
        hard_iron (numpy.ndarray): Estimated hard-iron bias as a 3x1 numpy array.
        calibrated_magnetic_field (numpy.ndarray): Calibrated magnetic field as a
            3xN numpy array.

    Raises:
        TypeError: If the magnetic field or reference magnetic field inputs are not
            numpy arrays or lists.
        ValueError: If the magnetic field input is not a 3xN or Nx3 numpy array, if
            the reference magnetic field input is not a 3, or 1x3, or 3x1 numpy array,
            if the maximum number of iterations is not a positive integer, or if the
            measurement noise standard deviation is not a positive float.
    """
    # Check if the magnetic field input is a list and convert it to a numpy array
    if isinstance(magnetic_field, list):
        magnetic_field = np.array(magnetic_field)

    # Check if the reference magnetic field input is a list and convert it to a numpy array
    if isinstance(reference_magnetic_field, list):
        reference_magnetic_field = np.array(reference_magnetic_field).flatten()

    # Check if the magnetic field input is a numpy array
    if not isinstance(magnetic_field, np.ndarray):
        raise TypeError("The magnetic field input must be a numpy array or a list.")

    # Check if the reference magnetic field input is a numpy array
    if not isinstance(reference_magnetic_field, np.ndarray):
        raise TypeError("The reference magnetic field input must be a numpy array or a list.")

    # Check if the magnetic field input is a 3xN or Nx3 numpy array
    if magnetic_field.ndim != 2 or (magnetic_field.shape[0] != 3 and magnetic_field.shape[1] != 3):
        raise ValueError("The magnetic field input must be a 3xN or Nx3 numpy array.")

    # Check if the reference magnetic field input is a 3, numpy array
    if reference_magnetic_field.ndim != 1 and reference_magnetic_field.size != 3:
        raise ValueError("The reference magnetic field input must be a 3, or 1x3, or 3x1 numpy array.")

    # Force the magnetic field array to be a Nx3 numpy array
    if magnetic_field.shape[0] == 3:
        magnetic_field = magnetic_field.T

    # Check that the maximum number of iterations is a positive integer
    if not isinstance(max_iterations, int) or max_iterations <= 0:
        raise ValueError("The maximum number of iterations must be a positive integer.")

    # Check that the measurement noise standard deviation is a positive float
    if not isinstance(measurement_noise_std, float) or measurement_noise_std <= 0:
        raise ValueError("The measurement noise standard deviation must be a positive float.")

    # Compute magnetic field calibration
    mf = magnetic_field

    stop_tol = 1e-24  # Stop Condition from Alonso paper
    I3 = np.eye(3, dtype=np.float64)

    # TWOSTEP Centered estimate
    # Set initial guess for b and D.
    b0 = np.zeros((3, 1))
    d0 = np.zeros((3, 3))

    # Form L matrix, equations (5.10b) and (5.12a)
    l1 = 2 * mf
    l2 = -np.square(mf)
    l3 = -2 * mf[:, [0]] * mf[:, [1]]
    l4 = -2 * mf[:, [0]] * mf[:, [2]]
    l5 = -2 * mf[:, [1]] * mf[:, [2]]
    L_k = np.concatenate([l1, l2, l3, l4, l5], axis=1)

    # Compute sensor error as scalar measurement, equation (5.7a)
    h_matrix = np.ones((mf.shape)) * reference_magnetic_field
    z_k = (np.square(np.linalg.norm(mf, axis=1)) - np.square(np.linalg.norm(h_matrix, axis=1))).reshape(-1, 1)

    # Sensor measurements noise modeled as white gaussian with standard deviation epsilon_k
    epsilon_sq_k = np.ones(mf.shape) * (measurement_noise_std**2)

    # Sensor error scalar measurement noise characterization as gaussian.
    # Gaussian distribution mean, equation (5.14)
    mu_k = -np.sum(epsilon_sq_k, axis=1, keepdims=True)

    # Gaussian distribution variance, equation (5.15)
    sigma_sq_k = (
        4
        * np.einsum(
            "ijk->ikj",
            np.tile(I3 + d0, (mf.shape[0], 1, 1)) @ mf.reshape(-1, 3, 1) - np.tile(b0, (mf.shape[0], 1, 1)),
        )
        @ np.apply_along_axis(np.diag, 1, epsilon_sq_k)
        @ (np.tile(I3 + d0, (mf.shape[0], 1, 1)) @ mf.reshape(-1, 3, 1) - np.tile(b0, (mf.shape[0], 1, 1)))
        + 2 * np.apply_along_axis(lambda x: np.square(np.trace(np.diag(x))), 1, epsilon_sq_k).reshape(-1, 1, 1)
    ).reshape(-1, 1)

    # Calculate centered sigma squared, equation (5.18)
    sigma_sq_bar = 1 / np.sum(1 / sigma_sq_k)

    # Center the data, equation (5.19)
    mu_bar, mu_k_tilde = _center_data(mu_k, sigma_sq_k, sigma_sq_bar)
    z_bar, z_k_tilde = _center_data(z_k, sigma_sq_k, sigma_sq_bar)
    L_bar, L_k_tilde = _center_data(L_k, sigma_sq_k, sigma_sq_bar)

    # Compute fisher information matrix
    I_fisher_tilde, I_fishinv_tilde = _TS_fisher_centered(sigma_sq_k, L_k_tilde)

    # Compute centered estimate, equation (5.24)
    f_matrix = np.einsum(
        "ijk->jk",
        (
            (1 / sigma_sq_k).reshape(-1, 1, 1)
            * ((z_k_tilde - mu_k_tilde).reshape(-1, 1, 1) * L_k_tilde.reshape(-1, 9, 1))
        ),
    )
    theta_0_tilde = I_fishinv_tilde @ f_matrix

    # TWOSTEP Center correction
    theta_n, theta_np1 = theta_0_tilde, theta_0_tilde  # Initiate theta for  first  iteration
    n = 0  # Initialise  iteration counter
    TS_err = np.Inf  # Initial  condition  for  error.

    # ABC is used to remove intensive calculations out of for loop
    abc = -np.einsum(
        "ijk->jk",
        (
            (1 / sigma_sq_k).reshape(-1, 1, 1)
            * ((z_k_tilde - mu_k_tilde).reshape(-1, 1, 1) * L_k_tilde.reshape(-1, 9, 1))
        ),
    )

    while TS_err > stop_tol and n < max_iterations:
        if n != 0:  # If  we are not  in the first	iteration
            theta_n = theta_np1

        # Extract  c  and  E  components
        c, e_matrix = _theta_to_c_E(theta_n)

        # Compute  second  derivative  of  b^2  wrt theta
        tmp = np.linalg.solve((np.eye(3) + e_matrix), c) @ np.linalg.solve((np.eye(3) + e_matrix), c).T
        dbsqdtheta_p = np.concatenate(
            [
                2 * np.linalg.solve((np.eye(3) + e_matrix), c),
                -np.diag(tmp).reshape(3, 1),
                np.vstack([-2 * tmp[0, 1], -2 * tmp[0, 2], -2 * tmp[1, 2]]),
            ]
        )
        # Compute gradient of J
        dJdThetap_tilde = abc + I_fisher_tilde @ theta_n
        dJdThetap_bar = (
            -(1 / sigma_sq_bar)
            * (L_bar.reshape(-1, 1) - dbsqdtheta_p)
            * (z_bar - (L_bar.reshape(1, -1) @ theta_n) + (c.T @ np.linalg.solve((np.eye(3) + e_matrix), c)) - mu_bar)
        )
        dJdTheta = dJdThetap_tilde + dJdThetap_bar

        # Calculate Fisher matrix
        I_fisher_bar = _TS_fisher_center(sigma_sq_bar, L_bar, dbsqdtheta_p)

        # Update theta
        theta_np1 = theta_n - np.linalg.solve((I_fisher_tilde + I_fisher_bar), dJdTheta)

        # Compute error
        TS_err = ((theta_np1 - theta_n).T @ (I_fisher_tilde + I_fisher_bar)) @ (theta_np1 - theta_n)
        n += 1

    b, d_matrix = _theta_to_b_D(theta_np1)

    # Extract covariance matrix
    m_cd = np.array(
        [
            [b[0, 0], 0, 0, b[1, 0], b[2, 0], 0],
            [0, b[1, 0], 0, b[0, 0], 0, b[2, 0]],
            [0, 0, b[2, 0], 0, b[0, 0], b[1, 0]],
        ]
    )
    m_ed = np.array(
        [
            [2 * d_matrix[0, 0], 0, 0, 2 * d_matrix[0, 1], 2 * d_matrix[0, 2], 0],
            [0, 2 * d_matrix[1, 1], 0, 2 * d_matrix[0, 1], 0, 2 * d_matrix[1, 2]],
            [0, 0, 2 * d_matrix[2, 2], 0, 2 * d_matrix[0, 2], 2 * d_matrix[1, 2]],
            [d_matrix[0, 1], d_matrix[0, 1], 0, d_matrix[0, 0] + d_matrix[1, 1], d_matrix[1, 2], d_matrix[0, 2]],
            [d_matrix[0, 2], 0, d_matrix[0, 2], d_matrix[1, 2], d_matrix[0, 0] + d_matrix[2, 2], d_matrix[0, 1]],
            [0, d_matrix[1, 2], d_matrix[1, 2], d_matrix[0, 2], d_matrix[0, 1], d_matrix[1, 1] + d_matrix[2, 2]],
        ]
    )
    dbD_dcE = np.eye(9)
    dbD_dcE[:3, :3], dbD_dcE[:3, 3:] = np.eye(3) + d_matrix, m_cd
    dbD_dcE[3:, :3], dbD_dcE[3:, 3:] = np.zeros((6, 3)), 2 * np.eye(6) @ m_ed
    dbD_dcE = np.linalg.inv(dbD_dcE)
    # Cov_est = dbD_dcE @ np.linalg.solve((I_fisher_tilde + I_fisher_bar), dbD_dcE.T)

    # END   TWOSTEP
    hard_iron = (np.linalg.inv(np.eye(3) + d_matrix)) @ b
    soft_iron = np.linalg.inv(np.eye(3) + d_matrix)

    # 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