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.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.StandardDeviationFrameBodyKinematics;
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 import com.irurueta.units.AngularSpeed;
29
30 import java.util.List;
31
32 /**
33 * Robustly estimates gyroscope cross couplings and scaling factors
34 * along with G-dependent cross biases introduced on the gyroscope by the
35 * specific forces sensed by the accelerometer using an LMedS algorithm to discard
36 * outliers.
37 * This estimator assumes that biases are known.
38 * <p>
39 * To use this calibrator at least 6 measurements at different known frames must
40 * be provided. In other words, accelerometer and gyroscope (i.e. body kinematics)
41 * samples must be obtained at 6 different positions, orientations and velocities
42 * (although typically velocities are always zero).
43 * <p>
44 * Measured gyroscope angular rates is assumed to follow the model shown below:
45 * <pre>
46 * Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
47 * </pre>
48 * Where:
49 * - Ωmeas is the measured gyroscope angular rates. This is a 3x1 vector.
50 * - bg is the gyroscope bias. Ideally, on a perfect gyroscope, this should be a
51 * 3x1 zero vector.
52 * - I is the 3x3 identity matrix.
53 * - Mg is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
54 * a perfect gyroscope, this should be a 3x3 zero matrix.
55 * - Ωtrue is ground-truth gyroscope angular rates.
56 * - Gg is the G-dependent cross biases introduced by the specific forces sensed
57 * by the accelerometer. Ideally, on a perfect gyroscope, this should be a 3x3
58 * zero matrix.
59 * - ftrue is ground-truth specific force. This is a 3x1 vector.
60 * - w is measurement noise. This is a 3x1 vector.
61 */
62 public class LMedSRobustKnownBiasAndFrameGyroscopeCalibrator extends RobustKnownBiasAndFrameGyroscopeCalibrator {
63
64 /**
65 * Default value to be used for stop threshold. Stop threshold can be used to
66 * avoid keeping the algorithm unnecessarily iterating in case that best
67 * estimated threshold using median of residuals is not small enough. Once a
68 * solution is found that generates a threshold below this value, the
69 * algorithm will stop.
70 * The stop threshold can be used to prevent the LMedS algorithm iterating
71 * too many times in cases where samples have a very similar accuracy.
72 * For instance, in cases where proportion of outliers is very small (close
73 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
74 * iterate for a long time trying to find the best solution when indeed
75 * there is no need to do that if a reasonable threshold has already been
76 * reached.
77 * Because of this behaviour the stop threshold can be set to a value much
78 * lower than the one typically used in RANSAC, and yet the algorithm could
79 * still produce even smaller thresholds in estimated results.
80 */
81 public static final double DEFAULT_STOP_THRESHOLD = 1e-5;
82
83 /**
84 * Minimum allowed stop threshold value.
85 */
86 public static final double MIN_STOP_THRESHOLD = 0.0;
87
88 /**
89 * Threshold to be used to keep the algorithm iterating in case that best
90 * estimated threshold using median of residuals is not small enough. Once
91 * a solution is found that generates a threshold below this value, the
92 * algorithm will stop.
93 * The stop threshold can be used to prevent the LMedS algorithm iterating
94 * too many times in cases where samples have a very similar accuracy.
95 * For instance, in cases where proportion of outliers is very small (close
96 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
97 * iterate for a long time trying to find the best solution when indeed
98 * there is no need to do that if a reasonable threshold has already been
99 * reached.
100 * Because of this behaviour the stop threshold can be set to a value much
101 * lower than the one typically used in RANSAC, and yet the algorithm could
102 * still produce even smaller thresholds in estimated results.
103 */
104 private double stopThreshold = DEFAULT_STOP_THRESHOLD;
105
106 /**
107 * Constructor.
108 */
109 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator() {
110 super();
111 }
112
113 /**
114 * Constructor.
115 *
116 * @param listener listener to be notified of events such as when estimation
117 * starts, ends or its progress significantly changes.
118 */
119 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
120 final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
121 super(listener);
122 }
123
124 /**
125 * Constructor.
126 *
127 * @param measurements list of body kinematics measurements with standard
128 * deviations taken at different frames (positions, orientations
129 * and velocities).
130 */
131 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
132 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 LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
145 final List<StandardDeviationFrameBodyKinematics> measurements,
146 final RobustKnownBiasAndFrameGyroscopeCalibratorListener 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 LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(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 LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
168 final boolean commonAxisUsed, final RobustKnownBiasAndFrameGyroscopeCalibratorListener 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 LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
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 LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
197 final List<StandardDeviationFrameBodyKinematics> measurements, final boolean commonAxisUsed,
198 final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
199 super(measurements, commonAxisUsed, listener);
200 }
201
202 /**
203 * Constructor.
204 *
205 * @param biasX known x coordinate of gyroscope bias expressed in radians per
206 * second (rad/s).
207 * @param biasY known y coordinate of gyroscope bias expressed in radians per
208 * second (rad/s).
209 * @param biasZ known z coordinate of gyroscope bias expressed in radians per
210 * second (rad/s).
211 */
212 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(final double biasX, final double biasY, final double biasZ) {
213 super(biasX, biasY, biasZ);
214 }
215
216 /**
217 * Constructor.
218 *
219 * @param biasX known x coordinate of gyroscope bias expressed in radians per
220 * second (rad/s).
221 * @param biasY known y coordinate of gyroscope bias expressed in radians per
222 * second (rad/s).
223 * @param biasZ known z coordinate of gyroscope bias expressed in radians per
224 * second (rad/s).
225 * @param listener listener to be notified of events such as when estimation
226 * starts, ends or its progress significantly changes.
227 */
228 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
229 final double biasX, final double biasY, final double biasZ,
230 final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
231 super(biasX, biasY, biasZ, listener);
232 }
233
234 /**
235 * Constructor.
236 *
237 * @param measurements list of body kinematics measurements with standard
238 * deviations taken at different frames (positions, orientations
239 * and velocities).
240 * @param biasX known x coordinate of gyroscope bias expressed in radians per
241 * second (rad/s).
242 * @param biasY known y coordinate of gyroscope bias expressed in radians per
243 * second (rad/s).
244 * @param biasZ known z coordinate of gyroscope bias expressed in radians per
245 * second (rad/s).
246 */
247 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
248 final List<StandardDeviationFrameBodyKinematics> measurements,
249 final double biasX, final double biasY, final double biasZ) {
250 super(measurements, biasX, biasY, biasZ);
251 }
252
253 /**
254 * Constructor.
255 *
256 * @param measurements list of body kinematics measurements with standard
257 * deviations taken at different frames (positions, orientations
258 * and velocities).
259 * @param biasX known x coordinate of gyroscope bias expressed in radians per
260 * second (rad/s).
261 * @param biasY known y coordinate of gyroscope bias expressed in radians per
262 * second (rad/s).
263 * @param biasZ known z coordinate of gyroscope bias expressed in radians per
264 * second (rad/s).
265 * @param listener listener to handle events raised by this calibrator.
266 */
267 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
268 final List<StandardDeviationFrameBodyKinematics> measurements,
269 final double biasX, final double biasY, final double biasZ,
270 final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
271 super(measurements, biasX, biasY, biasZ, listener);
272 }
273
274 /**
275 * Constructor.
276 *
277 * @param biasX known x coordinate of gyroscope bias expressed in radians per
278 * second (rad/s).
279 * @param biasY known y coordinate of gyroscope bias expressed in radians per
280 * second (rad/s).
281 * @param biasZ known z coordinate of gyroscope bias expressed in radians per
282 * second (rad/s).
283 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
284 * accelerometer and gyroscope.
285 */
286 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
287 final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed) {
288 super(biasX, biasY, biasZ, commonAxisUsed);
289 }
290
291 /**
292 * Constructor.
293 *
294 * @param biasX known x coordinate of gyroscope bias expressed in radians per
295 * second (rad/s).
296 * @param biasY known y coordinate of gyroscope bias expressed in radians per
297 * second (rad/s).
298 * @param biasZ known z coordinate of gyroscope bias expressed in radians per
299 * second (rad/s).
300 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
301 * accelerometer and gyroscope.
302 * @param listener listener to handle events raised by this calibrator.
303 */
304 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
305 final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
306 final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
307 super(biasX, biasY, biasZ, commonAxisUsed, listener);
308 }
309
310 /**
311 * Constructor.
312 *
313 * @param measurements list of body kinematics measurements with standard
314 * deviations taken at different frames (positions, orientations
315 * and velocities).
316 * @param biasX known x coordinate of gyroscope bias expressed in radians per
317 * second (rad/s).
318 * @param biasY known y coordinate of gyroscope bias expressed in radians per
319 * second (rad/s).
320 * @param biasZ known z coordinate of gyroscope bias expressed in radians per
321 * second (rad/s).
322 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
323 * accelerometer and gyroscope.
324 */
325 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
326 final List<StandardDeviationFrameBodyKinematics> measurements,
327 final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed) {
328 super(measurements, biasX, biasY, biasZ, commonAxisUsed);
329 }
330
331 /**
332 * Constructor.
333 *
334 * @param measurements list of body kinematics measurements with standard
335 * deviations taken at different frames (positions, orientations
336 * and velocities).
337 * @param biasX known x coordinate of gyroscope bias expressed in radians per
338 * second (rad/s).
339 * @param biasY known y coordinate of gyroscope bias expressed in radians per
340 * second (rad/s).
341 * @param biasZ known z coordinate of gyroscope bias expressed in radians per
342 * second (rad/s).
343 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
344 * accelerometer and gyroscope.
345 * @param listener listener to handle events raised by this calibrator.
346 */
347 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
348 final List<StandardDeviationFrameBodyKinematics> measurements,
349 final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
350 final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
351 super(measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
352 }
353
354 /**
355 * Constructor.
356 *
357 * @param biasX known x coordinate of gyroscope bias.
358 * @param biasY known y coordinate of gyroscope bias.
359 * @param biasZ known z coordinate of gyroscope bias.
360 */
361 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
362 final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ) {
363 super(biasX, biasY, biasZ);
364 }
365
366 /**
367 * Constructor.
368 *
369 * @param biasX known x coordinate of gyroscope bias.
370 * @param biasY known y coordinate of gyroscope bias.
371 * @param biasZ known z coordinate of gyroscope bias.
372 * @param listener listener to be notified of events such as when estimation
373 * starts, ends or its progress significantly changes.
374 */
375 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
376 final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ,
377 final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
378 super(biasX, biasY, biasZ, listener);
379 }
380
381 /**
382 * Constructor.
383 *
384 * @param measurements list of body kinematics measurements with standard
385 * deviations taken at different frames (positions, orientations
386 * and velocities).
387 * @param biasX known x coordinate of gyroscope bias.
388 * @param biasY known y coordinate of gyroscope bias.
389 * @param biasZ known z coordinate of gyroscope bias.
390 */
391 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
392 final List<StandardDeviationFrameBodyKinematics> measurements,
393 final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ) {
394 super(measurements, biasX, biasY, biasZ);
395 }
396
397 /**
398 * Constructor.
399 *
400 * @param measurements list of body kinematics measurements with standard
401 * deviations taken at different frames (positions, orientations
402 * and velocities).
403 * @param biasX known x coordinate of gyroscope bias.
404 * @param biasY known y coordinate of gyroscope bias.
405 * @param biasZ known z coordinate of gyroscope bias.
406 * @param listener listener to handle events raised by this calibrator.
407 */
408 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
409 final List<StandardDeviationFrameBodyKinematics> measurements,
410 final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ,
411 final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
412 super(measurements, biasX, biasY, biasZ, listener);
413 }
414
415 /**
416 * Constructor.
417 *
418 * @param biasX known x coordinate of gyroscope bias.
419 * @param biasY known y coordinate of gyroscope bias.
420 * @param biasZ known z coordinate of gyroscope bias.
421 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
422 * accelerometer and gyroscope.
423 */
424 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
425 final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ,
426 final boolean commonAxisUsed) {
427 super(biasX, biasY, biasZ, commonAxisUsed);
428 }
429
430 /**
431 * Constructor.
432 *
433 * @param biasX known x coordinate of gyroscope bias.
434 * @param biasY known y coordinate of gyroscope bias.
435 * @param biasZ known z coordinate of gyroscope bias.
436 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
437 * accelerometer and gyroscope.
438 * @param listener listener to handle events raised by this calibrator.
439 */
440 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
441 final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ,
442 final boolean commonAxisUsed, final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
443 super(biasX, biasY, biasZ, commonAxisUsed, listener);
444 }
445
446 /**
447 * Constructor.
448 *
449 * @param measurements list of body kinematics measurements with standard
450 * deviations taken at different frames (positions, orientations
451 * and velocities).
452 * @param biasX known x coordinate of gyroscope bias.
453 * @param biasY known y coordinate of gyroscope bias.
454 * @param biasZ known z coordinate of gyroscope bias.
455 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
456 * accelerometer and gyroscope.
457 */
458 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
459 final List<StandardDeviationFrameBodyKinematics> measurements,
460 final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ,
461 final boolean commonAxisUsed) {
462 super(measurements, biasX, biasY, biasZ, commonAxisUsed);
463 }
464
465 /**
466 * Constructor.
467 *
468 * @param measurements list of body kinematics measurements with standard
469 * deviations taken at different frames (positions, orientations
470 * and velocities).
471 * @param biasX known x coordinate of gyroscope bias.
472 * @param biasY known y coordinate of gyroscope bias.
473 * @param biasZ known z coordinate of gyroscope bias.
474 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
475 * accelerometer and gyroscope.
476 * @param listener listener to handle events raised by this calibrator.
477 */
478 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
479 final List<StandardDeviationFrameBodyKinematics> measurements,
480 final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ, final boolean commonAxisUsed,
481 final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
482 super(measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
483 }
484
485 /**
486 * Constructor.
487 *
488 * @param bias known gyroscope bias.
489 * @throws IllegalArgumentException if provided array does not have length 3.
490 */
491 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(final double[] bias) {
492 super(bias);
493 }
494
495 /**
496 * Constructor.
497 *
498 * @param bias known gyroscope bias.
499 * @param listener listener to be notified of events such as when estimation
500 * starts, ends or its progress significantly changes.
501 * @throws IllegalArgumentException if provided array does not have length 3.
502 */
503 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
504 final double[] bias, final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
505 super(bias, listener);
506 }
507
508 /**
509 * Constructor.
510 *
511 * @param measurements list of body kinematics measurements with standard
512 * deviations taken at different frames (positions, orientations
513 * and velocities).
514 * @param bias known gyroscope bias.
515 * @throws IllegalArgumentException if provided array does not have length 3.
516 */
517 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
518 final List<StandardDeviationFrameBodyKinematics> measurements, final double[] bias) {
519 super(measurements, bias);
520 }
521
522 /**
523 * Constructor.
524 *
525 * @param measurements list of body kinematics measurements with standard
526 * deviations taken at different frames (positions, orientations
527 * and velocities).
528 * @param bias known gyroscope bias.
529 * @param listener listener to handle events raised by this calibrator.
530 * @throws IllegalArgumentException if provided array does not have length 3.
531 */
532 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
533 final List<StandardDeviationFrameBodyKinematics> measurements, final double[] bias,
534 final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
535 super(measurements, bias, listener);
536 }
537
538 /**
539 * Constructor.
540 *
541 * @param bias known gyroscope bias.
542 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
543 * accelerometer and gyroscope.
544 * @throws IllegalArgumentException if provided array does not have length 3.
545 */
546 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(final double[] bias, final boolean commonAxisUsed) {
547 super(bias, commonAxisUsed);
548 }
549
550 /**
551 * Constructor.
552 *
553 * @param bias known gyroscope bias.
554 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
555 * accelerometer and gyroscope.
556 * @param listener listener to handle events raised by this calibrator.
557 * @throws IllegalArgumentException if provided array does not have length 3.
558 */
559 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
560 final double[] bias, final boolean commonAxisUsed,
561 final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
562 super(bias, commonAxisUsed, listener);
563 }
564
565 /**
566 * Constructor.
567 *
568 * @param measurements list of body kinematics measurements with standard
569 * deviations taken at different frames (positions, orientations
570 * and velocities).
571 * @param bias known gyroscope bias.
572 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
573 * accelerometer and gyroscope.
574 * @throws IllegalArgumentException if provided array does not have length 3.
575 */
576 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
577 final List<StandardDeviationFrameBodyKinematics> measurements, final double[] bias,
578 final boolean commonAxisUsed) {
579 super(measurements, bias, commonAxisUsed);
580 }
581
582 /**
583 * Constructor.
584 *
585 * @param measurements list of body kinematics measurements with standard
586 * deviations taken at different frames (positions, orientations
587 * and velocities).
588 * @param bias known gyroscope bias.
589 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
590 * accelerometer and gyroscope.
591 * @param listener listener to handle events raised by this calibrator.
592 * @throws IllegalArgumentException if provided array does not have length 3.
593 */
594 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
595 final List<StandardDeviationFrameBodyKinematics> measurements, final double[] bias,
596 final boolean commonAxisUsed, final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
597 super(measurements, bias, commonAxisUsed, listener);
598 }
599
600 /**
601 * Constructor.
602 *
603 * @param bias known gyroscope bias.
604 * @throws IllegalArgumentException if provided matrix is not 3x1.
605 */
606 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(final Matrix bias) {
607 super(bias);
608 }
609
610 /**
611 * Constructor.
612 *
613 * @param bias known gyroscope bias.
614 * @param listener listener to be notified of events such as when estimation
615 * starts, ends or its progress significantly changes.
616 * @throws IllegalArgumentException if provided matrix is not 3x1.
617 */
618 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
619 final Matrix bias, final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
620 super(bias, listener);
621 }
622
623 /**
624 * Constructor.
625 *
626 * @param measurements list of body kinematics measurements with standard
627 * deviations taken at different frames (positions, orientations
628 * and velocities).
629 * @param bias known gyroscope bias.
630 * @throws IllegalArgumentException if provided matrix is not 3x1.
631 */
632 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
633 final List<StandardDeviationFrameBodyKinematics> measurements, final Matrix bias) {
634 super(measurements, bias);
635 }
636
637 /**
638 * Constructor.
639 *
640 * @param measurements list of body kinematics measurements with standard
641 * deviations taken at different frames (positions, orientations
642 * and velocities).
643 * @param bias known gyroscope bias.
644 * @param listener listener to handle events raised by this calibrator.
645 * @throws IllegalArgumentException if provided matrix is not 3x1.
646 */
647 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
648 final List<StandardDeviationFrameBodyKinematics> measurements, final Matrix bias,
649 final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
650 super(measurements, bias, listener);
651 }
652
653 /**
654 * Constructor.
655 *
656 * @param bias known gyroscope bias.
657 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
658 * accelerometer and gyroscope.
659 * @throws IllegalArgumentException if provided matrix is not 3x1.
660 */
661 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(final Matrix bias, final boolean commonAxisUsed) {
662 super(bias, commonAxisUsed);
663 }
664
665 /**
666 * Constructor.
667 *
668 * @param bias known gyroscope bias.
669 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
670 * accelerometer and gyroscope.
671 * @param listener listener to handle events raised by this calibrator.
672 * @throws IllegalArgumentException if provided matrix is not 3x1.
673 */
674 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
675 final Matrix bias, final boolean commonAxisUsed,
676 final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
677 super(bias, commonAxisUsed, listener);
678 }
679
680 /**
681 * Constructor.
682 *
683 * @param measurements list of body kinematics measurements with standard
684 * deviations taken at different frames (positions, orientations
685 * and velocities).
686 * @param bias known gyroscope bias.
687 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
688 * accelerometer and gyroscope.
689 * @throws IllegalArgumentException if provided matrix is not 3x1.
690 */
691 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
692 final List<StandardDeviationFrameBodyKinematics> measurements,
693 final Matrix bias, final boolean commonAxisUsed) {
694 super(measurements, bias, commonAxisUsed);
695 }
696
697 /**
698 * Constructor.
699 *
700 * @param measurements list of body kinematics measurements with standard
701 * deviations taken at different frames (positions, orientations
702 * and velocities).
703 * @param bias known gyroscope bias.
704 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
705 * accelerometer and gyroscope.
706 * @param listener listener to handle events raised by this calibrator.
707 * @throws IllegalArgumentException if provided matrix is not 3x1.
708 */
709 public LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
710 final List<StandardDeviationFrameBodyKinematics> measurements, final Matrix bias,
711 final boolean commonAxisUsed, final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) {
712 super(measurements, bias, commonAxisUsed, listener);
713 }
714
715 /**
716 * Returns threshold to be used to keep the algorithm iterating in case that
717 * best estimated threshold using median of residuals is not small enough.
718 * Once a solution is found that generates a threshold below this value, the
719 * algorithm will stop.
720 * The stop threshold can be used to prevent the LMedS algorithm to iterate
721 * too many times in cases where samples have a very similar accuracy.
722 * For instance, in cases where proportion of outliers is very small (close
723 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
724 * iterate for a long time trying to find the best solution when indeed
725 * there is no need to do that if a reasonable threshold has already been
726 * reached.
727 * Because of this behaviour the stop threshold can be set to a value much
728 * lower than the one typically used in RANSAC, and yet the algorithm could
729 * still produce even smaller thresholds in estimated results.
730 *
731 * @return stop threshold to stop the algorithm prematurely when a certain
732 * accuracy has been reached.
733 */
734 public double getStopThreshold() {
735 return stopThreshold;
736 }
737
738 /**
739 * Sets threshold to be used to keep the algorithm iterating in case that
740 * best estimated threshold using median of residuals is not small enough.
741 * Once a solution is found that generates a threshold below this value,
742 * the algorithm will stop.
743 * The stop threshold can be used to prevent the LMedS algorithm to iterate
744 * too many times in cases where samples have a very similar accuracy.
745 * For instance, in cases where proportion of outliers is very small (close
746 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
747 * iterate for a long time trying to find the best solution when indeed
748 * there is no need to do that if a reasonable threshold has already been
749 * reached.
750 * Because of this behaviour the stop threshold can be set to a value much
751 * lower than the one typically used in RANSAC, and yet the algorithm could
752 * still produce even smaller thresholds in estimated results.
753 *
754 * @param stopThreshold stop threshold to stop the algorithm prematurely
755 * when a certain accuracy has been reached.
756 * @throws IllegalArgumentException if provided value is zero or negative.
757 * @throws LockedException if calibrator is currently running.
758 */
759 public void setStopThreshold(final double stopThreshold) throws LockedException {
760 if (running) {
761 throw new LockedException();
762 }
763 if (stopThreshold <= MIN_STOP_THRESHOLD) {
764 throw new IllegalArgumentException();
765 }
766
767 this.stopThreshold = stopThreshold;
768 }
769
770 /**
771 * Estimates gyroscope calibration parameters containing bias, scale factors
772 * cross-coupling errors and g-dependant cross biases.
773 *
774 * @throws LockedException if calibrator is currently running.
775 * @throws NotReadyException if calibrator is not ready.
776 * @throws CalibrationException if estimation fails for numerical reasons.
777 */
778 @SuppressWarnings("DuplicatedCode")
779 @Override
780 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
781 if (running) {
782 throw new LockedException();
783 }
784 if (!isReady()) {
785 throw new NotReadyException();
786 }
787
788 final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
789 @Override
790 public int getTotalSamples() {
791 return measurements.size();
792 }
793
794 @Override
795 public int getSubsetSize() {
796 return preliminarySubsetSize;
797 }
798
799 @Override
800 public void estimatePreliminarSolutions(
801 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
802 computePreliminarySolutions(samplesIndices, solutions);
803 }
804
805 @Override
806 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
807 return computeError(measurements.get(i), currentEstimation);
808 }
809
810 @Override
811 public boolean isReady() {
812 return LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.this.isReady();
813 }
814
815 @Override
816 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
817 // no action needed
818 }
819
820 @Override
821 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
822 // no action needed
823 }
824
825 @Override
826 public void onEstimateNextIteration(
827 final RobustEstimator<PreliminaryResult> estimator, final int iteration) {
828 if (listener != null) {
829 listener.onCalibrateNextIteration(
830 LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.this, iteration);
831 }
832 }
833
834 @Override
835 public void onEstimateProgressChange(
836 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
837 if (listener != null) {
838 listener.onCalibrateProgressChange(
839 LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.this, progress);
840 }
841 }
842 });
843
844 try {
845 running = true;
846
847 if (listener != null) {
848 listener.onCalibrateStart(this);
849 }
850
851 inliersData = null;
852 innerEstimator.setConfidence(confidence);
853 innerEstimator.setMaxIterations(maxIterations);
854 innerEstimator.setProgressDelta(progressDelta);
855 innerEstimator.setStopThreshold(stopThreshold);
856 final var preliminaryResult = innerEstimator.estimate();
857 inliersData = innerEstimator.getInliersData();
858
859 attemptRefine(preliminaryResult);
860
861 if (listener != null) {
862 listener.onCalibrateEnd(this);
863 }
864
865 } catch (final com.irurueta.numerical.LockedException e) {
866 throw new LockedException(e);
867 } catch (final com.irurueta.numerical.NotReadyException e) {
868 throw new NotReadyException(e);
869 } catch (final RobustEstimatorException e) {
870 throw new CalibrationException(e);
871 } finally {
872 running = false;
873 }
874 }
875
876 /**
877 * Returns method being used for robust estimation.
878 *
879 * @return method being used for robust estimation.
880 */
881 @Override
882 public RobustEstimatorMethod getMethod() {
883 return RobustEstimatorMethod.LMEDS;
884 }
885
886 /**
887 * Indicates whether this calibrator requires quality scores for each
888 * measurement/sequence or not.
889 *
890 * @return true if quality scores are required, false otherwise.
891 */
892 @Override
893 public boolean isQualityScoresRequired() {
894 return false;
895 }
896 }