1 /*
2 * Copyright (C) 2020 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.navigation.LockedException;
19 import com.irurueta.navigation.NotReadyException;
20 import com.irurueta.navigation.inertial.calibration.CalibrationException;
21 import com.irurueta.navigation.inertial.calibration.StandardDeviationFrameBodyMagneticFluxDensity;
22 import com.irurueta.numerical.robust.PROMedSRobustEstimator;
23 import com.irurueta.numerical.robust.PROMedSRobustEstimatorListener;
24 import com.irurueta.numerical.robust.RobustEstimator;
25 import com.irurueta.numerical.robust.RobustEstimatorException;
26 import com.irurueta.numerical.robust.RobustEstimatorMethod;
27
28 import java.io.IOException;
29 import java.util.List;
30
31 /**
32 * Robustly estimates magnetometer hard-iron biases, soft-iron cross
33 * couplings and scaling factors using PROMedS algorithm.
34 * <p>
35 * To use this calibrator at least 4 measurements at different known
36 * frames must be provided. In other words, magnetometer samples must
37 * be obtained at 4 different positions or orientations.
38 * Notice that frame velocities are ignored by this calibrator.
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 */
55 public class PROMedSRobustKnownFrameMagnetometerCalibrator extends RobustKnownFrameMagnetometerCalibrator {
56
57 /**
58 * Default value to be used for stop threshold. Stop threshold can be used to
59 * avoid keeping the algorithm unnecessarily iterating in case that best
60 * estimated threshold using median of residuals is not small enough. Once a
61 * solution is found that generates a threshold below this value, the
62 * algorithm will stop.
63 * The stop threshold can be used to prevent the LMedS algorithm iterating
64 * too many times in cases where samples have a very similar accuracy.
65 * For instance, in cases where proportion of outliers is very small (close
66 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
67 * iterate for a long time trying to find the best solution when indeed
68 * there is no need to do that if a reasonable threshold has already been
69 * reached.
70 * Because of this behaviour the stop threshold can be set to a value much
71 * lower than the one typically used in RANSAC, and yet the algorithm could
72 * still produce even smaller thresholds in estimated results.
73 */
74 public static final double DEFAULT_STOP_THRESHOLD = 500e-9;
75
76 /**
77 * Minimum allowed stop threshold value.
78 */
79 public static final double MIN_STOP_THRESHOLD = 0.0;
80
81 /**
82 * Threshold to be used to keep the algorithm iterating in case that best
83 * estimated threshold using median of residuals is not small enough. Once
84 * a solution is found that generates a threshold below this value, the
85 * algorithm will stop.
86 * The stop threshold can be used to prevent the LMedS algorithm iterating
87 * too many times in cases where samples have a very similar accuracy.
88 * For instance, in cases where proportion of outliers is very small (close
89 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
90 * iterate for a long time trying to find the best solution when indeed
91 * there is no need to do that if a reasonable threshold has already been
92 * reached.
93 * Because of this behaviour the stop threshold can be set to a value much
94 * lower than the one typically used in RANSAC, and yet the algorithm could
95 * still produce even smaller thresholds in estimated results.
96 */
97 private double stopThreshold = DEFAULT_STOP_THRESHOLD;
98
99 /**
100 * Quality scores corresponding to each provided sample.
101 * The larger the score value the better the quality of the sample.
102 */
103 private double[] qualityScores;
104
105 /**
106 * Constructor.
107 */
108 public PROMedSRobustKnownFrameMagnetometerCalibrator() {
109 super();
110 }
111
112 /**
113 * Constructor.
114 *
115 * @param listener listener to be notified of events such as when estimation
116 * starts, ends or its progress significantly changes.
117 */
118 public PROMedSRobustKnownFrameMagnetometerCalibrator(
119 final RobustKnownFrameMagnetometerCalibratorListener listener) {
120 super(listener);
121 }
122
123 /**
124 * Constructor.
125 *
126 * @param measurements list of body magnetic flux density measurements with standard
127 * deviations taken at different frames (positions and
128 * orientations).
129 */
130 public PROMedSRobustKnownFrameMagnetometerCalibrator(
131 final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements) {
132 super(measurements);
133 }
134
135 /**
136 * Constructor.
137 *
138 * @param measurements list of body magnetic flux density measurements with standard
139 * deviations taken at different frames (positions and
140 * orientations).
141 * @param listener listener to handle events raised by this calibrator.
142 */
143 public PROMedSRobustKnownFrameMagnetometerCalibrator(
144 final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements,
145 final RobustKnownFrameMagnetometerCalibratorListener listener) {
146 super(measurements, listener);
147 }
148
149 /**
150 * Constructor.
151 *
152 * @param commonAxisUsed indicates whether z-axis is assumed to be common
153 * for the accelerometer, gyroscope and magnetometer.
154 */
155 public PROMedSRobustKnownFrameMagnetometerCalibrator(final boolean commonAxisUsed) {
156 super(commonAxisUsed);
157 }
158
159 /**
160 * Constructor.
161 *
162 * @param commonAxisUsed indicates whether z-axis is assumed to be common
163 * for the accelerometer, gyroscope and magnetometer.
164 * @param listener listener to handle events raised by this calibrator.
165 */
166 public PROMedSRobustKnownFrameMagnetometerCalibrator(
167 final boolean commonAxisUsed, final RobustKnownFrameMagnetometerCalibratorListener listener) {
168 super(commonAxisUsed, listener);
169 }
170
171 /**
172 * Constructor.
173 *
174 * @param measurements list of body magnetic flux density measurements with standard
175 * deviations taken at different frames (positions and
176 * orientations).
177 * @param commonAxisUsed indicates whether z-axis is assumed to be common
178 * for the accelerometer, gyroscope and magnetometer.
179 */
180 public PROMedSRobustKnownFrameMagnetometerCalibrator(
181 final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed) {
182 super(measurements, commonAxisUsed);
183 }
184
185 /**
186 * Constructor.
187 *
188 * @param measurements list of body magnetic flux density measurements with standard
189 * deviations taken at different frames (positions and
190 * orientations).
191 * @param commonAxisUsed indicates whether z-axis is assumed to be common
192 * for the accelerometer, gyroscope and magnetometer.
193 * @param listener listener to handle events raised by this calibrator.
194 */
195 public PROMedSRobustKnownFrameMagnetometerCalibrator(
196 final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
197 final RobustKnownFrameMagnetometerCalibratorListener listener) {
198 super(measurements, commonAxisUsed, listener);
199 }
200
201 /**
202 * Constructor.
203 *
204 * @param qualityScores quality scores corresponding to each provided
205 * measurement. The larger the score value the better
206 * the quality of the sample.
207 * @throws IllegalArgumentException if provided quality scores length
208 * is smaller than 4 samples.
209 */
210 public PROMedSRobustKnownFrameMagnetometerCalibrator(final double[] qualityScores) {
211 super();
212 internalSetQualityScores(qualityScores);
213 }
214
215 /**
216 * Constructor.
217 *
218 * @param qualityScores quality scores corresponding to each provided
219 * measurement. The larger the score value the better
220 * the quality of the sample.
221 * @param listener listener to be notified of events such as when estimation
222 * starts, ends or its progress significantly changes.
223 * @throws IllegalArgumentException if provided quality scores length
224 * is smaller than 4 samples.
225 */
226 public PROMedSRobustKnownFrameMagnetometerCalibrator(
227 final double[] qualityScores, final RobustKnownFrameMagnetometerCalibratorListener listener) {
228 super(listener);
229 internalSetQualityScores(qualityScores);
230 }
231
232 /**
233 * Constructor.
234 *
235 * @param qualityScores quality scores corresponding to each provided
236 * measurement. The larger the score value the better
237 * the quality of the sample.
238 * @param measurements list of body magnetic flux density measurements with standard
239 * deviations taken at different frames (positions and
240 * orientations).
241 * @throws IllegalArgumentException if provided quality scores length
242 * is smaller than 4 samples.
243 */
244 public PROMedSRobustKnownFrameMagnetometerCalibrator(
245 final double[] qualityScores, final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements) {
246 super(measurements);
247 internalSetQualityScores(qualityScores);
248 }
249
250 /**
251 * Constructor.
252 *
253 * @param qualityScores quality scores corresponding to each provided
254 * measurement. The larger the score value the better
255 * the quality of the sample.
256 * @param measurements list of body magnetic flux density measurements with standard
257 * deviations taken at different frames (positions and
258 * orientations).
259 * @param listener listener to handle events raised by this calibrator.
260 * @throws IllegalArgumentException if provided quality scores length
261 * is smaller than 4 samples.
262 */
263 public PROMedSRobustKnownFrameMagnetometerCalibrator(
264 final double[] qualityScores, final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements,
265 final RobustKnownFrameMagnetometerCalibratorListener listener) {
266 super(measurements, listener);
267 internalSetQualityScores(qualityScores);
268 }
269
270 /**
271 * Constructor.
272 *
273 * @param qualityScores quality scores corresponding to each provided
274 * measurement. The larger the score value the better
275 * the quality of the sample.
276 * @param commonAxisUsed indicates whether z-axis is assumed to be common
277 * for the accelerometer, gyroscope and magnetometer.
278 * @throws IllegalArgumentException if provided quality scores length
279 * is smaller than 4 samples.
280 */
281 public PROMedSRobustKnownFrameMagnetometerCalibrator(final double[] qualityScores, final boolean commonAxisUsed) {
282 super(commonAxisUsed);
283 internalSetQualityScores(qualityScores);
284 }
285
286 /**
287 * Constructor.
288 *
289 * @param qualityScores quality scores corresponding to each provided
290 * measurement. The larger the score value the better
291 * the quality of the sample.
292 * @param commonAxisUsed indicates whether z-axis is assumed to be common
293 * for the accelerometer, gyroscope and magnetometer.
294 * @param listener listener to handle events raised by this calibrator.
295 * @throws IllegalArgumentException if provided quality scores length
296 * is smaller than 4 samples.
297 */
298 public PROMedSRobustKnownFrameMagnetometerCalibrator(
299 final double[] qualityScores, final boolean commonAxisUsed,
300 final RobustKnownFrameMagnetometerCalibratorListener listener) {
301 super(commonAxisUsed, listener);
302 internalSetQualityScores(qualityScores);
303 }
304
305 /**
306 * Constructor.
307 *
308 * @param qualityScores quality scores corresponding to each provided
309 * measurement. The larger the score value the better
310 * the quality of the sample.
311 * @param measurements list of body magnetic flux density measurements with standard
312 * deviations taken at different frames (positions and
313 * orientations).
314 * @param commonAxisUsed indicates whether z-axis is assumed to be common
315 * for the accelerometer, gyroscope and magnetometer.
316 * @throws IllegalArgumentException if provided quality scores length
317 * is smaller than 4 samples.
318 */
319 public PROMedSRobustKnownFrameMagnetometerCalibrator(
320 final double[] qualityScores, final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements,
321 final boolean commonAxisUsed) {
322 super(measurements, commonAxisUsed);
323 internalSetQualityScores(qualityScores);
324 }
325
326 /**
327 * Constructor.
328 *
329 * @param qualityScores quality scores corresponding to each provided
330 * measurement. The larger the score value the better
331 * the quality of the sample.
332 * @param measurements list of body magnetic flux density measurements with standard
333 * deviations taken at different frames (positions and
334 * orientations).
335 * @param commonAxisUsed indicates whether z-axis is assumed to be common
336 * for the accelerometer, gyroscope and magnetometer.
337 * @param listener listener to handle events raised by this calibrator.
338 * @throws IllegalArgumentException if provided quality scores length
339 * is smaller than 4 samples.
340 */
341 public PROMedSRobustKnownFrameMagnetometerCalibrator(
342 final double[] qualityScores, final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements,
343 final boolean commonAxisUsed, final RobustKnownFrameMagnetometerCalibratorListener listener) {
344 super(measurements, commonAxisUsed, listener);
345 internalSetQualityScores(qualityScores);
346 }
347
348 /**
349 * Returns threshold to be used to keep the algorithm iterating in case that
350 * best estimated threshold using median of residuals is not small enough.
351 * Once a solution is found that generates a threshold below this value, the
352 * algorithm will stop.
353 * The stop threshold can be used to prevent the LMedS algorithm to iterate
354 * too many times in cases where samples have a very similar accuracy.
355 * For instance, in cases where proportion of outliers is very small (close
356 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
357 * iterate for a long time trying to find the best solution when indeed
358 * there is no need to do that if a reasonable threshold has already been
359 * reached.
360 * Because of this behaviour the stop threshold can be set to a value much
361 * lower than the one typically used in RANSAC, and yet the algorithm could
362 * still produce even smaller thresholds in estimated results.
363 *
364 * @return stop threshold to stop the algorithm prematurely when a certain
365 * accuracy has been reached.
366 */
367 public double getStopThreshold() {
368 return stopThreshold;
369 }
370
371 /**
372 * Sets threshold to be used to keep the algorithm iterating in case that
373 * best estimated threshold using median of residuals is not small enough.
374 * Once a solution is found that generates a threshold below this value,
375 * the algorithm will stop.
376 * The stop threshold can be used to prevent the LMedS algorithm to iterate
377 * too many times in cases where samples have a very similar accuracy.
378 * For instance, in cases where proportion of outliers is very small (close
379 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
380 * iterate for a long time trying to find the best solution when indeed
381 * there is no need to do that if a reasonable threshold has already been
382 * reached.
383 * Because of this behaviour the stop threshold can be set to a value much
384 * lower than the one typically used in RANSAC, and yet the algorithm could
385 * still produce even smaller thresholds in estimated results.
386 *
387 * @param stopThreshold stop threshold to stop the algorithm prematurely
388 * when a certain accuracy has been reached.
389 * @throws IllegalArgumentException if provided value is zero or negative.
390 * @throws LockedException if calibrator is currently running.
391 */
392 public void setStopThreshold(final double stopThreshold) throws LockedException {
393 if (running) {
394 throw new LockedException();
395 }
396 if (stopThreshold <= MIN_STOP_THRESHOLD) {
397 throw new IllegalArgumentException();
398 }
399
400 this.stopThreshold = stopThreshold;
401 }
402
403 /**
404 * Returns quality scores corresponding to each provided sample.
405 * The larger the score value the better the quality of the sample.
406 *
407 * @return quality scores corresponding to each sample.
408 */
409 @Override
410 public double[] getQualityScores() {
411 return qualityScores;
412 }
413
414 /**
415 * Sets quality scores corresponding to each provided sample.
416 * The larger the score value the better the quality of the sample.
417 *
418 * @param qualityScores quality scores corresponding to each sample.
419 * @throws IllegalArgumentException if provided quality scores length
420 * is smaller than minimum required samples.
421 * @throws LockedException if calibrator is currently running.
422 */
423 @Override
424 public void setQualityScores(final double[] qualityScores) throws LockedException {
425 if (running) {
426 throw new LockedException();
427 }
428 internalSetQualityScores(qualityScores);
429 }
430
431 /**
432 * Indicates whether solver is ready to find a solution.
433 *
434 * @return true if solver is ready, false otherwise.
435 */
436 @Override
437 public boolean isReady() {
438 return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
439 }
440
441 /**
442 * Estimates magnetometer calibration parameters containing hard-iron
443 * bias and soft-iron scale factors and cross-coupling errors.
444 *
445 * @throws LockedException if calibrator is currently running.
446 * @throws NotReadyException if calibrator is not ready.
447 * @throws CalibrationException if estimation fails for numerical reasons.
448 */
449 @SuppressWarnings("DuplicatedCode")
450 @Override
451 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
452 if (running) {
453 throw new LockedException();
454 }
455 if (!isReady()) {
456 throw new NotReadyException();
457 }
458
459 final var innerEstimator = new PROMedSRobustEstimator<>(
460 new PROMedSRobustEstimatorListener<PreliminaryResult>() {
461 @Override
462 public double[] getQualityScores() {
463 return qualityScores;
464 }
465
466 @Override
467 public double getThreshold() {
468 return stopThreshold;
469 }
470
471 @Override
472 public int getTotalSamples() {
473 return measurements.size();
474 }
475
476 @Override
477 public int getSubsetSize() {
478 return preliminarySubsetSize;
479 }
480
481 @Override
482 public void estimatePreliminarSolutions(
483 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
484 computePreliminarySolutions(samplesIndices, solutions);
485 }
486
487 @Override
488 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
489 return computeError(measurements.get(i), currentEstimation);
490 }
491
492 @Override
493 public boolean isReady() {
494 return PROMedSRobustKnownFrameMagnetometerCalibrator.this.isReady();
495 }
496
497 @Override
498 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
499 // no action needed
500 }
501
502 @Override
503 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
504 // no action needed
505 }
506
507 @Override
508 public void onEstimateNextIteration(
509 final RobustEstimator<PreliminaryResult> estimator, final int iteration) {
510 if (listener != null) {
511 listener.onCalibrateNextIteration(
512 PROMedSRobustKnownFrameMagnetometerCalibrator.this, iteration);
513 }
514 }
515
516 @Override
517 public void onEstimateProgressChange(
518 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
519 if (listener != null) {
520 listener.onCalibrateProgressChange(
521 PROMedSRobustKnownFrameMagnetometerCalibrator.this, progress);
522 }
523 }
524 });
525
526 try {
527 running = true;
528
529 if (listener != null) {
530 listener.onCalibrateStart(this);
531 }
532
533 inliersData = null;
534
535 setupWmmEstimator();
536
537 innerEstimator.setUseInlierThresholds(true);
538 innerEstimator.setConfidence(confidence);
539 innerEstimator.setMaxIterations(maxIterations);
540 innerEstimator.setProgressDelta(progressDelta);
541 final var preliminaryResult = innerEstimator.estimate();
542 inliersData = innerEstimator.getInliersData();
543
544 attemptRefine(preliminaryResult);
545
546 if (listener != null) {
547 listener.onCalibrateEnd(this);
548 }
549
550 } catch (final com.irurueta.numerical.LockedException e) {
551 throw new LockedException(e);
552 } catch (final com.irurueta.numerical.NotReadyException e) {
553 throw new NotReadyException(e);
554 } catch (final RobustEstimatorException | IOException e) {
555 throw new CalibrationException(e);
556 } finally {
557 running = false;
558 }
559 }
560
561 /**
562 * Returns method being used for robust estimation.
563 *
564 * @return method being used for robust estimation.
565 */
566 @Override
567 public RobustEstimatorMethod getMethod() {
568 return RobustEstimatorMethod.PROMEDS;
569 }
570
571 /**
572 * Indicates whether this calibrator requires quality scores for each
573 * measurement or not.
574 *
575 * @return true if quality scores are required, false otherwise.
576 */
577 @Override
578 public boolean isQualityScoresRequired() {
579 return true;
580 }
581
582 /**
583 * Sets quality scores corresponding to each provided sample.
584 * This method is used internally and does not check whether instance is
585 * locked or not.
586 *
587 * @param qualityScores quality scores to be set.
588 * @throws IllegalArgumentException if provided quality scores length
589 * is smaller than 4 samples.
590 */
591 private void internalSetQualityScores(final double[] qualityScores) {
592 if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
593 throw new IllegalArgumentException();
594 }
595
596 this.qualityScores = qualityScores;
597 }
598 }