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.gyroscope;
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 gyroscope biases, cross couplings and scaling factors
32 * along with G-dependent cross biases introduced on the gyroscope by the
33 * specific forces sensed by the accelerometer using a PROMedS algorithm to discard
34 * outliers.
35 * <p>
36 * To use this calibrator at least 7 measurements at different known frames must
37 * be provided. In other words, accelerometer and gyroscope (i.e. body kinematics)
38 * samples must be obtained at 7 different positions, orientations and velocities
39 * (although typically velocities are always zero).
40 * <p>
41 * Measured gyroscope angular rates is assumed to follow the model shown below:
42 * <pre>
43 * Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
44 * </pre>
45 * Where:
46 * - Ωmeas is the measured gyroscope angular rates. This is a 3x1 vector.
47 * - bg is the gyroscope bias. Ideally, on a perfect gyroscope, this should be a
48 * 3x1 zero vector.
49 * - I is the 3x3 identity matrix.
50 * - Mg is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
51 * a perfect gyroscope, this should be a 3x3 zero matrix.
52 * - Ωtrue is ground-truth gyroscope angular rates.
53 * - Gg is the G-dependent cross biases introduced by the specific forces sensed
54 * by the accelerometer. Ideally, on a perfect gyroscope, this should be a 3x3
55 * zero matrix.
56 * - ftrue is ground-truth specific force. This is a 3x1 vector.
57 * - w is measurement noise. This is a 3x1 vector.
58 */
59 public class PROMedSRobustKnownFrameGyroscopeCalibrator extends RobustKnownFrameGyroscopeCalibrator {
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 RANSAC, and yet the algorithm could
76 * still produce even smaller thresholds in estimated results.
77 */
78 public static final double DEFAULT_STOP_THRESHOLD = 5e-4;
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 RANSAC, 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 * Quality scores corresponding to each provided sample.
105 * The larger the score value the better the quality of the sample.
106 */
107 private double[] qualityScores;
108
109 /**
110 * Constructor.
111 */
112 public PROMedSRobustKnownFrameGyroscopeCalibrator() {
113 }
114
115 /**
116 * Constructor.
117 *
118 * @param listener listener to be notified of events such as when estimation
119 * starts, ends or its progress significantly changes.
120 */
121 public PROMedSRobustKnownFrameGyroscopeCalibrator(final RobustKnownFrameGyroscopeCalibratorListener listener) {
122 super(listener);
123 }
124
125 /**
126 * Constructor.
127 *
128 * @param measurements list of body kinematics measurements with standard
129 * deviations taken at different frames (positions, orientations
130 * and velocities).
131 */
132 public PROMedSRobustKnownFrameGyroscopeCalibrator(final List<StandardDeviationFrameBodyKinematics> measurements) {
133 super(measurements);
134 }
135
136 /**
137 * Constructor.
138 *
139 * @param measurements list of body kinematics measurements with standard
140 * deviations taken at different frames (positions, orientations
141 * and velocities).
142 * @param listener listener to handle events raised by this calibrator.
143 */
144 public PROMedSRobustKnownFrameGyroscopeCalibrator(
145 final List<StandardDeviationFrameBodyKinematics> measurements,
146 final RobustKnownFrameGyroscopeCalibratorListener listener) {
147 super(measurements, listener);
148 }
149
150 /**
151 * Constructor.
152 *
153 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
154 * accelerometer and gyroscope.
155 */
156 public PROMedSRobustKnownFrameGyroscopeCalibrator(final boolean commonAxisUsed) {
157 super(commonAxisUsed);
158 }
159
160 /**
161 * Constructor.
162 *
163 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
164 * accelerometer and gyroscope.
165 * @param listener listener to handle events raised by this calibrator.
166 */
167 public PROMedSRobustKnownFrameGyroscopeCalibrator(
168 final boolean commonAxisUsed, final RobustKnownFrameGyroscopeCalibratorListener listener) {
169 super(commonAxisUsed, listener);
170 }
171
172 /**
173 * Constructor.
174 *
175 * @param measurements list of body kinematics measurements with standard
176 * deviations taken at different frames (positions, orientations
177 * and velocities).
178 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
179 * accelerometer and gyroscope.
180 */
181 public PROMedSRobustKnownFrameGyroscopeCalibrator(
182 final List<StandardDeviationFrameBodyKinematics> measurements, final boolean commonAxisUsed) {
183 super(measurements, commonAxisUsed);
184 }
185
186 /**
187 * Constructor.
188 *
189 * @param measurements list of body kinematics measurements with standard
190 * deviations taken at different frames (positions, orientations
191 * and velocities).
192 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
193 * accelerometer and gyroscope.
194 * @param listener listener to handle events raised by this calibrator.
195 */
196 public PROMedSRobustKnownFrameGyroscopeCalibrator(
197 final List<StandardDeviationFrameBodyKinematics> measurements, final boolean commonAxisUsed,
198 final RobustKnownFrameGyroscopeCalibratorListener listener) {
199 super(measurements, commonAxisUsed, listener);
200 }
201
202 /**
203 * Constructor.
204 *
205 * @param qualityScores quality scores corresponding to each provided
206 * measurement. The larger the score value the better
207 * the quality of the sample.
208 * @throws IllegalArgumentException if provided quality scores length
209 * is smaller than 7 samples.
210 */
211 public PROMedSRobustKnownFrameGyroscopeCalibrator(final double[] qualityScores) {
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 7 samples.
225 */
226 public PROMedSRobustKnownFrameGyroscopeCalibrator(
227 final double[] qualityScores, final RobustKnownFrameGyroscopeCalibratorListener 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 kinematics measurements with standard
239 * deviations taken at different frames (positions, orientations
240 * and velocities).
241 * @throws IllegalArgumentException if provided quality scores length
242 * is smaller than 7 samples.
243 */
244 public PROMedSRobustKnownFrameGyroscopeCalibrator(
245 final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> 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 kinematics measurements with standard
257 * deviations taken at different frames (positions, orientations
258 * and velocities).
259 * @param listener listener to handle events raised by this calibrator.
260 * @throws IllegalArgumentException if provided quality scores length
261 * is smaller than 7 samples.
262 */
263 public PROMedSRobustKnownFrameGyroscopeCalibrator(
264 final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
265 final RobustKnownFrameGyroscopeCalibratorListener 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 for
277 * accelerometer and gyroscope.
278 * @throws IllegalArgumentException if provided quality scores length
279 * is smaller than 7 samples.
280 */
281 public PROMedSRobustKnownFrameGyroscopeCalibrator(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 for
293 * accelerometer and gyroscope.
294 * @param listener listener to handle events raised by this calibrator.
295 * @throws IllegalArgumentException if provided quality scores length
296 * is smaller than 7 samples.
297 */
298 public PROMedSRobustKnownFrameGyroscopeCalibrator(
299 final double[] qualityScores, final boolean commonAxisUsed,
300 final RobustKnownFrameGyroscopeCalibratorListener 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 kinematics measurements with standard
312 * deviations taken at different frames (positions, orientations
313 * and velocities).
314 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
315 * accelerometer and gyroscope.
316 * @throws IllegalArgumentException if provided quality scores length
317 * is smaller than 7 samples.
318 */
319 public PROMedSRobustKnownFrameGyroscopeCalibrator(
320 final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> 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 kinematics measurements with standard
333 * deviations taken at different frames (positions, orientations
334 * and velocities).
335 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
336 * accelerometer and gyroscope.
337 * @param listener listener to handle events raised by this calibrator.
338 * @throws IllegalArgumentException if provided quality scores length
339 * is smaller than 7 samples.
340 */
341 public PROMedSRobustKnownFrameGyroscopeCalibrator(
342 final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
343 final boolean commonAxisUsed, final RobustKnownFrameGyroscopeCalibratorListener 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 accelerometer calibration parameters containing bias, scale factors
443 * 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 PROMedSRobustKnownFrameGyroscopeCalibrator.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 PROMedSRobustKnownFrameGyroscopeCalibrator.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 PROMedSRobustKnownFrameGyroscopeCalibrator.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 innerEstimator.setUseInlierThresholds(true);
535 innerEstimator.setConfidence(confidence);
536 innerEstimator.setMaxIterations(maxIterations);
537 innerEstimator.setProgressDelta(progressDelta);
538 final var preliminaryResult = innerEstimator.estimate();
539 inliersData = innerEstimator.getInliersData();
540
541 attemptRefine(preliminaryResult);
542
543 if (listener != null) {
544 listener.onCalibrateEnd(this);
545 }
546
547 } catch (final com.irurueta.numerical.LockedException e) {
548 throw new LockedException(e);
549 } catch (final com.irurueta.numerical.NotReadyException e) {
550 throw new NotReadyException(e);
551 } catch (final RobustEstimatorException e) {
552 throw new CalibrationException(e);
553 } finally {
554 running = false;
555 }
556 }
557
558 /**
559 * Returns method being used for robust estimation.
560 *
561 * @return method being used for robust estimation.
562 */
563 @Override
564 public RobustEstimatorMethod getMethod() {
565 return RobustEstimatorMethod.PROMEDS;
566 }
567
568 /**
569 * Indicates whether this calibrator requires quality scores for each
570 * measurement/sequence or not.
571 *
572 * @return true if quality scores are required, false otherwise.
573 */
574 @Override
575 public boolean isQualityScoresRequired() {
576 return true;
577 }
578
579 /**
580 * Sets quality scores corresponding to each provided sample.
581 * This method is used internally and does not check whether instance is
582 * locked or not.
583 *
584 * @param qualityScores quality scores to be set.
585 * @throws IllegalArgumentException if provided quality scores length
586 * is smaller than 3 samples.
587 */
588 private void internalSetQualityScores(final double[] qualityScores) {
589 if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
590 throw new IllegalArgumentException();
591 }
592
593 this.qualityScores = qualityScores;
594 }
595 }