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