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