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.PROMedSRobustEstimator;
26 import com.irurueta.numerical.robust.PROMedSRobustEstimatorListener;
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 PROMedS 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 PROMedSRobustEasyGyroscopeCalibrator 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 * Quality scores corresponding to each provided sample.
109 * The larger the score value the better the quality of the sample.
110 */
111 private double[] qualityScores;
112
113 /**
114 * Constructor.
115 */
116 public PROMedSRobustEasyGyroscopeCalibrator() {
117 super();
118 }
119
120 /**
121 * Constructor.
122 *
123 * @param sequences collection of sequences containing timestamped body
124 * kinematics measurements.
125 * @param initialBias initial gyroscope bias to be used to find a solution.
126 * This must be 3x1 and is expressed in radians per
127 * second (rad/s).
128 * @param initialMg initial gyroscope scale factors and cross coupling
129 * errors matrix. Must be 3x3.
130 * @param initialGg initial gyroscope G-dependent cross biases
131 * introduced on the gyroscope by the specific forces
132 * sensed by the accelerometer. Must be 3x3.
133 * @throws IllegalArgumentException if any of the provided values does
134 * not have proper size.
135 */
136 public PROMedSRobustEasyGyroscopeCalibrator(
137 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
138 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg) {
139 super(sequences, initialBias, initialMg, initialGg);
140 }
141
142 /**
143 * Constructor.
144 *
145 * @param sequences collection of sequences containing timestamped body
146 * kinematics measurements.
147 * @param initialBias initial gyroscope bias to be used to find a solution.
148 * This must be 3x1 and is expressed in radians per
149 * second (rad/s).
150 * @param initialMg initial gyroscope scale factors and cross coupling
151 * errors matrix. Must be 3x3.
152 * @param initialGg initial gyroscope G-dependent cross biases
153 * introduced on the gyroscope by the specific forces
154 * sensed by the accelerometer. Must be 3x3.
155 * @param listener listener to handle events raised by this
156 * calibrator.
157 * @throws IllegalArgumentException if any of the provided values does
158 * not have proper size.
159 */
160 public PROMedSRobustEasyGyroscopeCalibrator(
161 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
162 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
163 final RobustEasyGyroscopeCalibratorListener listener) {
164 super(sequences, initialBias, initialMg, initialGg, listener);
165 }
166
167 /**
168 * Constructor.
169 *
170 * @param sequences collection of sequences containing timestamped body
171 * kinematics measurements.
172 * @param initialBias initial gyroscope bias to be used to find a
173 * solution. This must have length 3 and is expressed
174 * in radians per second (rad/s).
175 * @param initialMg initial gyroscope scale factors and cross coupling
176 * errors matrix. Must be 3x3.
177 * @param initialGg initial gyroscope G-dependent cross biases
178 * introduced on the gyroscope by the specific forces
179 * sensed by the accelerometer. Must be 3x3.
180 * @throws IllegalArgumentException if any of the provided values does
181 * not have proper size.
182 */
183 public PROMedSRobustEasyGyroscopeCalibrator(
184 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
185 final double[] initialBias, final Matrix initialMg, final Matrix initialGg) {
186 super(sequences, initialBias, initialMg, initialGg);
187 }
188
189 /**
190 * Constructor.
191 *
192 * @param sequences collection of sequences containing timestamped body
193 * kinematics measurements.
194 * @param initialBias initial gyroscope bias to be used to find a
195 * solution. This must have length 3 and is expressed
196 * in radians per second (rad/s).
197 * @param initialMg initial gyroscope scale factors and cross coupling
198 * errors matrix. Must be 3x3.
199 * @param initialGg initial gyroscope G-dependent cross biases
200 * introduced on the gyroscope by the specific forces
201 * sensed by the accelerometer. Must be 3x3.
202 * @param listener listener to handle events raised by this
203 * calibrator.
204 * @throws IllegalArgumentException if any of the provided values does
205 * not have proper size.
206 */
207 public PROMedSRobustEasyGyroscopeCalibrator(
208 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
209 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
210 final RobustEasyGyroscopeCalibratorListener listener) {
211 super(sequences, initialBias, initialMg, initialGg, listener);
212 }
213
214 /**
215 * Constructor.
216 *
217 * @param sequences collection of sequences containing timestamped body
218 * kinematics measurements.
219 * @param initialBias initial gyroscope bias to be used to find a
220 * solution. This must have length 3 and is expressed
221 * in radians per second (rad/s).
222 * @param initialMg initial gyroscope scale factors and cross coupling
223 * errors matrix. Must be 3x3.
224 * @param initialGg initial gyroscope G-dependent cross biases
225 * introduced on the gyroscope by the specific forces
226 * sensed by the accelerometer. Must be 3x3.
227 * @param accelerometerBias known accelerometer bias. This must
228 * have length 3 and is expressed in
229 * meters per squared second
230 * (m/s^2).
231 * @param accelerometerMa known accelerometer scale factors and
232 * cross coupling matrix. Must be 3x3.
233 * @throws IllegalArgumentException if any of the provided values does
234 * not have proper size.
235 */
236 public PROMedSRobustEasyGyroscopeCalibrator(
237 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
238 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
239 final double[] accelerometerBias, final Matrix accelerometerMa) {
240 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
241 }
242
243 /**
244 * Constructor.
245 *
246 * @param sequences collection of sequences containing timestamped body
247 * kinematics measurements.
248 * @param initialBias initial gyroscope bias to be used to find a
249 * solution. This must have length 3 and is expressed
250 * in radians per second (rad/s).
251 * @param initialMg initial gyroscope scale factors and cross coupling
252 * errors matrix. Must be 3x3.
253 * @param initialGg initial gyroscope G-dependent cross biases
254 * introduced on the gyroscope by the specific forces
255 * sensed by the accelerometer. Must be 3x3.
256 * @param accelerometerBias known accelerometer bias. This must
257 * have length 3 and is expressed in
258 * meters per squared second
259 * (m/s^2).
260 * @param accelerometerMa known accelerometer scale factors and
261 * cross coupling matrix. Must be 3x3.
262 * @param listener listener to handle events raised by this
263 * calibrator.
264 * @throws IllegalArgumentException if any of the provided values does
265 * not have proper size.
266 */
267 public PROMedSRobustEasyGyroscopeCalibrator(
268 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
269 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
270 final double[] accelerometerBias, final Matrix accelerometerMa,
271 final RobustEasyGyroscopeCalibratorListener listener) {
272 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
273 }
274
275 /**
276 * Constructor.
277 *
278 * @param sequences collection of sequences containing timestamped body
279 * kinematics measurements.
280 * @param initialBias initial gyroscope bias to be used to find a
281 * solution. This must be 3x1 and is expressed
282 * in radians per second (rad/s).
283 * @param initialMg initial gyroscope scale factors and cross coupling
284 * errors matrix. Must be 3x3.
285 * @param initialGg initial gyroscope G-dependent cross biases
286 * introduced on the gyroscope by the specific forces
287 * sensed by the accelerometer. Must be 3x3.
288 * @param accelerometerBias known accelerometer bias. This must be 3x1
289 * and is expressed in meters per squared
290 * second (m/s^2).
291 * @param accelerometerMa known accelerometer scale factors and
292 * cross coupling matrix. Must be 3x3.
293 * @throws IllegalArgumentException if any of the provided values does
294 * not have proper size.
295 */
296 public PROMedSRobustEasyGyroscopeCalibrator(
297 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
298 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
299 final Matrix accelerometerMa) {
300 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
301 }
302
303 /**
304 * Constructor.
305 *
306 * @param sequences collection of sequences containing timestamped body
307 * kinematics measurements.
308 * @param initialBias initial gyroscope bias to be used to find a
309 * solution. This must be 3x1 and is expressed
310 * in radians per second (rad/s).
311 * @param initialMg initial gyroscope scale factors and cross coupling
312 * errors matrix. Must be 3x3.
313 * @param initialGg initial gyroscope G-dependent cross biases
314 * introduced on the gyroscope by the specific forces
315 * sensed by the accelerometer. Must be 3x3.
316 * @param accelerometerBias known accelerometer bias. This must be 3x1
317 * and is expressed in meters per squared
318 * second (m/s^2).
319 * @param accelerometerMa known accelerometer scale factors and
320 * cross coupling matrix. Must be 3x3.
321 * @param listener listener to handle events raised by this
322 * calibrator.
323 * @throws IllegalArgumentException if any of the provided values does
324 * not have proper size.
325 */
326 public PROMedSRobustEasyGyroscopeCalibrator(
327 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
328 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
329 final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
330 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
331 }
332
333 /**
334 * Constructor.
335 *
336 * @param sequences collection of sequences containing timestamped body
337 * kinematics measurements.
338 * @param commonAxisUsed indicates whether z-axis is
339 * assumed to be common for
340 * accelerometer and gyroscope.
341 * @param estimateGDependentCrossBiases true if G-dependent cross biases
342 * will be estimated, false
343 * otherwise.
344 * @param initialBias initial gyroscope bias to be used to find a
345 * solution. This must be 3x1 and is expressed
346 * in radians per second (rad/s).
347 * @param initialMg initial gyroscope scale factors and cross coupling
348 * errors matrix. Must be 3x3.
349 * @param initialGg initial gyroscope G-dependent cross biases
350 * introduced on the gyroscope by the specific forces
351 * sensed by the accelerometer. Must be 3x3.
352 * @throws IllegalArgumentException if any of the provided values does
353 * not have proper size.
354 */
355 public PROMedSRobustEasyGyroscopeCalibrator(
356 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
357 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
358 final Matrix initialMg, final Matrix initialGg) {
359 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
360 }
361
362 /**
363 * Constructor.
364 *
365 * @param sequences collection of sequences containing timestamped body
366 * kinematics measurements.
367 * @param commonAxisUsed indicates whether z-axis is
368 * assumed to be common for
369 * accelerometer and gyroscope.
370 * @param estimateGDependentCrossBiases true if G-dependent cross biases
371 * will be estimated, false
372 * otherwise.
373 * @param initialBias initial gyroscope bias to be used to find a
374 * solution. This must be 3x1 and is expressed
375 * in radians per second (rad/s).
376 * @param initialMg initial gyroscope scale factors and cross coupling
377 * errors matrix. Must be 3x3.
378 * @param initialGg initial gyroscope G-dependent cross biases
379 * introduced on the gyroscope by the specific forces
380 * sensed by the accelerometer. Must be 3x3.
381 * @param listener listener to handle events raised by this
382 * calibrator.
383 * @throws IllegalArgumentException if any of the provided values does
384 * not have proper size.
385 */
386 public PROMedSRobustEasyGyroscopeCalibrator(
387 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
388 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
389 final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener) {
390 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
391 }
392
393 /**
394 * Constructor.
395 *
396 * @param sequences collection of sequences containing timestamped body
397 * kinematics measurements.
398 * @param commonAxisUsed indicates whether z-axis is
399 * assumed to be common for
400 * accelerometer and gyroscope.
401 * @param estimateGDependentCrossBiases true if G-dependent cross biases
402 * will be estimated, false
403 * otherwise.
404 * @param initialBias initial gyroscope bias to be used to find a
405 * solution. This must have length 3 and is expressed
406 * in radians per second (rad/s).
407 * @param initialMg initial gyroscope scale factors and cross coupling
408 * errors matrix. Must be 3x3.
409 * @param initialGg initial gyroscope G-dependent cross biases
410 * introduced on the gyroscope by the specific forces
411 * sensed by the accelerometer. Must be 3x3.
412 * @throws IllegalArgumentException if any of the provided values does
413 * not have proper size.
414 */
415 public PROMedSRobustEasyGyroscopeCalibrator(
416 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
417 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
418 final Matrix initialMg, final Matrix initialGg) {
419 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
420 }
421
422 /**
423 * Constructor.
424 *
425 * @param sequences collection of sequences containing timestamped body
426 * kinematics measurements.
427 * @param commonAxisUsed indicates whether z-axis is
428 * assumed to be common for
429 * accelerometer and gyroscope.
430 * @param estimateGDependentCrossBiases true if G-dependent cross biases
431 * will be estimated, false
432 * otherwise.
433 * @param initialBias initial gyroscope bias to be used to find a
434 * solution. This must have length 3 and is expressed
435 * in radians per second (rad/s).
436 * @param initialMg initial gyroscope scale factors and cross coupling
437 * errors matrix. Must be 3x3.
438 * @param initialGg initial gyroscope G-dependent cross biases
439 * introduced on the gyroscope by the specific forces
440 * sensed by the accelerometer. Must be 3x3.
441 * @param listener listener to handle events raised by this
442 * calibrator.
443 * @throws IllegalArgumentException if any of the provided values does
444 * not have proper size.
445 */
446 public PROMedSRobustEasyGyroscopeCalibrator(
447 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
448 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
449 final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener) {
450 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
451 }
452
453 /**
454 * Constructor.
455 *
456 * @param sequences collection of sequences containing timestamped body
457 * kinematics measurements.
458 * @param commonAxisUsed indicates whether z-axis is
459 * assumed to be common for
460 * accelerometer and gyroscope.
461 * @param estimateGDependentCrossBiases true if G-dependent cross biases
462 * will be estimated, false
463 * otherwise.
464 * @param initialBias initial gyroscope bias to be used to find a
465 * solution. This must have length 3 and is expressed
466 * in radians per second (rad/s).
467 * @param initialMg initial gyroscope scale factors and cross coupling
468 * errors matrix. Must be 3x3.
469 * @param initialGg initial gyroscope G-dependent cross biases
470 * introduced on the gyroscope by the specific forces
471 * sensed by the accelerometer. Must be 3x3.
472 * @param accelerometerBias known accelerometer bias. This
473 * must have length 3 and is
474 * expressed in meters per squared
475 * second (m/s^2).
476 * @param accelerometerMa known accelerometer scale factors
477 * and cross coupling matrix. Must
478 * be 3x3.
479 * @throws IllegalArgumentException if any of the provided values does
480 * not have proper size.
481 */
482 public PROMedSRobustEasyGyroscopeCalibrator(
483 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
484 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
485 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
486 final Matrix accelerometerMa) {
487 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
488 accelerometerBias, accelerometerMa);
489 }
490
491 /**
492 * Constructor.
493 *
494 * @param sequences collection of sequences containing timestamped body
495 * kinematics measurements.
496 * @param commonAxisUsed indicates whether z-axis is
497 * assumed to be common for
498 * accelerometer and gyroscope.
499 * @param estimateGDependentCrossBiases true if G-dependent cross biases
500 * will be estimated, false
501 * otherwise.
502 * @param initialBias initial gyroscope bias to be used to find a
503 * solution. This must have length 3 and is expressed
504 * in radians per second (rad/s).
505 * @param initialMg initial gyroscope scale factors and cross coupling
506 * errors matrix. Must be 3x3.
507 * @param initialGg initial gyroscope G-dependent cross biases
508 * introduced on the gyroscope by the specific forces
509 * sensed by the accelerometer. Must be 3x3.
510 * @param accelerometerBias known accelerometer bias. This
511 * must have length 3 and is
512 * expressed in meters per squared
513 * second (m/s^2).
514 * @param accelerometerMa known accelerometer scale factors
515 * and cross coupling matrix. Must
516 * be 3x3.
517 * @param listener listener to handle events raised by this
518 * calibrator.
519 * @throws IllegalArgumentException if any of the provided values does
520 * not have proper size.
521 */
522 public PROMedSRobustEasyGyroscopeCalibrator(
523 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
524 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
525 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
526 final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
527 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
528 accelerometerBias, accelerometerMa, listener);
529 }
530
531 /**
532 * Constructor.
533 *
534 * @param sequences collection of sequences containing timestamped body
535 * kinematics measurements.
536 * @param commonAxisUsed indicates whether z-axis is
537 * assumed to be common for
538 * accelerometer and gyroscope.
539 * @param estimateGDependentCrossBiases true if G-dependent cross biases
540 * will be estimated, false
541 * otherwise.
542 * @param initialBias initial gyroscope bias to be used to find a
543 * solution. This must be 3x1 and is expressed
544 * in radians per second (rad/s).
545 * @param initialMg initial gyroscope scale factors and cross coupling
546 * errors matrix. Must be 3x3.
547 * @param initialGg initial gyroscope G-dependent cross biases
548 * introduced on the gyroscope by the specific forces
549 * sensed by the accelerometer. Must be 3x3.
550 * @param accelerometerBias known accelerometer bias. This
551 * must have length 3 and is
552 * expressed in meters per squared
553 * second (m/s^2).
554 * @param accelerometerMa known accelerometer scale factors
555 * and cross coupling matrix. Must
556 * be 3x3.
557 * @throws IllegalArgumentException if any of the provided values does
558 * not have proper size.
559 */
560 public PROMedSRobustEasyGyroscopeCalibrator(
561 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
562 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
563 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
564 final Matrix accelerometerMa) {
565 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
566 accelerometerBias, accelerometerMa);
567 }
568
569 /**
570 * Constructor.
571 *
572 * @param sequences collection of sequences containing timestamped body
573 * kinematics measurements.
574 * @param commonAxisUsed indicates whether z-axis is
575 * assumed to be common for
576 * accelerometer and gyroscope.
577 * @param estimateGDependentCrossBiases true if G-dependent cross biases
578 * will be estimated, false
579 * otherwise.
580 * @param initialBias initial gyroscope bias to be used to find a
581 * solution. This must be 3x1 and is expressed
582 * in radians per second (rad/s).
583 * @param initialMg initial gyroscope scale factors and cross coupling
584 * errors matrix. Must be 3x3.
585 * @param initialGg initial gyroscope G-dependent cross biases
586 * introduced on the gyroscope by the specific forces
587 * sensed by the accelerometer. Must be 3x3.
588 * @param accelerometerBias known accelerometer bias. This
589 * must have length 3 and is
590 * expressed in meters per squared
591 * second (m/s^2).
592 * @param accelerometerMa known accelerometer scale factors
593 * and cross coupling matrix. Must
594 * be 3x3.
595 * @param listener listener to handle events raised by this
596 * calibrator.
597 * @throws IllegalArgumentException if any of the provided values does
598 * not have proper size.
599 */
600 public PROMedSRobustEasyGyroscopeCalibrator(
601 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
602 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
603 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
604 final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
605 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
606 initialGg, accelerometerBias, accelerometerMa, listener);
607 }
608
609 /**
610 * Constructor.
611 *
612 * @param qualityScores quality scores corresponding to each provided
613 * sequence. The larger the score value the better
614 * the quality of the sequence.
615 * @throws IllegalArgumentException if provided quality scores length
616 * is smaller than 10.
617 */
618 public PROMedSRobustEasyGyroscopeCalibrator(final double[] qualityScores) {
619 super();
620 internalSetQualityScores(qualityScores);
621 }
622
623 /**
624 * Constructor.
625 *
626 * @param qualityScores quality scores corresponding to each provided
627 * sequence. The larger the score value the better
628 * the quality of the sequence.
629 * @param sequences collection of sequences containing timestamped body
630 * kinematics measurements.
631 * @param initialBias initial gyroscope bias to be used to find a solution.
632 * This must be 3x1 and is expressed in radians per
633 * second (rad/s).
634 * @param initialMg initial gyroscope scale factors and cross coupling
635 * errors matrix. Must be 3x3.
636 * @param initialGg initial gyroscope G-dependent cross biases
637 * introduced on the gyroscope by the specific forces
638 * sensed by the accelerometer. Must be 3x3.
639 * @throws IllegalArgumentException if any of the provided values does
640 * not have proper size or if provided
641 * quality scores length is smaller
642 * than 10.
643 */
644 public PROMedSRobustEasyGyroscopeCalibrator(
645 final double[] qualityScores,
646 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
647 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg) {
648 super(sequences, initialBias, initialMg, initialGg);
649 internalSetQualityScores(qualityScores);
650 }
651
652 /**
653 * Constructor.
654 *
655 * @param qualityScores quality scores corresponding to each provided
656 * sequence. The larger the score value the better
657 * the quality of the sequence.
658 * @param sequences collection of sequences containing timestamped body
659 * kinematics measurements.
660 * @param initialBias initial gyroscope bias to be used to find a solution.
661 * This must be 3x1 and is expressed in radians per
662 * second (rad/s).
663 * @param initialMg initial gyroscope scale factors and cross coupling
664 * errors matrix. Must be 3x3.
665 * @param initialGg initial gyroscope G-dependent cross biases
666 * introduced on the gyroscope by the specific forces
667 * sensed by the accelerometer. Must be 3x3.
668 * @param listener listener to handle events raised by this
669 * calibrator.
670 * @throws IllegalArgumentException if any of the provided values does
671 * not have proper size or if provided
672 * quality scores length is smaller
673 * than 10.
674 */
675 public PROMedSRobustEasyGyroscopeCalibrator(
676 final double[] qualityScores,
677 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
678 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
679 final RobustEasyGyroscopeCalibratorListener listener) {
680 super(sequences, initialBias, initialMg, initialGg, listener);
681 internalSetQualityScores(qualityScores);
682 }
683
684 /**
685 * Constructor.
686 *
687 * @param qualityScores quality scores corresponding to each provided
688 * sequence. The larger the score value the better
689 * the quality of the sequence.
690 * @param sequences collection of sequences containing timestamped body
691 * kinematics measurements.
692 * @param initialBias initial gyroscope bias to be used to find a
693 * solution. This must have length 3 and is expressed
694 * in radians per second (rad/s).
695 * @param initialMg initial gyroscope scale factors and cross coupling
696 * errors matrix. Must be 3x3.
697 * @param initialGg initial gyroscope G-dependent cross biases
698 * introduced on the gyroscope by the specific forces
699 * sensed by the accelerometer. Must be 3x3.
700 * @throws IllegalArgumentException if any of the provided values does
701 * not have proper size or if provided
702 * quality scores length is smaller
703 * than 10.
704 */
705 public PROMedSRobustEasyGyroscopeCalibrator(
706 final double[] qualityScores,
707 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
708 final double[] initialBias, final Matrix initialMg, final Matrix initialGg) {
709 super(sequences, initialBias, initialMg, initialGg);
710 internalSetQualityScores(qualityScores);
711 }
712
713 /**
714 * Constructor.
715 *
716 * @param qualityScores quality scores corresponding to each provided
717 * sequence. The larger the score value the better
718 * the quality of the sequence.
719 * @param sequences collection of sequences containing timestamped body
720 * kinematics measurements.
721 * @param initialBias initial gyroscope bias to be used to find a
722 * solution. This must have length 3 and is expressed
723 * in radians per second (rad/s).
724 * @param initialMg initial gyroscope scale factors and cross coupling
725 * errors matrix. Must be 3x3.
726 * @param initialGg initial gyroscope G-dependent cross biases
727 * introduced on the gyroscope by the specific forces
728 * sensed by the accelerometer. Must be 3x3.
729 * @param listener listener to handle events raised by this
730 * calibrator.
731 * @throws IllegalArgumentException if any of the provided values does
732 * not have proper size or if provided
733 * quality scores length is smaller
734 * than 10.
735 */
736 public PROMedSRobustEasyGyroscopeCalibrator(
737 final double[] qualityScores,
738 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
739 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
740 final RobustEasyGyroscopeCalibratorListener listener) {
741 super(sequences, initialBias, initialMg, initialGg, listener);
742 internalSetQualityScores(qualityScores);
743 }
744
745 /**
746 * Constructor.
747 *
748 * @param qualityScores quality scores corresponding to each provided
749 * sequence. The larger the score value the better
750 * the quality of the sequence.
751 * @param sequences collection of sequences containing timestamped body
752 * kinematics measurements.
753 * @param initialBias initial gyroscope bias to be used to find a
754 * solution. This must have length 3 and is expressed
755 * in radians per second (rad/s).
756 * @param initialMg initial gyroscope scale factors and cross coupling
757 * errors matrix. Must be 3x3.
758 * @param initialGg initial gyroscope G-dependent cross biases
759 * introduced on the gyroscope by the specific forces
760 * sensed by the accelerometer. Must be 3x3.
761 * @param accelerometerBias known accelerometer bias. This must
762 * have length 3 and is expressed in
763 * meters per squared second
764 * (m/s^2).
765 * @param accelerometerMa known accelerometer scale factors and
766 * cross coupling matrix. Must be 3x3.
767 * @throws IllegalArgumentException if any of the provided values does
768 * not have proper size or if provided
769 * quality scores length is smaller
770 * than 10.
771 */
772 public PROMedSRobustEasyGyroscopeCalibrator(
773 final double[] qualityScores,
774 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
775 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
776 final double[] accelerometerBias, final Matrix accelerometerMa) {
777 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
778 internalSetQualityScores(qualityScores);
779 }
780
781 /**
782 * Constructor.
783 *
784 * @param qualityScores quality scores corresponding to each provided
785 * sequence. The larger the score value the better
786 * the quality of the sequence.
787 * @param sequences collection of sequences containing timestamped body
788 * kinematics measurements.
789 * @param initialBias initial gyroscope bias to be used to find a
790 * solution. This must have length 3 and is expressed
791 * in radians per second (rad/s).
792 * @param initialMg initial gyroscope scale factors and cross coupling
793 * errors matrix. Must be 3x3.
794 * @param initialGg initial gyroscope G-dependent cross biases
795 * introduced on the gyroscope by the specific forces
796 * sensed by the accelerometer. Must be 3x3.
797 * @param accelerometerBias known accelerometer bias. This must
798 * have length 3 and is expressed in
799 * meters per squared second
800 * (m/s^2).
801 * @param accelerometerMa known accelerometer scale factors and
802 * cross coupling matrix. Must be 3x3.
803 * @param listener listener to handle events raised by this
804 * calibrator.
805 * @throws IllegalArgumentException if any of the provided values does
806 * not have proper size or if provided
807 * quality scores length is smaller
808 * than 10.
809 */
810 public PROMedSRobustEasyGyroscopeCalibrator(
811 final double[] qualityScores,
812 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
813 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
814 final double[] accelerometerBias, final Matrix accelerometerMa,
815 final RobustEasyGyroscopeCalibratorListener listener) {
816 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
817 internalSetQualityScores(qualityScores);
818 }
819
820 /**
821 * Constructor.
822 *
823 * @param qualityScores quality scores corresponding to each provided
824 * sequence. The larger the score value the better
825 * the quality of the sequence.
826 * @param sequences collection of sequences containing timestamped body
827 * kinematics measurements.
828 * @param initialBias initial gyroscope bias to be used to find a
829 * solution. This must be 3x1 and is expressed
830 * in radians per second (rad/s).
831 * @param initialMg initial gyroscope scale factors and cross coupling
832 * errors matrix. Must be 3x3.
833 * @param initialGg initial gyroscope G-dependent cross biases
834 * introduced on the gyroscope by the specific forces
835 * sensed by the accelerometer. Must be 3x3.
836 * @param accelerometerBias known accelerometer bias. This must be 3x1
837 * and is expressed in meters per squared
838 * second (m/s^2).
839 * @param accelerometerMa known accelerometer scale factors and
840 * cross coupling matrix. Must be 3x3.
841 * @throws IllegalArgumentException if any of the provided values does
842 * not have proper size or if provided
843 * quality scores length is smaller
844 * than 10.
845 */
846 public PROMedSRobustEasyGyroscopeCalibrator(
847 final double[] qualityScores,
848 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
849 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
850 final Matrix accelerometerMa) {
851 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
852 internalSetQualityScores(qualityScores);
853 }
854
855 /**
856 * Constructor.
857 *
858 * @param qualityScores quality scores corresponding to each provided
859 * sequence. The larger the score value the better
860 * the quality of the sequence.
861 * @param sequences collection of sequences containing timestamped body
862 * kinematics measurements.
863 * @param initialBias initial gyroscope bias to be used to find a
864 * solution. This must be 3x1 and is expressed
865 * in radians per second (rad/s).
866 * @param initialMg initial gyroscope scale factors and cross coupling
867 * errors matrix. Must be 3x3.
868 * @param initialGg initial gyroscope G-dependent cross biases
869 * introduced on the gyroscope by the specific forces
870 * sensed by the accelerometer. Must be 3x3.
871 * @param accelerometerBias known accelerometer bias. This must be 3x1
872 * and is expressed in meters per squared
873 * second (m/s^2).
874 * @param accelerometerMa known accelerometer scale factors and
875 * cross coupling matrix. Must be 3x3.
876 * @param listener listener to handle events raised by this
877 * calibrator.
878 * @throws IllegalArgumentException if any of the provided values does
879 * not have proper size or if provided
880 * quality scores length is smaller
881 * than 10.
882 */
883 public PROMedSRobustEasyGyroscopeCalibrator(
884 final double[] qualityScores,
885 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
886 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
887 final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
888 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
889 internalSetQualityScores(qualityScores);
890 }
891
892 /**
893 * Constructor.
894 *
895 * @param qualityScores quality scores corresponding to each provided
896 * sequence. The larger the score value the better
897 * the quality of the sequence.
898 * @param sequences collection of sequences containing timestamped body
899 * kinematics measurements.
900 * @param commonAxisUsed indicates whether z-axis is
901 * assumed to be common for
902 * accelerometer and gyroscope.
903 * @param estimateGDependentCrossBiases true if G-dependent cross biases
904 * will be estimated, false
905 * otherwise.
906 * @param initialBias initial gyroscope bias to be used to find a
907 * solution. This must be 3x1 and is expressed
908 * in radians per second (rad/s).
909 * @param initialMg initial gyroscope scale factors and cross coupling
910 * errors matrix. Must be 3x3.
911 * @param initialGg initial gyroscope G-dependent cross biases
912 * introduced on the gyroscope by the specific forces
913 * sensed by the accelerometer. Must be 3x3.
914 * @throws IllegalArgumentException if any of the provided values does
915 * not have proper size or if provided
916 * quality scores length is smaller
917 * than 10.
918 */
919 public PROMedSRobustEasyGyroscopeCalibrator(
920 final double[] qualityScores,
921 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
922 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
923 final Matrix initialMg, final Matrix initialGg) {
924 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
925 internalSetQualityScores(qualityScores);
926 }
927
928 /**
929 * Constructor.
930 *
931 * @param qualityScores quality scores corresponding to each provided
932 * sequence. The larger the score value the better
933 * the quality of the sequence.
934 * @param sequences collection of sequences containing timestamped body
935 * kinematics measurements.
936 * @param commonAxisUsed indicates whether z-axis is
937 * assumed to be common for
938 * accelerometer and gyroscope.
939 * @param estimateGDependentCrossBiases true if G-dependent cross biases
940 * will be estimated, false
941 * otherwise.
942 * @param initialBias initial gyroscope bias to be used to find a
943 * solution. This must be 3x1 and is expressed
944 * in radians per second (rad/s).
945 * @param initialMg initial gyroscope scale factors and cross coupling
946 * errors matrix. Must be 3x3.
947 * @param initialGg initial gyroscope G-dependent cross biases
948 * introduced on the gyroscope by the specific forces
949 * sensed by the accelerometer. Must be 3x3.
950 * @param listener listener to handle events raised by this
951 * calibrator.
952 * @throws IllegalArgumentException if any of the provided values does
953 * not have proper size or if provided
954 * quality scores length is smaller
955 * than 10.
956 */
957 public PROMedSRobustEasyGyroscopeCalibrator(
958 final double[] qualityScores,
959 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
960 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
961 final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener) {
962 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
963 internalSetQualityScores(qualityScores);
964 }
965
966 /**
967 * Constructor.
968 *
969 * @param qualityScores quality scores corresponding to each provided
970 * sequence. The larger the score value the better
971 * the quality of the sequence.
972 * @param sequences collection of sequences containing timestamped body
973 * kinematics measurements.
974 * @param commonAxisUsed indicates whether z-axis is
975 * assumed to be common for
976 * accelerometer and gyroscope.
977 * @param estimateGDependentCrossBiases true if G-dependent cross biases
978 * will be estimated, false
979 * otherwise.
980 * @param initialBias initial gyroscope bias to be used to find a
981 * solution. This must have length 3 and is expressed
982 * in radians per second (rad/s).
983 * @param initialMg initial gyroscope scale factors and cross coupling
984 * errors matrix. Must be 3x3.
985 * @param initialGg initial gyroscope G-dependent cross biases
986 * introduced on the gyroscope by the specific forces
987 * sensed by the accelerometer. Must be 3x3.
988 * @throws IllegalArgumentException if any of the provided values does
989 * not have proper size or if provided
990 * quality scores length is smaller
991 * than 10.
992 */
993 public PROMedSRobustEasyGyroscopeCalibrator(
994 final double[] qualityScores,
995 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
996 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
997 final Matrix initialMg, final Matrix initialGg) {
998 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
999 internalSetQualityScores(qualityScores);
1000 }
1001
1002 /**
1003 * Constructor.
1004 *
1005 * @param qualityScores quality scores corresponding to each provided
1006 * sequence. The larger the score value the better
1007 * the quality of the sequence.
1008 * @param sequences collection of sequences containing timestamped body
1009 * kinematics measurements.
1010 * @param commonAxisUsed indicates whether z-axis is
1011 * assumed to be common for
1012 * accelerometer and gyroscope.
1013 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1014 * will be estimated, false
1015 * otherwise.
1016 * @param initialBias initial gyroscope bias to be used to find a
1017 * solution. This must have length 3 and is expressed
1018 * in radians per second (rad/s).
1019 * @param initialMg initial gyroscope scale factors and cross coupling
1020 * errors matrix. Must be 3x3.
1021 * @param initialGg initial gyroscope G-dependent cross biases
1022 * introduced on the gyroscope by the specific forces
1023 * sensed by the accelerometer. Must be 3x3.
1024 * @param listener listener to handle events raised by this
1025 * calibrator.
1026 * @throws IllegalArgumentException if any of the provided values does
1027 * not have proper size or if provided
1028 * quality scores length is smaller
1029 * than 10.
1030 */
1031 public PROMedSRobustEasyGyroscopeCalibrator(
1032 final double[] qualityScores,
1033 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
1034 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
1035 final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener) {
1036 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
1037 internalSetQualityScores(qualityScores);
1038 }
1039
1040 /**
1041 * Constructor.
1042 *
1043 * @param qualityScores quality scores corresponding to each provided
1044 * sequence. The larger the score value the better
1045 * the quality of the sequence.
1046 * @param sequences collection of sequences containing timestamped body
1047 * kinematics measurements.
1048 * @param commonAxisUsed indicates whether z-axis is
1049 * assumed to be common for
1050 * accelerometer and gyroscope.
1051 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1052 * will be estimated, false
1053 * otherwise.
1054 * @param initialBias initial gyroscope bias to be used to find a
1055 * solution. This must have length 3 and is expressed
1056 * in radians per second (rad/s).
1057 * @param initialMg initial gyroscope scale factors and cross coupling
1058 * errors matrix. Must be 3x3.
1059 * @param initialGg initial gyroscope G-dependent cross biases
1060 * introduced on the gyroscope by the specific forces
1061 * sensed by the accelerometer. Must be 3x3.
1062 * @param accelerometerBias known accelerometer bias. This
1063 * must have length 3 and is
1064 * expressed in meters per squared
1065 * second (m/s^2).
1066 * @param accelerometerMa known accelerometer scale factors
1067 * and cross coupling matrix. Must
1068 * be 3x3.
1069 * @throws IllegalArgumentException if any of the provided values does
1070 * not have proper size or if provided
1071 * quality scores length is smaller
1072 * than 10.
1073 */
1074 public PROMedSRobustEasyGyroscopeCalibrator(
1075 final double[] qualityScores,
1076 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
1077 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
1078 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
1079 final Matrix accelerometerMa) {
1080 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
1081 accelerometerBias, accelerometerMa);
1082 internalSetQualityScores(qualityScores);
1083 }
1084
1085 /**
1086 * Constructor.
1087 *
1088 * @param qualityScores quality scores corresponding to each provided
1089 * sequence. The larger the score value the better
1090 * the quality of the sequence.
1091 * @param sequences collection of sequences containing timestamped body
1092 * kinematics measurements.
1093 * @param commonAxisUsed indicates whether z-axis is
1094 * assumed to be common for
1095 * accelerometer and gyroscope.
1096 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1097 * will be estimated, false
1098 * otherwise.
1099 * @param initialBias initial gyroscope bias to be used to find a
1100 * solution. This must have length 3 and is expressed
1101 * in radians per second (rad/s).
1102 * @param initialMg initial gyroscope scale factors and cross coupling
1103 * errors matrix. Must be 3x3.
1104 * @param initialGg initial gyroscope G-dependent cross biases
1105 * introduced on the gyroscope by the specific forces
1106 * sensed by the accelerometer. Must be 3x3.
1107 * @param accelerometerBias known accelerometer bias. This
1108 * must have length 3 and is
1109 * expressed in meters per squared
1110 * second (m/s^2).
1111 * @param accelerometerMa known accelerometer scale factors
1112 * and cross coupling matrix. Must
1113 * be 3x3.
1114 * @param listener listener to handle events raised by this
1115 * calibrator.
1116 * @throws IllegalArgumentException if any of the provided values does
1117 * not have proper size or if provided
1118 * quality scores length is smaller
1119 * than 10.
1120 */
1121 public PROMedSRobustEasyGyroscopeCalibrator(
1122 final double[] qualityScores,
1123 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
1124 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
1125 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
1126 final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
1127 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
1128 accelerometerBias, accelerometerMa, listener);
1129 internalSetQualityScores(qualityScores);
1130 }
1131
1132 /**
1133 * Constructor.
1134 *
1135 * @param qualityScores quality scores corresponding to each provided
1136 * sequence. The larger the score value the better
1137 * the quality of the sequence.
1138 * @param sequences collection of sequences containing timestamped body
1139 * kinematics measurements.
1140 * @param commonAxisUsed indicates whether z-axis is
1141 * assumed to be common for
1142 * accelerometer and gyroscope.
1143 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1144 * will be estimated, false
1145 * otherwise.
1146 * @param initialBias initial gyroscope bias to be used to find a
1147 * solution. This must be 3x1 and is expressed
1148 * in radians per second (rad/s).
1149 * @param initialMg initial gyroscope scale factors and cross coupling
1150 * errors matrix. Must be 3x3.
1151 * @param initialGg initial gyroscope G-dependent cross biases
1152 * introduced on the gyroscope by the specific forces
1153 * sensed by the accelerometer. Must be 3x3.
1154 * @param accelerometerBias known accelerometer bias. This
1155 * must have length 3 and is
1156 * expressed in meters per squared
1157 * second (m/s^2).
1158 * @param accelerometerMa known accelerometer scale factors
1159 * and cross coupling matrix. Must
1160 * be 3x3.
1161 * @throws IllegalArgumentException if any of the provided values does
1162 * not have proper size or if provided
1163 * quality scores length is smaller
1164 * than 10.
1165 */
1166 public PROMedSRobustEasyGyroscopeCalibrator(
1167 final double[] qualityScores,
1168 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
1169 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
1170 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
1171 final Matrix accelerometerMa) {
1172 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
1173 accelerometerBias, accelerometerMa);
1174 internalSetQualityScores(qualityScores);
1175 }
1176
1177 /**
1178 * Constructor.
1179 *
1180 * @param qualityScores quality scores corresponding to each provided
1181 * sequence. The larger the score value the better
1182 * the quality of the sequence.
1183 * @param sequences collection of sequences containing timestamped body
1184 * kinematics measurements.
1185 * @param commonAxisUsed indicates whether z-axis is
1186 * assumed to be common for
1187 * accelerometer and gyroscope.
1188 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1189 * will be estimated, false
1190 * otherwise.
1191 * @param initialBias initial gyroscope bias to be used to find a
1192 * solution. This must be 3x1 and is expressed
1193 * in radians per second (rad/s).
1194 * @param initialMg initial gyroscope scale factors and cross coupling
1195 * errors matrix. Must be 3x3.
1196 * @param initialGg initial gyroscope G-dependent cross biases
1197 * introduced on the gyroscope by the specific forces
1198 * sensed by the accelerometer. Must be 3x3.
1199 * @param accelerometerBias known accelerometer bias. This
1200 * must have length 3 and is
1201 * expressed in meters per squared
1202 * second (m/s^2).
1203 * @param accelerometerMa known accelerometer scale factors
1204 * and cross coupling matrix. Must
1205 * be 3x3.
1206 * @param listener listener to handle events raised by this
1207 * calibrator.
1208 * @throws IllegalArgumentException if any of the provided values does
1209 * not have proper size or if provided
1210 * quality scores length is smaller
1211 * than 10.
1212 */
1213 public PROMedSRobustEasyGyroscopeCalibrator(
1214 final double[] qualityScores,
1215 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
1216 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
1217 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
1218 final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
1219 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
1220 accelerometerBias, accelerometerMa, listener);
1221 internalSetQualityScores(qualityScores);
1222 }
1223
1224 /**
1225 * Returns threshold to be used to keep the algorithm iterating in case that
1226 * best estimated threshold using median of residuals is not small enough.
1227 * Once a solution is found that generates a threshold below this value, the
1228 * algorithm will stop.
1229 * The stop threshold can be used to prevent the LMedS algorithm to iterate
1230 * too many times in cases where samples have a very similar accuracy.
1231 * For instance, in cases where proportion of outliers is very small (close
1232 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
1233 * iterate for a long time trying to find the best solution when indeed
1234 * there is no need to do that if a reasonable threshold has already been
1235 * reached.
1236 * Because of this behaviour the stop threshold can be set to a value much
1237 * lower than the one typically used in RANSAC, and yet the algorithm could
1238 * still produce even smaller thresholds in estimated results.
1239 *
1240 * @return stop threshold to stop the algorithm prematurely when a certain
1241 * accuracy has been reached.
1242 */
1243 public double getStopThreshold() {
1244 return stopThreshold;
1245 }
1246
1247 /**
1248 * Sets threshold to be used to keep the algorithm iterating in case that
1249 * best estimated threshold using median of residuals is not small enough.
1250 * Once a solution is found that generates a threshold below this value,
1251 * the algorithm will stop.
1252 * The stop threshold can be used to prevent the LMedS algorithm to iterate
1253 * too many times in cases where samples have a very similar accuracy.
1254 * For instance, in cases where proportion of outliers is very small (close
1255 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
1256 * iterate for a long time trying to find the best solution when indeed
1257 * there is no need to do that if a reasonable threshold has already been
1258 * reached.
1259 * Because of this behaviour the stop threshold can be set to a value much
1260 * lower than the one typically used in RANSAC, and yet the algorithm could
1261 * still produce even smaller thresholds in estimated results.
1262 *
1263 * @param stopThreshold stop threshold to stop the algorithm prematurely
1264 * when a certain accuracy has been reached.
1265 * @throws IllegalArgumentException if provided value is zero or negative.
1266 * @throws LockedException if calibrator is currently running.
1267 */
1268 public void setStopThreshold(final double stopThreshold) throws LockedException {
1269 if (running) {
1270 throw new LockedException();
1271 }
1272 if (stopThreshold <= MIN_STOP_THRESHOLD) {
1273 throw new IllegalArgumentException();
1274 }
1275
1276 this.stopThreshold = stopThreshold;
1277 }
1278
1279 /**
1280 * Returns quality scores corresponding to each provided sample.
1281 * The larger the score value the better the quality of the sample.
1282 *
1283 * @return quality scores corresponding to each sample.
1284 */
1285 @Override
1286 public double[] getQualityScores() {
1287 return qualityScores;
1288 }
1289
1290 /**
1291 * Sets quality scores corresponding to each provided sample.
1292 * The larger the score value the better the quality of the sample.
1293 *
1294 * @param qualityScores quality scores corresponding to each sample.
1295 * @throws IllegalArgumentException if provided quality scores length
1296 * is smaller than minimum required samples.
1297 * @throws LockedException if calibrator is currently running.
1298 */
1299 @Override
1300 public void setQualityScores(final double[] qualityScores) throws LockedException {
1301 if (running) {
1302 throw new LockedException();
1303 }
1304 internalSetQualityScores(qualityScores);
1305 }
1306
1307 /**
1308 * Indicates whether solver is ready to find a solution.
1309 *
1310 * @return true if solver is ready, false otherwise.
1311 */
1312 @Override
1313 public boolean isReady() {
1314 return super.isReady() && qualityScores != null && qualityScores.length == sequences.size();
1315 }
1316
1317 /**
1318 * Estimates gyroscope calibration parameters containing bias, scale factors,
1319 * cross-coupling errors and G-dependent coupling.
1320 *
1321 * @throws LockedException if calibrator is currently running.
1322 * @throws NotReadyException if calibrator is not ready.
1323 * @throws CalibrationException if estimation fails for numerical reasons.
1324 */
1325 @SuppressWarnings("DuplicatedCode")
1326 @Override
1327 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
1328 if (running) {
1329 throw new LockedException();
1330 }
1331 if (!isReady()) {
1332 throw new NotReadyException();
1333 }
1334
1335 final var innerEstimator = new PROMedSRobustEstimator<>(
1336 new PROMedSRobustEstimatorListener<PreliminaryResult>() {
1337 @Override
1338 public double[] getQualityScores() {
1339 return qualityScores;
1340 }
1341
1342 @Override
1343 public double getThreshold() {
1344 return stopThreshold;
1345 }
1346
1347 @Override
1348 public int getTotalSamples() {
1349 return sequences.size();
1350 }
1351
1352 @Override
1353 public int getSubsetSize() {
1354 return preliminarySubsetSize;
1355 }
1356
1357 @Override
1358 public void estimatePreliminarSolutions(
1359 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
1360 computePreliminarySolutions(samplesIndices, solutions);
1361 }
1362
1363 @Override
1364 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
1365 return computeError(sequences.get(i), currentEstimation);
1366 }
1367
1368 @Override
1369 public boolean isReady() {
1370 return PROMedSRobustEasyGyroscopeCalibrator.this.isReady();
1371 }
1372
1373 @Override
1374 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
1375 // no action needed
1376 }
1377
1378 @Override
1379 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
1380 // no action needed
1381 }
1382
1383 @Override
1384 public void onEstimateNextIteration(
1385 final RobustEstimator<PreliminaryResult> estimator, final int iteration) {
1386 if (listener != null) {
1387 listener.onCalibrateNextIteration(
1388 PROMedSRobustEasyGyroscopeCalibrator.this, iteration);
1389 }
1390 }
1391
1392 @Override
1393 public void onEstimateProgressChange(
1394 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
1395 if (listener != null) {
1396 listener.onCalibrateProgressChange(
1397 PROMedSRobustEasyGyroscopeCalibrator.this, progress);
1398 }
1399 }
1400 });
1401
1402 try {
1403 running = true;
1404
1405 if (listener != null) {
1406 listener.onCalibrateStart(this);
1407 }
1408
1409 setupAccelerationFixer();
1410
1411 inliersData = null;
1412 innerEstimator.setUseInlierThresholds(true);
1413 innerEstimator.setConfidence(confidence);
1414 innerEstimator.setMaxIterations(maxIterations);
1415 innerEstimator.setProgressDelta(progressDelta);
1416 final var preliminaryResult = innerEstimator.estimate();
1417 inliersData = innerEstimator.getInliersData();
1418
1419 attemptRefine(preliminaryResult);
1420
1421 if (listener != null) {
1422 listener.onCalibrateEnd(this);
1423 }
1424
1425 } catch (final com.irurueta.numerical.LockedException e) {
1426 throw new LockedException(e);
1427 } catch (final com.irurueta.numerical.NotReadyException e) {
1428 throw new NotReadyException(e);
1429 } catch (final RobustEstimatorException | AlgebraException e) {
1430 throw new CalibrationException(e);
1431 } finally {
1432 running = false;
1433 }
1434 }
1435
1436 /**
1437 * Returns method being used for robust estimation.
1438 *
1439 * @return method being used for robust estimation.
1440 */
1441 @Override
1442 public RobustEstimatorMethod getMethod() {
1443 return RobustEstimatorMethod.PROMEDS;
1444 }
1445
1446 /**
1447 * Indicates whether this calibrator requires quality scores for each
1448 * measurement/sequence or not.
1449 *
1450 * @return true if quality scores are required, false otherwise.
1451 */
1452 @Override
1453 public boolean isQualityScoresRequired() {
1454 return true;
1455 }
1456
1457 /**
1458 * Sets quality scores corresponding to each provided sample.
1459 * This method is used internally and does not check whether instance is
1460 * locked or not.
1461 *
1462 * @param qualityScores quality scores to be set.
1463 * @throws IllegalArgumentException if provided quality scores length
1464 * is smaller than 4 samples.
1465 */
1466 private void internalSetQualityScores(final double[] qualityScores) {
1467 if (qualityScores == null
1468 || qualityScores.length < TurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
1469 throw new IllegalArgumentException();
1470 }
1471
1472 this.qualityScores = qualityScores;
1473 }
1474 }