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