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