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.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.navigation.LockedException;
21 import com.irurueta.navigation.NotReadyException;
22 import com.irurueta.navigation.inertial.calibration.BodyKinematicsSequence;
23 import com.irurueta.navigation.inertial.calibration.CalibrationException;
24 import com.irurueta.navigation.inertial.calibration.StandardDeviationTimedBodyKinematics;
25 import com.irurueta.numerical.robust.RANSACRobustEstimator;
26 import com.irurueta.numerical.robust.RANSACRobustEstimatorListener;
27 import com.irurueta.numerical.robust.RobustEstimator;
28 import com.irurueta.numerical.robust.RobustEstimatorException;
29 import com.irurueta.numerical.robust.RobustEstimatorMethod;
30
31 import java.util.List;
32
33 /**
34 * Robustly estimates gyroscope biases, cross couplings and scaling factors
35 * along with G-dependent cross biases introduced on the gyroscope by the
36 * specific forces sensed by the accelerometer using RANSAC robust estimator.
37 * <p>
38 * This calibrator assumes that the IMU is at a more or less fixed location on
39 * Earth, and evaluates sequences of measured body kinematics to perform
40 * calibration for unknown orientations on those provided sequences.
41 * Each provided sequence will be preceded by a static period where mean
42 * specific force will be measured to determine gravity (and hence partial
43 * body attitude).
44 * <p>
45 * Measured gyroscope angular rates is assumed to follow the model shown below:
46 * <pre>
47 * Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
48 * </pre>
49 * Where:
50 * - Ωmeas is the measured gyroscope angular rates. This is a 3x1 vector.
51 * - bg is the gyroscope bias. Ideally, on a perfect gyroscope, this should be a
52 * 3x1 zero vector.
53 * - I is the 3x3 identity matrix.
54 * - Mg is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
55 * a perfect gyroscope, this should be a 3x3 zero matrix.
56 * - Ωtrue is ground-truth gyroscope angular rates.
57 * - Gg is the G-dependent cross biases introduced by the specific forces sensed
58 * by the accelerometer. Ideally, on a perfect gyroscope, this should be a 3x3
59 * zero matrix.
60 * - ftrue is ground-truth specific force. This is a 3x1 vector.
61 * - w is measurement noise. This is a 3x1 vector.
62 */
63 public class RANSACRobustEasyGyroscopeCalibrator extends RobustEasyGyroscopeCalibrator {
64
65 /**
66 * Constant defining default threshold to determine whether samples are inliers or not.
67 */
68 public static final double DEFAULT_THRESHOLD = 1e-3;
69
70 /**
71 * Minimum value that can be set as threshold.
72 * Threshold must be strictly greater than 0.0.
73 */
74 public static final double MIN_THRESHOLD = 0.0;
75
76 /**
77 * Indicates that by default inliers will only be computed but not kept.
78 */
79 public static final boolean DEFAULT_COMPUTE_AND_KEEP_INLIERS = false;
80
81 /**
82 * Indicates that by default residuals will only be computed but not kept.
83 */
84 public static final boolean DEFAULT_COMPUTE_AND_KEEP_RESIDUALS = false;
85
86 /**
87 * Threshold to determine whether samples are inliers or not when testing possible solutions.
88 * The threshold refers to the amount of error on distance between estimated position and
89 * distances provided for each sample.
90 */
91 private double threshold = DEFAULT_THRESHOLD;
92
93 /**
94 * Indicates whether inliers must be computed and kept.
95 */
96 private boolean computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
97
98 /**
99 * Indicates whether residuals must be computed and kept.
100 */
101 private boolean computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
102
103 /**
104 * Constructor.
105 */
106 public RANSACRobustEasyGyroscopeCalibrator() {
107 super();
108 }
109
110 /**
111 * Constructor.
112 *
113 * @param sequences collection of sequences containing timestamped body
114 * kinematics measurements.
115 * @param initialBias initial gyroscope bias to be used to find a solution.
116 * This must be 3x1 and is expressed in radians per
117 * second (rad/s).
118 * @param initialMg initial gyroscope scale factors and cross coupling
119 * errors matrix. Must be 3x3.
120 * @param initialGg initial gyroscope G-dependent cross biases
121 * introduced on the gyroscope by the specific forces
122 * sensed by the accelerometer. Must be 3x3.
123 * @throws IllegalArgumentException if any of the provided values does
124 * not have proper size.
125 */
126 public RANSACRobustEasyGyroscopeCalibrator(
127 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
128 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg) {
129 super(sequences, initialBias, initialMg, initialGg);
130 }
131
132 /**
133 * Constructor.
134 *
135 * @param sequences collection of sequences containing timestamped body
136 * kinematics measurements.
137 * @param initialBias initial gyroscope bias to be used to find a solution.
138 * This must be 3x1 and is expressed in radians per
139 * second (rad/s).
140 * @param initialMg initial gyroscope scale factors and cross coupling
141 * errors matrix. Must be 3x3.
142 * @param initialGg initial gyroscope G-dependent cross biases
143 * introduced on the gyroscope by the specific forces
144 * sensed by the accelerometer. Must be 3x3.
145 * @param listener listener to handle events raised by this
146 * calibrator.
147 * @throws IllegalArgumentException if any of the provided values does
148 * not have proper size.
149 */
150 public RANSACRobustEasyGyroscopeCalibrator(
151 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
152 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
153 final RobustEasyGyroscopeCalibratorListener listener) {
154 super(sequences, initialBias, initialMg, initialGg, listener);
155 }
156
157 /**
158 * Constructor.
159 *
160 * @param sequences collection of sequences containing timestamped body
161 * kinematics measurements.
162 * @param initialBias initial gyroscope bias to be used to find a
163 * solution. This must have length 3 and is expressed
164 * in radians per second (rad/s).
165 * @param initialMg initial gyroscope scale factors and cross coupling
166 * errors matrix. Must be 3x3.
167 * @param initialGg initial gyroscope G-dependent cross biases
168 * introduced on the gyroscope by the specific forces
169 * sensed by the accelerometer. Must be 3x3.
170 * @throws IllegalArgumentException if any of the provided values does
171 * not have proper size.
172 */
173 public RANSACRobustEasyGyroscopeCalibrator(
174 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
175 final double[] initialBias, final Matrix initialMg, final Matrix initialGg) {
176 super(sequences, initialBias, initialMg, initialGg);
177 }
178
179 /**
180 * Constructor.
181 *
182 * @param sequences collection of sequences containing timestamped body
183 * kinematics measurements.
184 * @param initialBias initial gyroscope bias to be used to find a
185 * solution. This must have length 3 and is expressed
186 * in radians per second (rad/s).
187 * @param initialMg initial gyroscope scale factors and cross coupling
188 * errors matrix. Must be 3x3.
189 * @param initialGg initial gyroscope G-dependent cross biases
190 * introduced on the gyroscope by the specific forces
191 * sensed by the accelerometer. Must be 3x3.
192 * @param listener listener to handle events raised by this
193 * calibrator.
194 * @throws IllegalArgumentException if any of the provided values does
195 * not have proper size.
196 */
197 public RANSACRobustEasyGyroscopeCalibrator(
198 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
199 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
200 final RobustEasyGyroscopeCalibratorListener listener) {
201 super(sequences, initialBias, initialMg, initialGg, listener);
202 }
203
204 /**
205 * Constructor.
206 *
207 * @param sequences collection of sequences containing timestamped body
208 * kinematics measurements.
209 * @param initialBias initial gyroscope bias to be used to find a
210 * solution. This must have length 3 and is expressed
211 * in radians per second (rad/s).
212 * @param initialMg initial gyroscope scale factors and cross coupling
213 * errors matrix. Must be 3x3.
214 * @param initialGg initial gyroscope G-dependent cross biases
215 * introduced on the gyroscope by the specific forces
216 * sensed by the accelerometer. Must be 3x3.
217 * @param accelerometerBias known accelerometer bias. This must
218 * have length 3 and is expressed in
219 * meters per squared second
220 * (m/s^2).
221 * @param accelerometerMa known accelerometer scale factors and
222 * cross coupling matrix. Must be 3x3.
223 * @throws IllegalArgumentException if any of the provided values does
224 * not have proper size.
225 */
226 public RANSACRobustEasyGyroscopeCalibrator(
227 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
228 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
229 final double[] accelerometerBias, final Matrix accelerometerMa) {
230 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
231 }
232
233 /**
234 * Constructor.
235 *
236 * @param sequences collection of sequences containing timestamped body
237 * kinematics measurements.
238 * @param initialBias initial gyroscope bias to be used to find a
239 * solution. This must have length 3 and is expressed
240 * in radians per second (rad/s).
241 * @param initialMg initial gyroscope scale factors and cross coupling
242 * errors matrix. Must be 3x3.
243 * @param initialGg initial gyroscope G-dependent cross biases
244 * introduced on the gyroscope by the specific forces
245 * sensed by the accelerometer. Must be 3x3.
246 * @param accelerometerBias known accelerometer bias. This must
247 * have length 3 and is expressed in
248 * meters per squared second
249 * (m/s^2).
250 * @param accelerometerMa known accelerometer scale factors and
251 * cross coupling matrix. Must be 3x3.
252 * @param listener listener to handle events raised by this
253 * calibrator.
254 * @throws IllegalArgumentException if any of the provided values does
255 * not have proper size.
256 */
257 public RANSACRobustEasyGyroscopeCalibrator(
258 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
259 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
260 final double[] accelerometerBias, final Matrix accelerometerMa,
261 final RobustEasyGyroscopeCalibratorListener listener) {
262 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
263 }
264
265 /**
266 * Constructor.
267 *
268 * @param sequences collection of sequences containing timestamped body
269 * kinematics measurements.
270 * @param initialBias initial gyroscope bias to be used to find a
271 * solution. This must be 3x1 and is expressed
272 * in radians per second (rad/s).
273 * @param initialMg initial gyroscope scale factors and cross coupling
274 * errors matrix. Must be 3x3.
275 * @param initialGg initial gyroscope G-dependent cross biases
276 * introduced on the gyroscope by the specific forces
277 * sensed by the accelerometer. Must be 3x3.
278 * @param accelerometerBias known accelerometer bias. This must be 3x1
279 * and is expressed in meters per squared
280 * second (m/s^2).
281 * @param accelerometerMa known accelerometer scale factors and
282 * cross coupling matrix. Must be 3x3.
283 * @throws IllegalArgumentException if any of the provided values does
284 * not have proper size.
285 */
286 public RANSACRobustEasyGyroscopeCalibrator(
287 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
288 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
289 final Matrix accelerometerMa) {
290 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
291 }
292
293 /**
294 * Constructor.
295 *
296 * @param sequences collection of sequences containing timestamped body
297 * kinematics measurements.
298 * @param initialBias initial gyroscope bias to be used to find a
299 * solution. This must be 3x1 and is expressed
300 * in radians per second (rad/s).
301 * @param initialMg initial gyroscope scale factors and cross coupling
302 * errors matrix. Must be 3x3.
303 * @param initialGg initial gyroscope G-dependent cross biases
304 * introduced on the gyroscope by the specific forces
305 * sensed by the accelerometer. Must be 3x3.
306 * @param accelerometerBias known accelerometer bias. This must be 3x1
307 * and is expressed in meters per squared
308 * second (m/s^2).
309 * @param accelerometerMa known accelerometer scale factors and
310 * cross coupling matrix. Must be 3x3.
311 * @param listener listener to handle events raised by this
312 * calibrator.
313 * @throws IllegalArgumentException if any of the provided values does
314 * not have proper size.
315 */
316 public RANSACRobustEasyGyroscopeCalibrator(
317 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
318 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
319 final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
320 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
321 }
322
323 /**
324 * Constructor.
325 *
326 * @param sequences collection of sequences containing timestamped body
327 * kinematics measurements.
328 * @param commonAxisUsed indicates whether z-axis is
329 * assumed to be common for
330 * accelerometer and gyroscope.
331 * @param estimateGDependentCrossBiases true if G-dependent cross biases
332 * will be estimated, false
333 * otherwise.
334 * @param initialBias initial gyroscope bias to be used to find a
335 * solution. This must be 3x1 and is expressed
336 * in radians per second (rad/s).
337 * @param initialMg initial gyroscope scale factors and cross coupling
338 * errors matrix. Must be 3x3.
339 * @param initialGg initial gyroscope G-dependent cross biases
340 * introduced on the gyroscope by the specific forces
341 * sensed by the accelerometer. Must be 3x3.
342 * @throws IllegalArgumentException if any of the provided values does
343 * not have proper size.
344 */
345 public RANSACRobustEasyGyroscopeCalibrator(
346 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
347 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
348 final Matrix initialMg, final Matrix initialGg) {
349 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
350 }
351
352 /**
353 * Constructor.
354 *
355 * @param sequences collection of sequences containing timestamped body
356 * kinematics measurements.
357 * @param commonAxisUsed indicates whether z-axis is
358 * assumed to be common for
359 * accelerometer and gyroscope.
360 * @param estimateGDependentCrossBiases true if G-dependent cross biases
361 * will be estimated, false
362 * otherwise.
363 * @param initialBias initial gyroscope bias to be used to find a
364 * solution. This must be 3x1 and is expressed
365 * in radians per second (rad/s).
366 * @param initialMg initial gyroscope scale factors and cross coupling
367 * errors matrix. Must be 3x3.
368 * @param initialGg initial gyroscope G-dependent cross biases
369 * introduced on the gyroscope by the specific forces
370 * sensed by the accelerometer. Must be 3x3.
371 * @param listener listener to handle events raised by this
372 * calibrator.
373 * @throws IllegalArgumentException if any of the provided values does
374 * not have proper size.
375 */
376 public RANSACRobustEasyGyroscopeCalibrator(
377 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
378 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
379 final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener) {
380 super(sequences, commonAxisUsed, estimateGDependentCrossBiases,
381 initialBias, initialMg, initialGg, listener);
382 }
383
384 /**
385 * Constructor.
386 *
387 * @param sequences collection of sequences containing timestamped body
388 * kinematics measurements.
389 * @param commonAxisUsed indicates whether z-axis is
390 * assumed to be common for
391 * accelerometer and gyroscope.
392 * @param estimateGDependentCrossBiases true if G-dependent cross biases
393 * will be estimated, false
394 * otherwise.
395 * @param initialBias initial gyroscope bias to be used to find a
396 * solution. This must have length 3 and is expressed
397 * in radians per second (rad/s).
398 * @param initialMg initial gyroscope scale factors and cross coupling
399 * errors matrix. Must be 3x3.
400 * @param initialGg initial gyroscope G-dependent cross biases
401 * introduced on the gyroscope by the specific forces
402 * sensed by the accelerometer. Must be 3x3.
403 * @throws IllegalArgumentException if any of the provided values does
404 * not have proper size.
405 */
406 public RANSACRobustEasyGyroscopeCalibrator(
407 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
408 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
409 final Matrix initialMg, final Matrix initialGg) {
410 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
411 }
412
413 /**
414 * Constructor.
415 *
416 * @param sequences collection of sequences containing timestamped body
417 * kinematics measurements.
418 * @param commonAxisUsed indicates whether z-axis is
419 * assumed to be common for
420 * accelerometer and gyroscope.
421 * @param estimateGDependentCrossBiases true if G-dependent cross biases
422 * will be estimated, false
423 * otherwise.
424 * @param initialBias initial gyroscope bias to be used to find a
425 * solution. This must have length 3 and is expressed
426 * in radians per second (rad/s).
427 * @param initialMg initial gyroscope scale factors and cross coupling
428 * errors matrix. Must be 3x3.
429 * @param initialGg initial gyroscope G-dependent cross biases
430 * introduced on the gyroscope by the specific forces
431 * sensed by the accelerometer. Must be 3x3.
432 * @param listener listener to handle events raised by this
433 * calibrator.
434 * @throws IllegalArgumentException if any of the provided values does
435 * not have proper size.
436 */
437 public RANSACRobustEasyGyroscopeCalibrator(
438 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
439 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
440 final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener) {
441 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
442 }
443
444 /**
445 * Constructor.
446 *
447 * @param sequences collection of sequences containing timestamped body
448 * kinematics measurements.
449 * @param commonAxisUsed indicates whether z-axis is
450 * assumed to be common for
451 * accelerometer and gyroscope.
452 * @param estimateGDependentCrossBiases true if G-dependent cross biases
453 * will be estimated, false
454 * otherwise.
455 * @param initialBias initial gyroscope bias to be used to find a
456 * solution. This must have length 3 and is expressed
457 * in radians per second (rad/s).
458 * @param initialMg initial gyroscope scale factors and cross coupling
459 * errors matrix. Must be 3x3.
460 * @param initialGg initial gyroscope G-dependent cross biases
461 * introduced on the gyroscope by the specific forces
462 * sensed by the accelerometer. Must be 3x3.
463 * @param accelerometerBias known accelerometer bias. This
464 * must have length 3 and is
465 * expressed in meters per squared
466 * second (m/s^2).
467 * @param accelerometerMa known accelerometer scale factors
468 * and cross coupling matrix. Must
469 * be 3x3.
470 * @throws IllegalArgumentException if any of the provided values does
471 * not have proper size.
472 */
473 public RANSACRobustEasyGyroscopeCalibrator(
474 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
475 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
476 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
477 final Matrix accelerometerMa) {
478 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
479 accelerometerBias, accelerometerMa);
480 }
481
482 /**
483 * Constructor.
484 *
485 * @param sequences collection of sequences containing timestamped body
486 * kinematics measurements.
487 * @param commonAxisUsed indicates whether z-axis is
488 * assumed to be common for
489 * accelerometer and gyroscope.
490 * @param estimateGDependentCrossBiases true if G-dependent cross biases
491 * will be estimated, false
492 * otherwise.
493 * @param initialBias initial gyroscope bias to be used to find a
494 * solution. This must have length 3 and is expressed
495 * in radians per second (rad/s).
496 * @param initialMg initial gyroscope scale factors and cross coupling
497 * errors matrix. Must be 3x3.
498 * @param initialGg initial gyroscope G-dependent cross biases
499 * introduced on the gyroscope by the specific forces
500 * sensed by the accelerometer. Must be 3x3.
501 * @param accelerometerBias known accelerometer bias. This
502 * must have length 3 and is
503 * expressed in meters per squared
504 * second (m/s^2).
505 * @param accelerometerMa known accelerometer scale factors
506 * and cross coupling matrix. Must
507 * be 3x3.
508 * @param listener listener to handle events raised by this
509 * calibrator.
510 * @throws IllegalArgumentException if any of the provided values does
511 * not have proper size.
512 */
513 public RANSACRobustEasyGyroscopeCalibrator(
514 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
515 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
516 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
517 final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
518 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
519 accelerometerBias, accelerometerMa, listener);
520 }
521
522 /**
523 * Constructor.
524 *
525 * @param sequences collection of sequences containing timestamped body
526 * kinematics measurements.
527 * @param commonAxisUsed indicates whether z-axis is
528 * assumed to be common for
529 * accelerometer and gyroscope.
530 * @param estimateGDependentCrossBiases true if G-dependent cross biases
531 * will be estimated, false
532 * otherwise.
533 * @param initialBias initial gyroscope bias to be used to find a
534 * solution. This must be 3x1 and is expressed
535 * in radians per second (rad/s).
536 * @param initialMg initial gyroscope scale factors and cross coupling
537 * errors matrix. Must be 3x3.
538 * @param initialGg initial gyroscope G-dependent cross biases
539 * introduced on the gyroscope by the specific forces
540 * sensed by the accelerometer. Must be 3x3.
541 * @param accelerometerBias known accelerometer bias. This
542 * must have length 3 and is
543 * expressed in meters per squared
544 * second (m/s^2).
545 * @param accelerometerMa known accelerometer scale factors
546 * and cross coupling matrix. Must
547 * be 3x3.
548 * @throws IllegalArgumentException if any of the provided values does
549 * not have proper size.
550 */
551 public RANSACRobustEasyGyroscopeCalibrator(
552 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
553 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
554 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
555 final Matrix accelerometerMa) {
556 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
557 accelerometerBias, accelerometerMa);
558 }
559
560 /**
561 * Constructor.
562 *
563 * @param sequences collection of sequences containing timestamped body
564 * kinematics measurements.
565 * @param commonAxisUsed indicates whether z-axis is
566 * assumed to be common for
567 * accelerometer and gyroscope.
568 * @param estimateGDependentCrossBiases true if G-dependent cross biases
569 * will be estimated, false
570 * otherwise.
571 * @param initialBias initial gyroscope bias to be used to find a
572 * solution. This must be 3x1 and is expressed
573 * in radians per second (rad/s).
574 * @param initialMg initial gyroscope scale factors and cross coupling
575 * errors matrix. Must be 3x3.
576 * @param initialGg initial gyroscope G-dependent cross biases
577 * introduced on the gyroscope by the specific forces
578 * sensed by the accelerometer. Must be 3x3.
579 * @param accelerometerBias known accelerometer bias. This
580 * must have length 3 and is
581 * expressed in meters per squared
582 * second (m/s^2).
583 * @param accelerometerMa known accelerometer scale factors
584 * and cross coupling matrix. Must
585 * be 3x3.
586 * @param listener listener to handle events raised by this
587 * calibrator.
588 * @throws IllegalArgumentException if any of the provided values does
589 * not have proper size.
590 */
591 public RANSACRobustEasyGyroscopeCalibrator(
592 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
593 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
594 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
595 final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
596 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
597 accelerometerBias, accelerometerMa, listener);
598 }
599
600 /**
601 * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
602 * The threshold refers to the amount of error on norm between measured angular rates and the
603 * ones generated with estimated calibration parameters provided for each sample.
604 *
605 * @return threshold to determine whether samples are inliers or not.
606 */
607 public double getThreshold() {
608 return threshold;
609 }
610
611 /**
612 * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
613 * The threshold refers to the amount of error on norm between measured angular rates and the
614 * ones generated with estimated calibration parameters provided for each sample.
615 *
616 * @param threshold threshold to determine whether samples are inliers or not.
617 * @throws IllegalArgumentException if provided value is equal or less than zero.
618 * @throws LockedException if calibrator is currently running.
619 */
620 public void setThreshold(final double threshold) throws LockedException {
621 if (running) {
622 throw new LockedException();
623 }
624 if (threshold <= MIN_THRESHOLD) {
625 throw new IllegalArgumentException();
626 }
627 this.threshold = threshold;
628 }
629
630 /**
631 * Indicates whether inliers must be computed and kept.
632 *
633 * @return true if inliers must be computed and kept, false if inliers
634 * only need to be computed but not kept.
635 */
636 public boolean isComputeAndKeepInliersEnabled() {
637 return computeAndKeepInliers;
638 }
639
640 /**
641 * Specifies whether inliers must be computed and kept.
642 *
643 * @param computeAndKeepInliers true if inliers must be computed and kept,
644 * false if inliers only need to be computed but not kept.
645 * @throws LockedException if calibrator is currently running.
646 */
647 public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
648 if (running) {
649 throw new LockedException();
650 }
651 this.computeAndKeepInliers = computeAndKeepInliers;
652 }
653
654 /**
655 * Indicates whether residuals must be computed and kept.
656 *
657 * @return true if residuals must be computed and kept, false if residuals
658 * only need to be computed but not kept.
659 */
660 public boolean isComputeAndKeepResiduals() {
661 return computeAndKeepResiduals;
662 }
663
664 /**
665 * Specifies whether residuals must be computed and kept.
666 *
667 * @param computeAndKeepResiduals true if residuals must be computed and kept,
668 * false if residuals only need to be computed but not kept.
669 * @throws LockedException if calibrator is currently running.
670 */
671 public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
672 if (running) {
673 throw new LockedException();
674 }
675 this.computeAndKeepResiduals = computeAndKeepResiduals;
676 }
677
678 /**
679 * Estimates gyroscope calibration parameters containing bias, scale factors,
680 * cross-coupling errors and G-dependent coupling.
681 *
682 * @throws LockedException if calibrator is currently running.
683 * @throws NotReadyException if calibrator is not ready.
684 * @throws CalibrationException if estimation fails for numerical reasons.
685 */
686 @SuppressWarnings("DuplicatedCode")
687 @Override
688 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
689 if (running) {
690 throw new LockedException();
691 }
692 if (!isReady()) {
693 throw new NotReadyException();
694 }
695
696 final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
697 @Override
698 public double getThreshold() {
699 return threshold;
700 }
701
702 @Override
703 public int getTotalSamples() {
704 return sequences.size();
705 }
706
707 @Override
708 public int getSubsetSize() {
709 return preliminarySubsetSize;
710 }
711
712 @Override
713 public void estimatePreliminarSolutions(
714 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
715 computePreliminarySolutions(samplesIndices, solutions);
716 }
717
718 @Override
719 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
720 return computeError(sequences.get(i), currentEstimation);
721 }
722
723 @Override
724 public boolean isReady() {
725 return RANSACRobustEasyGyroscopeCalibrator.super.isReady();
726 }
727
728 @Override
729 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
730 // no action needed
731 }
732
733 @Override
734 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
735 // no action needed
736 }
737
738 @Override
739 public void onEstimateNextIteration(
740 final RobustEstimator<PreliminaryResult> estimator, final int iteration) {
741 if (listener != null) {
742 listener.onCalibrateNextIteration(RANSACRobustEasyGyroscopeCalibrator.this, iteration);
743 }
744 }
745
746 @Override
747 public void onEstimateProgressChange(
748 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
749 if (listener != null) {
750 listener.onCalibrateProgressChange(RANSACRobustEasyGyroscopeCalibrator.this, progress);
751 }
752 }
753 });
754
755 try {
756 running = true;
757
758 if (listener != null) {
759 listener.onCalibrateStart(this);
760 }
761
762 setupAccelerationFixer();
763
764 inliersData = null;
765 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
766 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
767 innerEstimator.setConfidence(confidence);
768 innerEstimator.setMaxIterations(maxIterations);
769 innerEstimator.setProgressDelta(progressDelta);
770 final var preliminaryResult = innerEstimator.estimate();
771 inliersData = innerEstimator.getInliersData();
772
773 attemptRefine(preliminaryResult);
774
775 if (listener != null) {
776 listener.onCalibrateEnd(this);
777 }
778
779 } catch (final com.irurueta.numerical.LockedException e) {
780 throw new LockedException(e);
781 } catch (final com.irurueta.numerical.NotReadyException e) {
782 throw new NotReadyException(e);
783 } catch (final RobustEstimatorException | AlgebraException e) {
784 throw new CalibrationException(e);
785 } finally {
786 running = false;
787 }
788 }
789
790 /**
791 * Returns method being used for robust estimation.
792 *
793 * @return method being used for robust estimation.
794 */
795 @Override
796 public RobustEstimatorMethod getMethod() {
797 return RobustEstimatorMethod.RANSAC;
798 }
799
800 /**
801 * Indicates whether this calibrator requires quality scores for each
802 * measurement/sequence or not.
803 *
804 * @return true if quality scores are required, false otherwise.
805 */
806 @Override
807 public boolean isQualityScoresRequired() {
808 return false;
809 }
810 }