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