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