1 /*
2 * Copyright (C) 2022 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.navigation.inertial.calibration.magnetometer;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.navigation.LockedException;
20 import com.irurueta.navigation.NotReadyException;
21 import com.irurueta.navigation.inertial.calibration.CalibrationException;
22 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyMagneticFluxDensity;
23 import com.irurueta.numerical.robust.LMedSRobustEstimator;
24 import com.irurueta.numerical.robust.LMedSRobustEstimatorListener;
25 import com.irurueta.numerical.robust.RobustEstimator;
26 import com.irurueta.numerical.robust.RobustEstimatorException;
27 import com.irurueta.numerical.robust.RobustEstimatorMethod;
28
29 import java.util.List;
30
31 /**
32 * Robustly estimates magnetometer hard-iron biases, cross couplings and
33 * scaling factors using LMedS algorithm.
34 * <p>
35 * To use this calibrator at least 10 measurements with known magnetic field norm at
36 * an unknown position and instant must be taken at 10 different unknown orientations
37 * when common z-axis is assumed, otherwise at least 13
38 * measurements are required.
39 * <p>
40 * Measured magnetic flux density is assumed to follow the model shown below:
41 * <pre>
42 * mBmeas = bm + (I + Mm) * mBtrue + w
43 * </pre>
44 * Where:
45 * - mBmeas is the measured magnetic flux density. This is a 3x1 vector.
46 * - bm is magnetometer hard-iron bias. Ideally, on a perfect magnetometer,
47 * this should be a 3x1 zero vector.
48 * - I is the 3x3 identity matrix.
49 * - Mm is the 3x3 soft-iron matrix containing cross-couplings and scaling
50 * factors. Ideally, on a perfect magnetometer, this should be a 3x3 zero
51 * matrix.
52 * - mBtrue is ground-truth magnetic flux density. This is a 3x1 vector.
53 * - w is measurement noise. This is a 3x1 vector.
54 * Notice that this calibrator assumes that all measurements are taken in
55 * a short span of time, where Earth magnetic field can be assumed to be
56 * constant at provided location and instant.
57 */
58 public class LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator extends
59 RobustKnownMagneticFluxDensityNormMagnetometerCalibrator {
60
61 /**
62 * Default value to be used for stop threshold. Stop threshold can be used to
63 * avoid keeping the algorithm unnecessarily iterating in case that best
64 * estimated threshold using median of residuals is not small enough. Once a
65 * solution is found that generates a threshold below this value, the
66 * algorithm will stop.
67 * The stop threshold can be used to prevent the LMedS algorithm iterating
68 * too many times in cases where samples have a very similar accuracy.
69 * For instance, in cases where proportion of outliers is very small (close
70 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
71 * iterate for a long time trying to find the best solution when indeed
72 * there is no need to do that if a reasonable threshold has already been
73 * reached.
74 * Because of this behaviour the stop threshold can be set to a value much
75 * lower than the one typically used in LMedS, and yet the algorithm could
76 * still produce even smaller thresholds in estimated results.
77 */
78 public static final double DEFAULT_STOP_THRESHOLD = 1e-9;
79
80 /**
81 * Minimum allowed stop threshold value.
82 */
83 public static final double MIN_STOP_THRESHOLD = 0.0;
84
85 /**
86 * Threshold to be used to keep the algorithm iterating in case that best
87 * estimated threshold using median of residuals is not small enough. Once
88 * a solution is found that generates a threshold below this value, the
89 * algorithm will stop.
90 * The stop threshold can be used to prevent the LMedS algorithm iterating
91 * too many times in cases where samples have a very similar accuracy.
92 * For instance, in cases where proportion of outliers is very small (close
93 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
94 * iterate for a long time trying to find the best solution when indeed
95 * there is no need to do that if a reasonable threshold has already been
96 * reached.
97 * Because of this behaviour the stop threshold can be set to a value much
98 * lower than the one typically used in LMedS, and yet the algorithm could
99 * still produce even smaller thresholds in estimated results.
100 */
101 private double stopThreshold = DEFAULT_STOP_THRESHOLD;
102
103 /**
104 * Constructor.
105 */
106 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator() {
107 super();
108 }
109
110 /**
111 * Constructor.
112 *
113 * @param listener listener to handle events raised by this calibrator.
114 */
115 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
116 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
117 super(listener);
118 }
119
120 /**
121 * Constructor.
122 *
123 * @param measurements list of body magnetic flux density
124 * measurements with standard deviation of
125 * magnetometer measurements taken at the same
126 * position with zero velocity and unknown different
127 * orientations.
128 */
129 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
130 final List<StandardDeviationBodyMagneticFluxDensity> measurements) {
131 super(measurements);
132 }
133
134 /**
135 * Constructor.
136 *
137 * @param commonAxisUsed indicates whether z-axis is assumed to be common
138 * for the accelerometer, gyroscope and magnetometer.
139 */
140 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(final boolean commonAxisUsed) {
141 super(commonAxisUsed);
142 }
143
144 /**
145 * Constructor.
146 *
147 * @param initialHardIron initial hard-iron to find a solution.
148 * @throws IllegalArgumentException if provided hard-iron array does
149 * not have length 3.
150 */
151 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(final double[] initialHardIron) {
152 super(initialHardIron);
153 }
154
155 /**
156 * Constructor.
157 *
158 * @param initialHardIron initial hard-iron to find a solution.
159 * @throws IllegalArgumentException if provided hard-iron matrix is not
160 * 3x1.
161 */
162 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(final Matrix initialHardIron) {
163 super(initialHardIron);
164 }
165
166 /**
167 * Constructor.
168 *
169 * @param initialHardIron initial hard-iron to find a solution.
170 * @param initialMm initial soft-iron matrix containing scale factors
171 * and cross coupling errors.
172 * @throws IllegalArgumentException if provided hard-iron matrix is not
173 * 3x1 or if soft-iron matrix is not
174 * 3x3.
175 */
176 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
177 final Matrix initialHardIron, final Matrix initialMm) {
178 super(initialHardIron, initialMm);
179 }
180
181 /**
182 * Constructor.
183 *
184 * @param measurements list of body magnetic flux density
185 * measurements with standard deviation of
186 * magnetometer measurements taken at the same
187 * position with zero velocity and unknown different
188 * orientations.
189 * @param listener listener to handle events raised by this calibrator.
190 */
191 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
192 final List<StandardDeviationBodyMagneticFluxDensity> measurements,
193 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
194 super(measurements, listener);
195 }
196
197 /**
198 * Constructor.
199 *
200 * @param measurements collection of body magnetic flux density
201 * measurements with standard deviation of
202 * magnetometer measurements taken at the same
203 * position with zero velocity and unknown different
204 * orientations.
205 * @param commonAxisUsed indicates whether z-axis is assumed to be common
206 * for the accelerometer, gyroscope and magnetometer.
207 */
208 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
209 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed) {
210 super(measurements, commonAxisUsed);
211 }
212
213 /**
214 * Constructor.
215 *
216 * @param measurements collection of body magnetic flux density
217 * measurements with standard deviation of
218 * magnetometer measurements taken at the same
219 * position with zero velocity and unknown different
220 * orientations.
221 * @param commonAxisUsed indicates whether z-axis is assumed to be common
222 * for the accelerometer, gyroscope and magnetometer.
223 * @param listener listener to handle events raised by this calibrator.
224 */
225 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
226 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
227 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
228 super(measurements, commonAxisUsed, listener);
229 }
230
231 /**
232 * Constructor.
233 *
234 * @param measurements collection of body magnetic flux density
235 * measurements with standard deviation of
236 * magnetometer measurements taken at the same
237 * position with zero velocity and unknown different
238 * orientations.
239 * @param initialHardIron initial hard-iron to find a solution.
240 * @throws IllegalArgumentException if provided hard-iron array does
241 * not have length 3.
242 */
243 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
244 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron) {
245 super(measurements, initialHardIron);
246 }
247
248 /**
249 * Constructor.
250 *
251 * @param measurements collection of body magnetic flux density
252 * measurements with standard deviation of
253 * magnetometer measurements taken at the same
254 * position with zero velocity and unknown different
255 * orientations.
256 * @param initialHardIron initial hard-iron to find a solution.
257 * @param listener listener to handle events raised by this calibrator.
258 * @throws IllegalArgumentException if provided hard-iron array does
259 * not have length 3.
260 */
261 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
262 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
263 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
264 super(measurements, initialHardIron, listener);
265 }
266
267 /**
268 * Constructor.
269 *
270 * @param measurements collection of body magnetic flux density
271 * measurements with standard deviation of
272 * magnetometer measurements taken at the same
273 * position with zero velocity and unknown different
274 * orientations.
275 * @param commonAxisUsed indicates whether z-axis is assumed to be common
276 * for the accelerometer, gyroscope and magnetometer.
277 * @param initialHardIron initial hard-iron to find a solution.
278 * @throws IllegalArgumentException if provided hard-iron array does
279 * not have length 3.
280 */
281 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
282 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
283 final double[] initialHardIron) {
284 super(measurements, commonAxisUsed, initialHardIron);
285 }
286
287 /**
288 * Constructor.
289 *
290 * @param measurements collection of body magnetic flux density
291 * measurements with standard deviation of
292 * magnetometer measurements taken at the same
293 * position with zero velocity and unknown different
294 * orientations.
295 * @param commonAxisUsed indicates whether z-axis is assumed to be common
296 * for the accelerometer, gyroscope and magnetometer.
297 * @param initialHardIron initial hard-iron to find a solution.
298 * @param listener listener to handle events raised by this calibrator.
299 * @throws IllegalArgumentException if provided hard-iron array does
300 * not have length 3.
301 */
302 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
303 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
304 final double[] initialHardIron,
305 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
306 super(measurements, commonAxisUsed, initialHardIron, listener);
307 }
308
309 /**
310 * Constructor.
311 *
312 * @param measurements collection of body magnetic flux density
313 * measurements with standard deviation of
314 * magnetometer measurements taken at the same
315 * position with zero velocity and unknown different
316 * orientations.
317 * @param initialHardIron initial hard-iron to find a solution.
318 * @throws IllegalArgumentException if provided hard-iron matrix is not
319 * 3x1.
320 */
321 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
322 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron) {
323 super(measurements, initialHardIron);
324 }
325
326 /**
327 * Constructor.
328 *
329 * @param measurements collection of body magnetic flux density
330 * measurements with standard deviation of
331 * magnetometer measurements taken at the same
332 * position with zero velocity and unknown different
333 * orientations.
334 * @param initialHardIron initial hard-iron to find a solution.
335 * @param listener listener to handle events raised by this calibrator.
336 * @throws IllegalArgumentException if provided hard-iron matrix is not
337 * 3x1.
338 */
339 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
340 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
341 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
342 super(measurements, initialHardIron, listener);
343 }
344
345 /**
346 * Constructor.
347 *
348 * @param measurements collection of body magnetic flux density
349 * measurements with standard deviation of
350 * magnetometer measurements taken at the same
351 * position with zero velocity and unknown different
352 * orientations.
353 * @param commonAxisUsed indicates whether z-axis is assumed to be common
354 * for the accelerometer, gyroscope and magnetometer.
355 * @param initialHardIron initial hard-iron to find a solution.
356 * @throws IllegalArgumentException if provided hard-iron matrix is not
357 * 3x1.
358 */
359 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
360 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
361 final Matrix initialHardIron) {
362 super(measurements, commonAxisUsed, initialHardIron);
363 }
364
365 /**
366 * Constructor.
367 *
368 * @param measurements collection of body magnetic flux density
369 * measurements with standard deviation of
370 * magnetometer measurements taken at the same
371 * position with zero velocity and unknown different
372 * orientations.
373 * @param commonAxisUsed indicates whether z-axis is assumed to be common
374 * for the accelerometer, gyroscope and magnetometer.
375 * @param initialHardIron initial hard-iron to find a solution.
376 * @param listener listener to handle events raised by this calibrator.
377 * @throws IllegalArgumentException if provided hard-iron matrix is not
378 * 3x1.
379 */
380 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
381 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
382 final Matrix initialHardIron,
383 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
384 super(measurements, commonAxisUsed, initialHardIron, listener);
385 }
386
387 /**
388 * Constructor.
389 *
390 * @param measurements collection of body magnetic flux density
391 * measurements with standard deviation of
392 * magnetometer measurements taken at the same
393 * position with zero velocity and unknown different
394 * orientations.
395 * @param initialHardIron initial hard-iron to find a solution.
396 * @param initialMm initial soft-iron matrix containing scale factors
397 * and cross coupling errors.
398 * @throws IllegalArgumentException if provided hard-iron matrix is not
399 * 3x1 or if soft-iron matrix is not
400 * 3x3.
401 */
402 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
403 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
404 final Matrix initialMm) {
405 super(measurements, initialHardIron, initialMm);
406 }
407
408 /**
409 * Constructor.
410 *
411 * @param measurements collection of body magnetic flux density
412 * measurements with standard deviation of
413 * magnetometer measurements taken at the same
414 * position with zero velocity and unknown different
415 * orientations.
416 * @param initialHardIron initial hard-iron to find a solution.
417 * @param initialMm initial soft-iron matrix containing scale factors
418 * and cross coupling errors.
419 * @param listener listener to handle events raised by this calibrator.
420 * @throws IllegalArgumentException if provided hard-iron matrix is not
421 * 3x1 or if soft-iron matrix is not
422 * 3x3.
423 */
424 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
425 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
426 final Matrix initialMm, final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
427 super(measurements, initialHardIron, initialMm, listener);
428 }
429
430 /**
431 * Constructor.
432 *
433 * @param measurements collection of body magnetic flux density
434 * measurements with standard deviation of
435 * magnetometer measurements taken at the same
436 * position with zero velocity and unknown different
437 * orientations.
438 * @param commonAxisUsed indicates whether z-axis is assumed to be common
439 * for the accelerometer, gyroscope and magnetometer.
440 * @param initialHardIron initial hard-iron to find a solution.
441 * @param initialMm initial soft-iron matrix containing scale factors
442 * and cross coupling errors.
443 * @throws IllegalArgumentException if provided hard-iron matrix is not
444 * 3x1 or if soft-iron matrix is not
445 * 3x3.
446 */
447 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
448 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
449 final Matrix initialHardIron, final Matrix initialMm) {
450 super(measurements, commonAxisUsed, initialHardIron, initialMm);
451 }
452
453 /**
454 * Constructor.
455 *
456 * @param measurements collection of body magnetic flux density
457 * measurements with standard deviation of
458 * magnetometer measurements taken at the same
459 * position with zero velocity and unknown different
460 * orientations.
461 * @param commonAxisUsed indicates whether z-axis is assumed to be common
462 * for the accelerometer, gyroscope and magnetometer.
463 * @param initialHardIron initial hard-iron to find a solution.
464 * @param initialMm initial soft-iron matrix containing scale factors
465 * and cross coupling errors.
466 * @param listener listener to handle events raised by this calibrator.
467 * @throws IllegalArgumentException if provided hard-iron matrix is not
468 * 3x1 or if soft-iron matrix is not
469 * 3x3.
470 */
471 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
472 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
473 final Matrix initialHardIron, final Matrix initialMm,
474 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
475 super(measurements, commonAxisUsed, initialHardIron, initialMm, listener);
476 }
477
478 /**
479 * Constructor.
480 *
481 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
482 * @throws IllegalArgumentException if provided magnetic flux norm value is negative.
483 */
484 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
485 final Double groundTruthMagneticFluxDensityNorm) {
486 super(groundTruthMagneticFluxDensityNorm);
487 }
488
489 /**
490 * Constructor.
491 *
492 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
493 * @param listener listener to handle events raised by this calibrator.
494 * @throws IllegalArgumentException if provided magnetic flux norm value is negative.
495 */
496 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
497 final Double groundTruthMagneticFluxDensityNorm,
498 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
499 super(groundTruthMagneticFluxDensityNorm, listener);
500 }
501
502 /**
503 * Constructor.
504 *
505 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
506 * @param measurements collection of body magnetic flux density
507 * measurements with standard deviation of
508 * magnetometer measurements taken at the same
509 * position with zero velocity and unknown different
510 * orientations.
511 * @throws IllegalArgumentException if provided magnetic flux norm value is negative.
512 */
513 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
514 final Double groundTruthMagneticFluxDensityNorm,
515 final List<StandardDeviationBodyMagneticFluxDensity> measurements) {
516 super(groundTruthMagneticFluxDensityNorm, measurements);
517 }
518
519 /**
520 * Constructor.
521 *
522 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
523 * @param commonAxisUsed indicates whether z-axis is assumed to be common
524 * for the accelerometer, gyroscope and magnetometer.
525 * @throws IllegalArgumentException if provided magnetic flux norm value is negative.
526 */
527 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
528 final Double groundTruthMagneticFluxDensityNorm, final boolean commonAxisUsed) {
529 super(groundTruthMagneticFluxDensityNorm, commonAxisUsed);
530 }
531
532 /**
533 * Constructor.
534 *
535 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
536 * @param initialHardIron initial hard-iron to find a solution.
537 * @throws IllegalArgumentException if provided magnetic flux norm value is
538 * negative, or if provided hard-iron array does
539 * not have length 3.
540 */
541 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
542 final Double groundTruthMagneticFluxDensityNorm, final double[] initialHardIron) {
543 super(groundTruthMagneticFluxDensityNorm, initialHardIron);
544 }
545
546 /**
547 * Constructor.
548 *
549 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
550 * @param initialHardIron initial hard-iron to find a solution.
551 * @throws IllegalArgumentException if provided magnetic flux norm value is
552 * negative, or if provided hard-iron matrix is not
553 * 3x1.
554 */
555 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
556 final Double groundTruthMagneticFluxDensityNorm, final Matrix initialHardIron) {
557 super(groundTruthMagneticFluxDensityNorm, initialHardIron);
558 }
559
560 /**
561 * Constructor.
562 *
563 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
564 * @param initialHardIron initial hard-iron to find a solution.
565 * @param initialMm initial soft-iron matrix containing scale factors
566 * and cross coupling errors.
567 * @throws IllegalArgumentException if provided magnetic flux norm value is
568 * negative, or if provided hard-iron matrix is not
569 * 3x1 or if soft-iron matrix is not
570 * 3x3.
571 */
572 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
573 final Double groundTruthMagneticFluxDensityNorm, final Matrix initialHardIron, final Matrix initialMm) {
574 super(groundTruthMagneticFluxDensityNorm, initialHardIron, initialMm);
575 }
576
577 /**
578 * Constructor.
579 *
580 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
581 * @param measurements collection of body magnetic flux density
582 * measurements with standard deviation of
583 * magnetometer measurements taken at the same
584 * position with zero velocity and unknown different
585 * orientations.
586 * @param listener listener to handle events raised by this calibrator.
587 * @throws IllegalArgumentException if provided magnetic flux norm value is negative.
588 */
589 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
590 final Double groundTruthMagneticFluxDensityNorm,
591 final List<StandardDeviationBodyMagneticFluxDensity> measurements,
592 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
593 super(groundTruthMagneticFluxDensityNorm, measurements, listener);
594 }
595
596 /**
597 * Constructor.
598 *
599 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
600 * @param measurements collection of body magnetic flux density
601 * measurements with standard deviation of
602 * magnetometer measurements taken at the same
603 * position with zero velocity and unknown different
604 * orientations.
605 * @param commonAxisUsed indicates whether z-axis is assumed to be common
606 * for the accelerometer, gyroscope and magnetometer.
607 * @throws IllegalArgumentException if provided magnetic flux norm value is negative.
608 */
609 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
610 final Double groundTruthMagneticFluxDensityNorm,
611 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed) {
612 super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed);
613 }
614
615 /**
616 * Constructor.
617 *
618 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
619 * @param measurements collection of body magnetic flux density
620 * measurements with standard deviation of
621 * magnetometer measurements taken at the same
622 * position with zero velocity and unknown different
623 * orientations.
624 * @param commonAxisUsed indicates whether z-axis is assumed to be common
625 * for the accelerometer, gyroscope and magnetometer.
626 * @param listener listener to handle events raised by this calibrator.
627 * @throws IllegalArgumentException if provided magnetic flux norm value is negative.
628 */
629 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
630 final Double groundTruthMagneticFluxDensityNorm,
631 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
632 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
633 super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, listener);
634 }
635
636 /**
637 * Constructor.
638 *
639 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
640 * @param measurements collection of body magnetic flux density
641 * measurements with standard deviation of
642 * magnetometer measurements taken at the same
643 * position with zero velocity and unknown different
644 * orientations.
645 * @param initialHardIron initial hard-iron to find a solution.
646 * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
647 * or if provided hard-iron array does not have length 3.
648 */
649 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
650 final Double groundTruthMagneticFluxDensityNorm,
651 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron) {
652 super(groundTruthMagneticFluxDensityNorm, measurements, initialHardIron);
653 }
654
655 /**
656 * Constructor.
657 *
658 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
659 * @param measurements collection of body magnetic flux density
660 * measurements with standard deviation of
661 * magnetometer measurements taken at the same
662 * position with zero velocity and unknown different
663 * orientations.
664 * @param initialHardIron initial hard-iron to find a solution.
665 * @param listener listener to handle events raised by this calibrator.
666 * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
667 * or if provided hard-iron array does not have length 3.
668 */
669 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
670 final Double groundTruthMagneticFluxDensityNorm,
671 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
672 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
673 super(groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
674 }
675
676 /**
677 * Constructor.
678 *
679 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
680 * @param measurements collection of body magnetic flux density
681 * measurements with standard deviation of
682 * magnetometer measurements taken at the same
683 * position with zero velocity and unknown different
684 * orientations.
685 * @param commonAxisUsed indicates whether z-axis is assumed to be common
686 * for the accelerometer, gyroscope and magnetometer.
687 * @param initialHardIron initial hard-iron to find a solution.
688 * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
689 * or if provided hard-iron array does not have length 3.
690 */
691 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
692 final Double groundTruthMagneticFluxDensityNorm,
693 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
694 final double[] initialHardIron) {
695 super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
696 }
697
698 /**
699 * Constructor.
700 *
701 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
702 * @param measurements collection of body magnetic flux density
703 * measurements with standard deviation of
704 * magnetometer measurements taken at the same
705 * position with zero velocity and unknown different
706 * orientations.
707 * @param commonAxisUsed indicates whether z-axis is assumed to be common
708 * for the accelerometer, gyroscope and magnetometer.
709 * @param initialHardIron initial hard-iron to find a solution.
710 * @param listener listener to handle events raised by this calibrator.
711 * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
712 * or if provided hard-iron array does not have length 3.
713 */
714 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
715 final Double groundTruthMagneticFluxDensityNorm,
716 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
717 final double[] initialHardIron,
718 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
719 super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
720 }
721
722 /**
723 * Constructor.
724 *
725 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
726 * @param measurements collection of body magnetic flux density
727 * measurements with standard deviation of
728 * magnetometer measurements taken at the same
729 * position with zero velocity and unknown different
730 * orientations.
731 * @param initialHardIron initial hard-iron to find a solution.
732 * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
733 * or if provided hard-iron matrix is not 3x1.
734 */
735 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
736 final Double groundTruthMagneticFluxDensityNorm,
737 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron) {
738 super(groundTruthMagneticFluxDensityNorm, measurements, initialHardIron);
739 }
740
741 /**
742 * Constructor.
743 *
744 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
745 * @param measurements collection of body magnetic flux density
746 * measurements with standard deviation of
747 * magnetometer measurements taken at the same
748 * position with zero velocity and unknown different
749 * orientations.
750 * @param initialHardIron initial hard-iron to find a solution.
751 * @param listener listener to handle events raised by this calibrator.
752 * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
753 * or if provided hard-iron matrix is not 3x1.
754 */
755 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
756 final Double groundTruthMagneticFluxDensityNorm,
757 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
758 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
759 super(groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
760 }
761
762 /**
763 * Constructor.
764 *
765 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
766 * @param measurements collection of body magnetic flux density
767 * measurements with standard deviation of
768 * magnetometer measurements taken at the same
769 * position with zero velocity and unknown different
770 * orientations.
771 * @param commonAxisUsed indicates whether z-axis is assumed to be common
772 * for the accelerometer, gyroscope and magnetometer.
773 * @param initialHardIron initial hard-iron to find a solution.
774 * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
775 * or if provided hard-iron matrix is not 3x1.
776 */
777 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
778 final Double groundTruthMagneticFluxDensityNorm,
779 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
780 final Matrix initialHardIron) {
781 super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
782 }
783
784 /**
785 * Constructor.
786 *
787 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
788 * @param measurements collection of body magnetic flux density
789 * measurements with standard deviation of
790 * magnetometer measurements taken at the same
791 * position with zero velocity and unknown different
792 * orientations.
793 * @param commonAxisUsed indicates whether z-axis is assumed to be common
794 * for the accelerometer, gyroscope and magnetometer.
795 * @param initialHardIron initial hard-iron to find a solution.
796 * @param listener listener to handle events raised by this calibrator.
797 * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
798 * or if provided hard-iron matrix is not 3x1.
799 */
800 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
801 final Double groundTruthMagneticFluxDensityNorm,
802 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
803 final Matrix initialHardIron,
804 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
805 super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
806 }
807
808 /**
809 * Constructor.
810 *
811 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
812 * @param measurements collection of body magnetic flux density
813 * measurements with standard deviation of
814 * magnetometer measurements taken at the same
815 * position with zero velocity and unknown different
816 * orientations.
817 * @param initialHardIron initial hard-iron to find a solution.
818 * @param initialMm initial soft-iron matrix containing scale factors
819 * and cross coupling errors.
820 * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
821 * or if provided hard-iron matrix is not 3x1 or if
822 * soft-iron matrix is not 3x3.
823 */
824 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
825 final Double groundTruthMagneticFluxDensityNorm,
826 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
827 final Matrix initialMm) {
828 super(groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, initialMm);
829 }
830
831 /**
832 * Constructor.
833 *
834 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
835 * @param measurements collection of body magnetic flux density
836 * measurements with standard deviation of
837 * magnetometer measurements taken at the same
838 * position with zero velocity and unknown different
839 * orientations.
840 * @param initialHardIron initial hard-iron to find a solution.
841 * @param initialMm initial soft-iron matrix containing scale factors
842 * and cross coupling errors.
843 * @param listener listener to handle events raised by this calibrator.
844 * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
845 * or if provided hard-iron matrix is not 3x1 or if
846 * soft-iron matrix is not 3x3.
847 */
848 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
849 final Double groundTruthMagneticFluxDensityNorm,
850 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
851 final Matrix initialMm, final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
852 super(groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, initialMm, listener);
853 }
854
855 /**
856 * Constructor.
857 *
858 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
859 * @param measurements collection of body magnetic flux density
860 * measurements with standard deviation of
861 * magnetometer measurements taken at the same
862 * position with zero velocity and unknown different
863 * orientations.
864 * @param commonAxisUsed indicates whether z-axis is assumed to be common
865 * for the accelerometer, gyroscope and magnetometer.
866 * @param initialHardIron initial hard-iron to find a solution.
867 * @param initialMm initial soft-iron matrix containing scale factors
868 * and cross coupling errors.
869 * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
870 * or if provided hard-iron matrix is not 3x1
871 * or if soft-iron matrix is not 3x3.
872 */
873 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
874 final Double groundTruthMagneticFluxDensityNorm,
875 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
876 final Matrix initialHardIron, final Matrix initialMm) {
877 super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, initialMm);
878 }
879
880 /**
881 * Constructor.
882 *
883 * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
884 * @param measurements collection of body magnetic flux density
885 * measurements with standard deviation of
886 * magnetometer measurements taken at the same
887 * position with zero velocity and unknown different
888 * orientations.
889 * @param commonAxisUsed indicates whether z-axis is assumed to be common
890 * for the accelerometer, gyroscope and magnetometer.
891 * @param initialHardIron initial hard-iron to find a solution.
892 * @param initialMm initial soft-iron matrix containing scale factors
893 * and cross coupling errors.
894 * @param listener listener to handle events raised by this calibrator.
895 * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
896 * or if provided hard-iron matrix is not 3x1
897 * or if soft-iron matrix is not 3x3.
898 */
899 public LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
900 final Double groundTruthMagneticFluxDensityNorm,
901 final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
902 final Matrix initialHardIron, final Matrix initialMm,
903 final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener) {
904 super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
905 }
906
907 /**
908 * Returns threshold to be used to keep the algorithm iterating in case that
909 * best estimated threshold using median of residuals is not small enough.
910 * Once a solution is found that generates a threshold below this value, the
911 * algorithm will stop.
912 * The stop threshold can be used to prevent the LMedS algorithm to iterate
913 * too many times in cases where samples have a very similar accuracy.
914 * For instance, in cases where proportion of outliers is very small (close
915 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
916 * iterate for a long time trying to find the best solution when indeed
917 * there is no need to do that if a reasonable threshold has already been
918 * reached.
919 * Because of this behaviour the stop threshold can be set to a value much
920 * lower than the one typically used in RANSAC, and yet the algorithm could
921 * still produce even smaller thresholds in estimated results.
922 *
923 * @return stop threshold to stop the algorithm prematurely when a certain
924 * accuracy has been reached.
925 */
926 public double getStopThreshold() {
927 return stopThreshold;
928 }
929
930 /**
931 * Sets threshold to be used to keep the algorithm iterating in case that
932 * best estimated threshold using median of residuals is not small enough.
933 * Once a solution is found that generates a threshold below this value,
934 * the algorithm will stop.
935 * The stop threshold can be used to prevent the LMedS algorithm to iterate
936 * too many times in cases where samples have a very similar accuracy.
937 * For instance, in cases where proportion of outliers is very small (close
938 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
939 * iterate for a long time trying to find the best solution when indeed
940 * there is no need to do that if a reasonable threshold has already been
941 * reached.
942 * Because of this behaviour the stop threshold can be set to a value much
943 * lower than the one typically used in RANSAC, and yet the algorithm could
944 * still produce even smaller thresholds in estimated results.
945 *
946 * @param stopThreshold stop threshold to stop the algorithm prematurely
947 * when a certain accuracy has been reached.
948 * @throws IllegalArgumentException if provided value is zero or negative.
949 * @throws LockedException if calibrator is currently running.
950 */
951 public void setStopThreshold(final double stopThreshold) throws LockedException {
952 if (running) {
953 throw new LockedException();
954 }
955 if (stopThreshold <= MIN_STOP_THRESHOLD) {
956 throw new IllegalArgumentException();
957 }
958
959 this.stopThreshold = stopThreshold;
960 }
961
962 /**
963 * Estimates magnetometer calibration parameters containing hard-iron
964 * bias and soft-iron scale factors and cross-coupling errors.
965 *
966 * @throws LockedException if calibrator is currently running.
967 * @throws NotReadyException if calibrator is not ready.
968 * @throws CalibrationException if estimation fails for numerical reasons.
969 */
970 @SuppressWarnings("DuplicatedCode")
971 @Override
972 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
973 if (running) {
974 throw new LockedException();
975 }
976 if (!isReady()) {
977 throw new NotReadyException();
978 }
979
980 final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
981 @Override
982 public int getTotalSamples() {
983 return measurements.size();
984 }
985
986 @Override
987 public int getSubsetSize() {
988 return preliminarySubsetSize;
989 }
990
991 @Override
992 public void estimatePreliminarSolutions(
993 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
994 computePreliminarySolutions(samplesIndices, solutions);
995 }
996
997 @Override
998 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
999 return computeError(measurements.get(i), currentEstimation);
1000 }
1001
1002 @Override
1003 public boolean isReady() {
1004 return LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.this.isReady();
1005 }
1006
1007 @Override
1008 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
1009 // no action needed
1010 }
1011
1012 @Override
1013 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
1014 // no action needed
1015 }
1016
1017 @Override
1018 public void onEstimateNextIteration(
1019 final RobustEstimator<PreliminaryResult> estimator, final int iteration) {
1020 if (listener != null) {
1021 listener.onCalibrateNextIteration(
1022 LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.this, iteration);
1023 }
1024 }
1025
1026 @Override
1027 public void onEstimateProgressChange(
1028 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
1029 if (listener != null) {
1030 listener.onCalibrateProgressChange(
1031 LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.this, progress);
1032 }
1033 }
1034 });
1035
1036 try {
1037 running = true;
1038
1039 if (listener != null) {
1040 listener.onCalibrateStart(this);
1041 }
1042
1043 inliersData = null;
1044
1045 innerEstimator.setConfidence(confidence);
1046 innerEstimator.setMaxIterations(maxIterations);
1047 innerEstimator.setProgressDelta(progressDelta);
1048 innerEstimator.setStopThreshold(stopThreshold);
1049 final var preliminaryResult = innerEstimator.estimate();
1050 inliersData = innerEstimator.getInliersData();
1051
1052 attemptRefine(preliminaryResult);
1053
1054 if (listener != null) {
1055 listener.onCalibrateEnd(this);
1056 }
1057
1058 } catch (final com.irurueta.numerical.LockedException e) {
1059 throw new LockedException(e);
1060 } catch (final com.irurueta.numerical.NotReadyException e) {
1061 throw new NotReadyException(e);
1062 } catch (final RobustEstimatorException e) {
1063 throw new CalibrationException(e);
1064 } finally {
1065 running = false;
1066 }
1067
1068 }
1069
1070 /**
1071 * Returns method being used for robust estimation.
1072 *
1073 * @return method being used for robust estimation.
1074 */
1075 @Override
1076 public RobustEstimatorMethod getMethod() {
1077 return RobustEstimatorMethod.LMEDS;
1078 }
1079
1080 /**
1081 * Indicates whether this calibrator requires quality scores for each
1082 * measurement or not.
1083 *
1084 * @return true if quality scores are required, false otherwise.
1085 */
1086 @Override
1087 public boolean isQualityScoresRequired() {
1088 return false;
1089 }
1090 }