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.PROSACRobustEstimator;
23 import com.irurueta.numerical.robust.PROSACRobustEstimatorListener;
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 PROSAC 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 PROSACRobustKnownFrameGyroscopeCalibrator extends RobustKnownFrameGyroscopeCalibrator {
60
61 /**
62 * Constant defining default threshold to determine whether samples are inliers or not.
63 */
64 public static final double DEFAULT_THRESHOLD = 5e-4;
65
66 /**
67 * Minimum value that can be set as threshold.
68 * Threshold must be strictly greater than 0.0.
69 */
70 public static final double MIN_THRESHOLD = 0.0;
71
72 /**
73 * Indicates that by default inliers will only be computed but not kept.
74 */
75 public static final boolean DEFAULT_COMPUTE_AND_KEEP_INLIERS = false;
76
77 /**
78 * Indicates that by default residuals will only be computed but not kept.
79 */
80 public static final boolean DEFAULT_COMPUTE_AND_KEEP_RESIDUALS = false;
81
82 /**
83 * Threshold to determine whether samples are inliers or not when testing possible solutions.
84 * The threshold refers to the amount of error on distance between estimated position and
85 * distances provided for each sample.
86 */
87 private double threshold = DEFAULT_THRESHOLD;
88
89 /**
90 * Indicates whether inliers must be computed and kept.
91 */
92 private boolean computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
93
94 /**
95 * Indicates whether residuals must be computed and kept.
96 */
97 private boolean computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
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 PROSACRobustKnownFrameGyroscopeCalibrator() {
109 }
110
111 /**
112 * Constructor.
113 *
114 * @param listener listener to be notified of events such as when estimation
115 * starts, ends or its progress significantly changes.
116 */
117 public PROSACRobustKnownFrameGyroscopeCalibrator(final RobustKnownFrameGyroscopeCalibratorListener listener) {
118 super(listener);
119 }
120
121 /**
122 * Constructor.
123 *
124 * @param measurements list of body kinematics measurements with standard
125 * deviations taken at different frames (positions, orientations
126 * and velocities).
127 */
128 public PROSACRobustKnownFrameGyroscopeCalibrator(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 PROSACRobustKnownFrameGyroscopeCalibrator(
141 final List<StandardDeviationFrameBodyKinematics> measurements,
142 final RobustKnownFrameGyroscopeCalibratorListener 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 PROSACRobustKnownFrameGyroscopeCalibrator(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 PROSACRobustKnownFrameGyroscopeCalibrator(
164 final boolean commonAxisUsed, final RobustKnownFrameGyroscopeCalibratorListener 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 PROSACRobustKnownFrameGyroscopeCalibrator(
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 PROSACRobustKnownFrameGyroscopeCalibrator(
193 final List<StandardDeviationFrameBodyKinematics> measurements, final boolean commonAxisUsed,
194 final RobustKnownFrameGyroscopeCalibratorListener 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 7 samples.
206 */
207 public PROSACRobustKnownFrameGyroscopeCalibrator(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 7 samples.
221 */
222 public PROSACRobustKnownFrameGyroscopeCalibrator(
223 final double[] qualityScores, final RobustKnownFrameGyroscopeCalibratorListener 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 7 samples.
239 */
240 public PROSACRobustKnownFrameGyroscopeCalibrator(
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 7 samples.
258 */
259 public PROSACRobustKnownFrameGyroscopeCalibrator(
260 final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
261 final RobustKnownFrameGyroscopeCalibratorListener 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 7 samples.
276 */
277 public PROSACRobustKnownFrameGyroscopeCalibrator(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 7 samples.
293 */
294 public PROSACRobustKnownFrameGyroscopeCalibrator(
295 final double[] qualityScores, final boolean commonAxisUsed,
296 final RobustKnownFrameGyroscopeCalibratorListener 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 7 samples.
314 */
315 public PROSACRobustKnownFrameGyroscopeCalibrator(
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 7 samples.
336 */
337 public PROSACRobustKnownFrameGyroscopeCalibrator(
338 final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
339 final boolean commonAxisUsed, final RobustKnownFrameGyroscopeCalibratorListener listener) {
340 super(measurements, commonAxisUsed, listener);
341 internalSetQualityScores(qualityScores);
342 }
343
344 /**
345 * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
346 * The threshold refers to the amount of error on norm between measured angular rates and the
347 * ones generated with estimated calibration parameters provided for each sample.
348 *
349 * @return threshold to determine whether samples are inliers or not.
350 */
351 public double getThreshold() {
352 return threshold;
353 }
354
355 /**
356 * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
357 * The threshold refers to the amount of error on norm between measured angular rates and the
358 * ones generated with estimated calibration parameters provided for each sample.
359 *
360 * @param threshold threshold to determine whether samples are inliers or not.
361 * @throws IllegalArgumentException if provided value is equal or less than zero.
362 * @throws LockedException if calibrator is currently running.
363 */
364 public void setThreshold(final double threshold) throws LockedException {
365 if (running) {
366 throw new LockedException();
367 }
368 if (threshold <= MIN_THRESHOLD) {
369 throw new IllegalArgumentException();
370 }
371 this.threshold = threshold;
372 }
373
374 /**
375 * Returns quality scores corresponding to each provided sample.
376 * The larger the score value the better the quality of the sample.
377 *
378 * @return quality scores corresponding to each sample.
379 */
380 @Override
381 public double[] getQualityScores() {
382 return qualityScores;
383 }
384
385 /**
386 * Sets quality scores corresponding to each provided sample.
387 * The larger the score value the better the quality of the sample.
388 *
389 * @param qualityScores quality scores corresponding to each sample.
390 * @throws IllegalArgumentException if provided quality scores length
391 * is smaller than minimum required samples.
392 * @throws LockedException if calibrator is currently running.
393 */
394 @Override
395 public void setQualityScores(final double[] qualityScores) throws LockedException {
396 if (running) {
397 throw new LockedException();
398 }
399 internalSetQualityScores(qualityScores);
400 }
401
402 /**
403 * Indicates whether calibrator is ready to find a solution.
404 *
405 * @return true if calibrator is ready, false otherwise.
406 */
407 @Override
408 public boolean isReady() {
409 return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
410 }
411
412 /**
413 * Indicates whether inliers must be computed and kept.
414 *
415 * @return true if inliers must be computed and kept, false if inliers
416 * only need to be computed but not kept.
417 */
418 public boolean isComputeAndKeepInliersEnabled() {
419 return computeAndKeepInliers;
420 }
421
422 /**
423 * Specifies whether inliers must be computed and kept.
424 *
425 * @param computeAndKeepInliers true if inliers must be computed and kept,
426 * false if inliers only need to be computed but not kept.
427 * @throws LockedException if calibrator is currently running.
428 */
429 public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
430 if (running) {
431 throw new LockedException();
432 }
433 this.computeAndKeepInliers = computeAndKeepInliers;
434 }
435
436 /**
437 * Indicates whether residuals must be computed and kept.
438 *
439 * @return true if residuals must be computed and kept, false if residuals
440 * only need to be computed but not kept.
441 */
442 public boolean isComputeAndKeepResiduals() {
443 return computeAndKeepResiduals;
444 }
445
446 /**
447 * Specifies whether residuals must be computed and kept.
448 *
449 * @param computeAndKeepResiduals true if residuals must be computed and kept,
450 * false if residuals only need to be computed but not kept.
451 * @throws LockedException if calibrator is currently running.
452 */
453 public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
454 if (running) {
455 throw new LockedException();
456 }
457 this.computeAndKeepResiduals = computeAndKeepResiduals;
458 }
459
460 /**
461 * Estimates gyroscope calibration parameters containing bias, scale factors
462 * cross-coupling errors and g-dependant cross biases.
463 *
464 * @throws LockedException if calibrator is currently running.
465 * @throws NotReadyException if calibrator is not ready.
466 * @throws CalibrationException if estimation fails for numerical reasons.
467 */
468 @SuppressWarnings("DuplicatedCode")
469 @Override
470 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
471 if (running) {
472 throw new LockedException();
473 }
474 if (!isReady()) {
475 throw new NotReadyException();
476 }
477
478 final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
479 @Override
480 public double[] getQualityScores() {
481 return qualityScores;
482 }
483
484 @Override
485 public double getThreshold() {
486 return threshold;
487 }
488
489 @Override
490 public int getTotalSamples() {
491 return measurements.size();
492 }
493
494 @Override
495 public int getSubsetSize() {
496 return preliminarySubsetSize;
497 }
498
499 @Override
500 public void estimatePreliminarSolutions(
501 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
502 computePreliminarySolutions(samplesIndices, solutions);
503 }
504
505 @Override
506 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
507 return computeError(measurements.get(i), currentEstimation);
508 }
509
510 @Override
511 public boolean isReady() {
512 return PROSACRobustKnownFrameGyroscopeCalibrator.this.isReady();
513 }
514
515 @Override
516 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
517 // no action needed
518 }
519
520 @Override
521 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
522 // no action needed
523 }
524
525 @Override
526 public void onEstimateNextIteration(
527 final RobustEstimator<PreliminaryResult> estimator, final int iteration) {
528 if (listener != null) {
529 listener.onCalibrateNextIteration(
530 PROSACRobustKnownFrameGyroscopeCalibrator.this, iteration);
531 }
532 }
533
534 @Override
535 public void onEstimateProgressChange(
536 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
537 if (listener != null) {
538 listener.onCalibrateProgressChange(
539 PROSACRobustKnownFrameGyroscopeCalibrator.this, progress);
540 }
541 }
542 });
543
544 try {
545 running = true;
546
547 if (listener != null) {
548 listener.onCalibrateStart(this);
549 }
550
551 inliersData = null;
552 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
553 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
554 innerEstimator.setConfidence(confidence);
555 innerEstimator.setMaxIterations(maxIterations);
556 innerEstimator.setProgressDelta(progressDelta);
557 final var preliminaryResult = innerEstimator.estimate();
558 inliersData = innerEstimator.getInliersData();
559
560 attemptRefine(preliminaryResult);
561
562 if (listener != null) {
563 listener.onCalibrateEnd(this);
564 }
565
566 } catch (final com.irurueta.numerical.LockedException e) {
567 throw new LockedException(e);
568 } catch (final com.irurueta.numerical.NotReadyException e) {
569 throw new NotReadyException(e);
570 } catch (final RobustEstimatorException e) {
571 throw new CalibrationException(e);
572 } finally {
573 running = false;
574 }
575 }
576
577 /**
578 * Returns method being used for robust estimation.
579 *
580 * @return method being used for robust estimation.
581 */
582 @Override
583 public RobustEstimatorMethod getMethod() {
584 return RobustEstimatorMethod.PROSAC;
585 }
586
587 /**
588 * Indicates whether this calibrator requires quality scores for each
589 * measurement/sequence or not.
590 *
591 * @return true if quality scores are required, false otherwise.
592 */
593 @Override
594 public boolean isQualityScoresRequired() {
595 return true;
596 }
597
598 /**
599 * Sets quality scores corresponding to each provided sample.
600 * This method is used internally and does not check whether instance is
601 * locked or not.
602 *
603 * @param qualityScores quality scores to be set.
604 * @throws IllegalArgumentException if provided quality scores length
605 * is smaller than 7 samples.
606 */
607 private void internalSetQualityScores(final double[] qualityScores) {
608 if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
609 throw new IllegalArgumentException();
610 }
611
612 this.qualityScores = qualityScores;
613 }
614 }