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.ArrayUtils;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.WrongSizeException;
21 import com.irurueta.geometry.AxisRotation3D;
22 import com.irurueta.geometry.InvalidRotationMatrixException;
23 import com.irurueta.geometry.Quaternion;
24 import com.irurueta.navigation.LockedException;
25 import com.irurueta.navigation.NotReadyException;
26 import com.irurueta.navigation.frames.CoordinateTransformation;
27 import com.irurueta.navigation.frames.ECEFPosition;
28 import com.irurueta.navigation.frames.ECEFVelocity;
29 import com.irurueta.navigation.frames.FrameType;
30 import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
31 import com.irurueta.navigation.frames.NEDFrame;
32 import com.irurueta.navigation.frames.NEDPosition;
33 import com.irurueta.navigation.frames.NEDVelocity;
34 import com.irurueta.navigation.frames.converters.ECEFtoNEDPositionVelocityConverter;
35 import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
36 import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
37 import com.irurueta.navigation.inertial.BodyKinematics;
38 import com.irurueta.navigation.inertial.INSLooselyCoupledKalmanInitializerConfig;
39 import com.irurueta.navigation.inertial.INSTightlyCoupledKalmanInitializerConfig;
40 import com.irurueta.navigation.inertial.calibration.AngularSpeedTriad;
41 import com.irurueta.navigation.inertial.calibration.CalibrationException;
42 import com.irurueta.navigation.inertial.calibration.GyroscopeBiasUncertaintySource;
43 import com.irurueta.navigation.inertial.calibration.GyroscopeCalibrationSource;
44 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
45 import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
46 import com.irurueta.numerical.robust.InliersData;
47 import com.irurueta.numerical.robust.RobustEstimatorMethod;
48 import com.irurueta.units.Acceleration;
49 import com.irurueta.units.AccelerationConverter;
50 import com.irurueta.units.AccelerationUnit;
51 import com.irurueta.units.AngularSpeed;
52 import com.irurueta.units.AngularSpeedConverter;
53 import com.irurueta.units.AngularSpeedUnit;
54 import com.irurueta.units.Time;
55 import com.irurueta.units.TimeConverter;
56 import com.irurueta.units.TimeUnit;
57
58 import java.util.ArrayList;
59 import java.util.List;
60
61 /**
62 * This is an abstract class to robustly estimate gyroscope
63 * biases, cross couplings and scaling factors
64 * along with G-dependent cross biases introduced on the gyroscope by the
65 * specific forces sensed by the accelerometer.
66 * <p>
67 * This calibrator assumes that the IMU is placed flat on a turntable spinning
68 * at constant speed, but absolute orientation or position of IMU is unknown.
69 * Turntable must rotate fast enough so that Earth rotation effects can be
70 * neglected, bus slow enough so that gyroscope readings can be properly made.
71 * <p>
72 * To use this calibrator at least 10 measurements are needed when common
73 * z-axis is assumed and G-dependent cross biases are ignored, otherwise
74 * at least 13 measurements are required when common z-axis is not assumed.
75 * If G-dependent cross biases are being estimated, then at least 19
76 * measurements are needed when common z-axis is assumed, otherwise at
77 * least 22 measurements are required when common z-axis is not assumed.
78 * <p>
79 * Measured gyroscope angular rates is assumed to follow the model shown below:
80 * <pre>
81 * Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
82 * </pre>
83 * Where:
84 * - Ωmeas is the measured gyroscope angular rates. This is a 3x1 vector.
85 * - bg is the gyroscope bias. Ideally, on a perfect gyroscope, this should be a
86 * 3x1 zero vector.
87 * - I is the 3x3 identity matrix.
88 * - Mg is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
89 * a perfect gyroscope, this should be a 3x3 zero matrix.
90 * - Ωtrue is ground-truth gyroscope angular rates.
91 * - Gg is the G-dependent cross biases introduced by the specific forces sensed
92 * by the accelerometer. Ideally, on a perfect gyroscope, this should be a 3x3
93 * zero matrix.
94 * - ftrue is ground-truth specific force. This is a 3x1 vector.
95 * - w is measurement noise. This is a 3x1 vector.
96 */
97 public abstract class RobustTurntableGyroscopeCalibrator implements GyroscopeNonLinearCalibrator,
98 UnknownBiasGyroscopeCalibrator, GyroscopeCalibrationSource, GyroscopeBiasUncertaintySource,
99 OrderedStandardDeviationBodyKinematicsGyroscopeCalibrator, QualityScoredGyroscopeCalibrator,
100 AccelerometerDependentGyroscopeCalibrator {
101
102 /**
103 * Indicates whether by default a common z-axis is assumed for both the accelerometer
104 * and gyroscope.
105 */
106 public static final boolean DEFAULT_USE_COMMON_Z_AXIS = true;
107
108 /**
109 * Indicates that by default G-dependent cross biases introduced
110 * by the accelerometer on the gyroscope are estimated.
111 */
112 public static final boolean DEFAULT_ESTIMATE_G_DEPENDENT_CROSS_BIASES = true;
113
114 /**
115 * Default turntable rotation rate.
116 */
117 public static final double DEFAULT_TURNTABLE_ROTATION_RATE =
118 TurntableGyroscopeCalibrator.DEFAULT_TURNTABLE_ROTATION_RATE;
119
120 /**
121 * Default time interval between measurements expressed in seconds (s).
122 * This is a typical value when we have 50 samples per second.
123 */
124 public static final double DEFAULT_TIME_INTERVAL = TurntableGyroscopeCalibrator.DEFAULT_TIME_INTERVAL;
125
126 /**
127 * Default robust estimator method when none is provided.
128 */
129 public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
130
131 /**
132 * Indicates that result is refined by default using a non-linear calibrator
133 * (which uses a Levenberg-Marquardt fitter).
134 */
135 public static final boolean DEFAULT_REFINE_RESULT = true;
136
137 /**
138 * Indicates that covariance is kept by default after refining result.
139 */
140 public static final boolean DEFAULT_KEEP_COVARIANCE = true;
141
142 /**
143 * Default amount of progress variation before notifying a change in estimation progress.
144 * By default this is set to 5%.
145 */
146 public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
147
148 /**
149 * Minimum allowed value for progress delta.
150 */
151 public static final float MIN_PROGRESS_DELTA = 0.0f;
152
153 /**
154 * Maximum allowed value for progress delta.
155 */
156 public static final float MAX_PROGRESS_DELTA = 1.0f;
157
158 /**
159 * Constant defining default confidence of the estimated result, which is
160 * 99%. This means that with a probability of 99% estimation will be
161 * accurate because chosen sub-samples will be inliers.
162 */
163 public static final double DEFAULT_CONFIDENCE = 0.99;
164
165 /**
166 * Default maximum allowed number of iterations.
167 */
168 public static final int DEFAULT_MAX_ITERATIONS = 5000;
169
170 /**
171 * Minimum allowed confidence value.
172 */
173 public static final double MIN_CONFIDENCE = 0.0;
174
175 /**
176 * Maximum allowed confidence value.
177 */
178 public static final double MAX_CONFIDENCE = 1.0;
179
180 /**
181 * Minimum allowed number of iterations.
182 */
183 public static final int MIN_ITERATIONS = 1;
184
185 /**
186 * Known x-coordinate of accelerometer bias to be used to fix measured
187 * specific force and find cross biases introduced by the accelerometer.
188 * This is expressed in meters per squared second (m/s^2).
189 */
190 private double accelerometerBiasX;
191
192 /**
193 * Known y-coordinate of accelerometer bias to be used to fix measured
194 * specific force and find cross biases introduced by the accelerometer.
195 * This is expressed in meters per squared second (m/s^2).
196 */
197 private double accelerometerBiasY;
198
199 /**
200 * Known z-coordinate of accelerometer bias to be used to fix measured
201 * specific force and find cross biases introduced by the accelerometer.
202 * This is expressed in meters per squared second (m/s^2).
203 */
204 private double accelerometerBiasZ;
205
206 /**
207 * Known accelerometer x scaling factor to be used to fix measured
208 * specific force and find cross biases introduced by the accelerometer.
209 */
210 private double accelerometerSx;
211
212 /**
213 * Known accelerometer y scaling factor to be used to fix measured
214 * specific force and find cross biases introduced by the accelerometer.
215 */
216 private double accelerometerSy;
217
218 /**
219 * Known accelerometer z scaling factor to be used to fix measured
220 * specific force and find cross biases introduced by the accelerometer.
221 */
222 private double accelerometerSz;
223
224 /**
225 * Known accelerometer x-y cross coupling error to be used to fix measured
226 * specific force and find cross biases introduced by the accelerometer.
227 */
228 private double accelerometerMxy;
229
230 /**
231 * Know accelerometer x-z cross coupling error to be used to fix measured
232 * specific force and find cross biases introduced by the accelerometer.
233 */
234 private double accelerometerMxz;
235
236 /**
237 * Known accelerometer y-x cross coupling error to be used to fix measured
238 * specific force and find cross biases introduced by the accelerometer.
239 */
240 private double accelerometerMyx;
241
242 /**
243 * Known accelerometer y-z cross coupling error to be used to fix measured
244 * specific force and find cross biases introduced by the accelerometer.
245 */
246 private double accelerometerMyz;
247
248 /**
249 * Known accelerometer z-x cross coupling error to be used to fix measured
250 * specific force and find cross biases introduced by the accelerometer.
251 */
252 private double accelerometerMzx;
253
254 /**
255 * Known accelerometer z-y cross coupling error to be used to fix measured
256 * specific force and find cross biases introduced by the accelerometer.
257 */
258 private double accelerometerMzy;
259
260 /**
261 * Initial x-coordinate of gyroscope bias to be used to find a solution.
262 * This is expressed in radians per second (rad/s).
263 */
264 private double initialBiasX;
265
266 /**
267 * Initial y-coordinate of gyroscope bias to be used to find a solution.
268 * This is expressed in radians per second (rad/s).
269 */
270 private double initialBiasY;
271
272 /**
273 * Initial z-coordinate of gyroscope bias to be used to find a solution.
274 * This is expressed in radians per second (rad/s).
275 */
276 private double initialBiasZ;
277
278 /**
279 * Initial gyroscope x scaling factor.
280 */
281 private double initialSx;
282
283 /**
284 * Initial gyroscope y scaling factor.
285 */
286 private double initialSy;
287
288 /**
289 * Initial gyroscope z scaling factor.
290 */
291 private double initialSz;
292
293 /**
294 * Initial gyroscope x-y cross coupling error.
295 */
296 private double initialMxy;
297
298 /**
299 * Initial gyroscope x-z cross coupling error.
300 */
301 private double initialMxz;
302
303 /**
304 * Initial gyroscope y-x cross coupling error.
305 */
306 private double initialMyx;
307
308 /**
309 * Initial gyroscope y-z cross coupling error.
310 */
311 private double initialMyz;
312
313 /**
314 * Initial gyroscope z-x cross coupling error.
315 */
316 private double initialMzx;
317
318 /**
319 * Initial gyroscope z-y cross coupling error.
320 */
321 private double initialMzy;
322
323 /**
324 * Initial G-dependent cross biases introduced on the gyroscope by the
325 * specific forces sensed by the accelerometer.
326 */
327 private Matrix initialGg;
328
329 /**
330 * Constant rotation rate at which the turntable is spinning.
331 * This is expressed in radians per second (rad/s).
332 */
333 private double turntableRotationRate = DEFAULT_TURNTABLE_ROTATION_RATE;
334
335 /**
336 * Time interval between measurements being captured expressed in
337 * second (s).
338 */
339 private double timeInterval = DEFAULT_TIME_INTERVAL;
340
341 /**
342 * Contains a collection of body kinematics measurements taken at
343 * a given position with different unknown orientations and containing
344 * the standard deviations of accelerometer and gyroscope measurements.
345 */
346 protected List<StandardDeviationBodyKinematics> measurements;
347
348 /**
349 * Position where body kinematics measures have been taken.
350 */
351 private ECEFPosition position;
352
353 /**
354 * This flag indicates whether z-axis is assumed to be common for accelerometer
355 * and gyroscope.
356 * When enabled, this eliminates 3 variables from Mg matrix.
357 */
358 private boolean commonAxisUsed = DEFAULT_USE_COMMON_Z_AXIS;
359
360 /**
361 * This flag indicates whether G-dependent cross biases are being
362 * estimated or not.
363 * When enabled, this adds 9 variables from Gg matrix.
364 */
365 private boolean estimateGDependentCrossBiases = DEFAULT_ESTIMATE_G_DEPENDENT_CROSS_BIASES;
366
367 /**
368 * Listener to be notified of events such as when calibration starts, ends or its
369 * progress significantly changes.
370 */
371 protected RobustTurntableGyroscopeCalibratorListener listener;
372
373 /**
374 * Estimated angular rate biases for each IMU axis expressed in radians per
375 * second (rad/s).
376 */
377 private double[] estimatedBiases;
378
379 /**
380 * Estimated gyroscope scale factors and cross coupling errors.
381 * This is the product of matrix Tg containing cross coupling errors and Kg
382 * containing scaling factors.
383 * So that:
384 * <pre>
385 * Mg = [sx mxy mxz] = Tg*Kg
386 * [myx sy myz]
387 * [mzx mzy sz ]
388 * </pre>
389 * Where:
390 * <pre>
391 * Kg = [sx 0 0 ]
392 * [0 sy 0 ]
393 * [0 0 sz]
394 * </pre>
395 * and
396 * <pre>
397 * Tg = [1 -alphaXy alphaXz ]
398 * [alphaYx 1 -alphaYz]
399 * [-alphaZx alphaZy 1 ]
400 * </pre>
401 * Hence:
402 * <pre>
403 * Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
404 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
405 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
406 * </pre>
407 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
408 * are considered to be zero if the gyroscope z-axis is assumed to be the same
409 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
410 * becomes upper diagonal:
411 * <pre>
412 * Mg = [sx mxy mxz]
413 * [0 sy myz]
414 * [0 0 sz ]
415 * </pre>
416 * Values of this matrix are unit-less.
417 */
418 private Matrix estimatedMg;
419
420 /**
421 * Estimated G-dependent cross biases introduced on the gyroscope by the
422 * specific forces sensed by the accelerometer.
423 * This instance allows any 3x3 matrix.
424 */
425 private Matrix estimatedGg;
426
427 /**
428 * Estimated covariance matrix for estimated parameters.
429 */
430 private Matrix estimatedCovariance;
431
432 /**
433 * Estimated chi square value.
434 */
435 private double estimatedChiSq;
436
437 /**
438 * Estimated degrees of freedom of chi square value. Degrees of freedom is equal to the number of sampled data
439 * minus the number of estimated parameters.
440 */
441 private int estimatedChiSqDegreesOfFreedom;
442
443 /**
444 * Estimated reduced chi square value. This is equal to estimated chi square value divided by its degrees of
445 * freedom. Ideally this value should be close to 1.0.
446 */
447 private double estimatedReducedChiSq;
448
449 /**
450 * Estimated mean square error respect to provided measurements.
451 */
452 private double estimatedMse;
453
454 /**
455 * Estimated probability of finding a smaller chi square value expressed as a value between 0.0 and 1.0. The smaller
456 * the found chi square value is, the better the fit of the estimated parameters to the actual parameter. Thus, the
457 * smaller the chance of finding a smaller chi square value, then the better the estimated fit is.
458 */
459 private double estimatedP;
460
461 /**
462 * Estimated measure of quality of estimated fit as a value between 0.0 and 1.0. The larger the quality value is,
463 * the better the fit that has been estimated.
464 */
465 private double estimatedQ;
466
467 /**
468 * Indicates whether calibrator is running.
469 */
470 protected boolean running;
471
472 /**
473 * Amount of progress variation before notifying a progress change during calibration.
474 */
475 protected float progressDelta = DEFAULT_PROGRESS_DELTA;
476
477 /**
478 * Amount of confidence expressed as a value between 0.0 and 1.0 (which is equivalent
479 * to 100%). The amount of confidence indicates the probability that the estimated
480 * result is correct. Usually this value will be close to 1.0, but not exactly 1.0.
481 */
482 protected double confidence = DEFAULT_CONFIDENCE;
483
484 /**
485 * Maximum allowed number of iterations. When the maximum number of iterations is
486 * exceeded, result will not be available, however an approximate result will be
487 * available for retrieval.
488 */
489 protected int maxIterations = DEFAULT_MAX_ITERATIONS;
490
491 /**
492 * Data related to inliers found after calibration.
493 */
494 protected InliersData inliersData;
495
496 /**
497 * Indicates whether result must be refined using a non linear calibrator over
498 * found inliers.
499 * If true, inliers will be computed and kept in any implementation regardless of the
500 * settings.
501 */
502 protected boolean refineResult = DEFAULT_REFINE_RESULT;
503
504 /**
505 * Size of subsets to be checked during robust estimation.
506 */
507 protected int preliminarySubsetSize = TurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_GENERAL_AND_CROSS_BIASES;
508
509 /**
510 * Indicates whether covariance must be kept after refining result.
511 * This setting is only taken into account if result is refined.
512 */
513 private boolean keepCovariance = DEFAULT_KEEP_COVARIANCE;
514
515 /**
516 * Inner non-robust calibrator.
517 */
518 private final TurntableGyroscopeCalibrator innerCalibrator = new TurntableGyroscopeCalibrator();
519
520 /**
521 * Constructor.
522 */
523 protected RobustTurntableGyroscopeCalibrator() {
524 try {
525 initialGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
526 } catch (final WrongSizeException ignore) {
527 // never happens
528 }
529 }
530
531 /**
532 * Constructor.
533 *
534 * @param position position where body kinematics measures
535 * have been taken.
536 * @param turntableRotationRate constant rotation rate at which the
537 * turntable is spinning. Must be
538 * expressed in radians per second (rad/s).
539 * @param timeInterval time interval between measurements being
540 * captured expressed in seconds (s).
541 * @param measurements collection of body kinematics
542 * measurements with standard deviations
543 * taken at the same position with zero
544 * velocity and unknown different
545 * orientations.
546 * @param initialBias initial gyroscope bias to be used to
547 * find a solution. This must be 3x1 and
548 * is expressed in radians per second
549 * (rad/s).
550 * @param initialMg initial gyroscope scale factors and
551 * cross coupling errors matrix. Must
552 * be 3x3.
553 * @param initialGg initial gyroscope G-dependent cross
554 * biases introduced on the gyroscope by
555 * the specific forces sensed by the
556 * accelerometer. Must be 3x3.
557 * @throws IllegalArgumentException if any of the provided values does
558 * not have proper size or if either
559 * turntable rotation rate or
560 * time interval is zero or negative.
561 */
562 protected RobustTurntableGyroscopeCalibrator(
563 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
564 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
565 final Matrix initialGg) {
566 this();
567 this.position = position;
568 this.measurements = measurements;
569 try {
570 setTurntableRotationRate(turntableRotationRate);
571 setTimeInterval(timeInterval);
572 setInitialBias(initialBias);
573 setInitialMg(initialMg);
574 setInitialGg(initialGg);
575 } catch (final LockedException ignore) {
576 // never happens
577 }
578 }
579
580 /**
581 * Constructor.
582 *
583 * @param position position where body kinematics measures
584 * have been taken.
585 * @param turntableRotationRate constant rotation rate at which the
586 * turntable is spinning. Must be
587 * expressed in radians per second (rad/s).
588 * @param timeInterval time interval between measurements being
589 * captured expressed in seconds (s).
590 * @param measurements collection of body kinematics
591 * measurements with standard deviations
592 * taken at the same position with zero
593 * velocity and unknown different
594 * orientations.
595 * @param initialBias initial gyroscope bias to be used to
596 * find a solution. This must be 3x1 and
597 * is expressed in radians per second
598 * (rad/s).
599 * @param initialMg initial gyroscope scale factors and
600 * cross coupling errors matrix. Must
601 * be 3x3.
602 * @param initialGg initial gyroscope G-dependent cross
603 * biases introduced on the gyroscope by
604 * the specific forces sensed by the
605 * accelerometer. Must be 3x3.
606 * @param listener listener to handle events raised by this
607 * calibrator.
608 * @throws IllegalArgumentException if any of the provided values does
609 * not have proper size or if either
610 * turntable rotation rate or
611 * time interval is zero or negative.
612 */
613 protected RobustTurntableGyroscopeCalibrator(
614 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
615 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
616 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
617 this(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg);
618 this.listener = listener;
619 }
620
621 /**
622 * Constructor.
623 *
624 * @param position position where body kinematics measures
625 * have been taken.
626 * @param turntableRotationRate constant rotation rate at which the
627 * turntable is spinning. Must be
628 * expressed in radians per second (rad/s).
629 * @param timeInterval time interval between measurements being
630 * captured expressed in seconds (s).
631 * @param measurements collection of body kinematics
632 * measurements with standard deviations
633 * taken at the same position with zero
634 * velocity and unknown different
635 * orientations.
636 * @param initialBias initial gyroscope bias to be used to
637 * find a solution. This must have
638 * length 3 and is expressed in radians
639 * per second (rad/s).
640 * @param initialMg initial gyroscope scale factors and
641 * cross coupling errors matrix. Must
642 * be 3x3.
643 * @param initialGg initial gyroscope G-dependent cross
644 * biases introduced on the gyroscope by
645 * the specific forces sensed by the
646 * accelerometer. Must be 3x3.
647 * @throws IllegalArgumentException if any of the provided values does
648 * not have proper size or if either
649 * turntable rotation rate or
650 * time interval is zero or negative.
651 */
652 protected RobustTurntableGyroscopeCalibrator(
653 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
654 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
655 final Matrix initialMg, final Matrix initialGg) {
656 this();
657 this.position = position;
658 this.measurements = measurements;
659 try {
660 setTurntableRotationRate(turntableRotationRate);
661 setTimeInterval(timeInterval);
662 setInitialBias(initialBias);
663 setInitialMg(initialMg);
664 setInitialGg(initialGg);
665 } catch (final LockedException ignore) {
666 // never happens
667 }
668 }
669
670 /**
671 * Constructor.
672 *
673 * @param position position where body kinematics measures
674 * have been taken.
675 * @param turntableRotationRate constant rotation rate at which the
676 * turntable is spinning. Must be
677 * expressed in radians per second (rad/s).
678 * @param timeInterval time interval between measurements being
679 * captured expressed in seconds (s).
680 * @param measurements collection of body kinematics
681 * measurements with standard deviations
682 * taken at the same position with zero
683 * velocity and unknown different
684 * orientations.
685 * @param initialBias initial gyroscope bias to be used to
686 * find a solution. This must have length
687 * 3 and is expressed in radians
688 * per second (rad/s).
689 * @param initialMg initial gyroscope scale factors and
690 * cross coupling errors matrix. Must
691 * be 3x3.
692 * @param initialGg initial gyroscope G-dependent cross
693 * biases introduced on the gyroscope by
694 * the specific forces sensed by the
695 * accelerometer. Must be 3x3.
696 * @param listener listener to handle events raised by
697 * this calibrator.
698 * @throws IllegalArgumentException if any of the provided values does
699 * not have proper size or if either
700 * turntable rotation rate or
701 * time interval is zero or negative.
702 */
703 protected RobustTurntableGyroscopeCalibrator(
704 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
705 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
706 final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
707 this(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg);
708 this.listener = listener;
709 }
710
711 /**
712 * Constructor.
713 *
714 * @param position position where body kinematics measures
715 * have been taken.
716 * @param turntableRotationRate constant rotation rate at which the
717 * turntable is spinning. Must be
718 * expressed in radians per second (rad/s).
719 * @param timeInterval time interval between measurements being
720 * captured expressed in seconds (s).
721 * @param measurements collection of body kinematics
722 * measurements with standard deviations
723 * taken at the same position with zero
724 * velocity and unknown different
725 * orientations.
726 * @param initialBias initial gyroscope bias to be used to
727 * find a solution. This must have length
728 * 3 and is expressed in radians per
729 * second (rad/s).
730 * @param initialMg initial gyroscope scale factors and
731 * cross coupling errors matrix. Must
732 * be 3x3.
733 * @param initialGg initial gyroscope G-dependent cross
734 * biases introduced on the gyroscope by
735 * the specific forces sensed by the
736 * accelerometer. Must be 3x3.
737 * @param accelerometerBias known accelerometer bias. This must
738 * have length 3 and is expressed in
739 * meters per squared second
740 * (m/s^2).
741 * @param accelerometerMa known accelerometer scale factors and
742 * cross coupling matrix. Must be 3x3.
743 * @throws IllegalArgumentException if any of the provided values does
744 * not have proper size or if either
745 * turntable rotation rate or
746 * time interval is zero or negative.
747 */
748 protected RobustTurntableGyroscopeCalibrator(
749 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
750 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
751 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
752 final Matrix accelerometerMa) {
753 this(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg);
754 try {
755 setAccelerometerBias(accelerometerBias);
756 setAccelerometerMa(accelerometerMa);
757 } catch (final LockedException ignore) {
758 // never happens
759 }
760 }
761
762 /**
763 * Constructor.
764 *
765 * @param position position where body kinematics measures
766 * have been taken.
767 * @param turntableRotationRate constant rotation rate at which the
768 * turntable is spinning. Must be
769 * expressed in radians per second (rad/s).
770 * @param timeInterval time interval between measurements being
771 * captured expressed in seconds (s).
772 * @param measurements collection of body kinematics
773 * measurements with standard deviations
774 * taken at the same position with zero
775 * velocity and unknown different
776 * orientations.
777 * @param initialBias initial gyroscope bias to be used to
778 * find a solution. This must have length
779 * 3 and is expressed in radians per
780 * second (rad/s).
781 * @param initialMg initial gyroscope scale factors and
782 * cross coupling errors matrix. Must
783 * be 3x3.
784 * @param initialGg initial gyroscope G-dependent cross
785 * biases introduced on the gyroscope by
786 * the specific forces sensed by the
787 * accelerometer. Must be 3x3.
788 * @param accelerometerBias known accelerometer bias. This must
789 * have length 3 and is expressed in
790 * meters per squared second (m/s^2).
791 * @param accelerometerMa known accelerometer scale factors and
792 * cross coupling matrix. Must be 3x3.
793 * @param listener listener to handle events raised by
794 * this calibrator.
795 * @throws IllegalArgumentException if any of the provided values does
796 * not have proper size or if either
797 * turntable rotation rate or
798 * time interval is zero or negative.
799 */
800 protected RobustTurntableGyroscopeCalibrator(
801 final ECEFPosition position, final double turntableRotationRate,
802 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
803 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
804 final double[] accelerometerBias, final Matrix accelerometerMa,
805 final RobustTurntableGyroscopeCalibratorListener listener) {
806 this(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
807 accelerometerBias, accelerometerMa);
808 this.listener = listener;
809 }
810
811 /**
812 * Constructor.
813 *
814 * @param position position where body kinematics measures
815 * have been taken.
816 * @param turntableRotationRate constant rotation rate at which the
817 * turntable is spinning. Must be
818 * expressed in radians per second (rad/s).
819 * @param timeInterval time interval between measurements being
820 * captured expressed in seconds (s).
821 * @param measurements collection of body kinematics
822 * measurements with standard deviations
823 * taken at the same position with zero
824 * velocity and unknown different
825 * orientations.
826 * @param initialBias initial gyroscope bias to be used to
827 * find a solution. This must be 3x1 and
828 * is expressed in radians per second
829 * (rad/s).
830 * @param initialMg initial gyroscope scale factors and
831 * cross coupling errors matrix. Must
832 * be 3x3.
833 * @param initialGg initial gyroscope G-dependent cross
834 * biases introduced on the gyroscope by
835 * the specific forces sensed by the
836 * accelerometer. Must be 3x3.
837 * @param accelerometerBias known accelerometer bias. This must
838 * have length 3 and is expressed in
839 * meters per squared second
840 * (m/s^2).
841 * @param accelerometerMa known accelerometer scale factors and
842 * cross coupling matrix. Must be 3x3.
843 * @throws IllegalArgumentException if any of the provided values does
844 * not have proper size or if either
845 * turntable rotation rate or
846 * time interval is zero or negative.
847 */
848 protected RobustTurntableGyroscopeCalibrator(
849 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
850 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
851 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa) {
852 this(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg);
853 try {
854 setAccelerometerBias(accelerometerBias);
855 setAccelerometerMa(accelerometerMa);
856 } catch (final LockedException ignore) {
857 // never happens
858 }
859 }
860
861 /**
862 * Constructor.
863 *
864 * @param position position where body kinematics measures
865 * have been taken.
866 * @param turntableRotationRate constant rotation rate at which the
867 * turntable is spinning. Must be
868 * expressed in radians per second (rad/s).
869 * @param timeInterval time interval between measurements being
870 * captured expressed in seconds (s).
871 * @param measurements collection of body kinematics
872 * measurements with standard deviations
873 * taken at the same position with zero
874 * velocity and unknown different
875 * orientations.
876 * @param initialBias initial gyroscope bias to be used to
877 * find a solution. This must be 3x1 and
878 * is expressed in radians per second
879 * (rad/s).
880 * @param initialMg initial gyroscope scale factors and
881 * cross coupling errors matrix. Must
882 * be 3x3.
883 * @param initialGg initial gyroscope G-dependent cross
884 * biases introduced on the gyroscope by
885 * the specific forces sensed by the
886 * accelerometer. Must be 3x3.
887 * @param accelerometerBias known accelerometer bias. This must
888 * have length 3 and is expressed in
889 * meters per squared second (m/s^2).
890 * @param accelerometerMa known accelerometer scale factors and
891 * cross coupling matrix. Must be 3x3.
892 * @param listener listener to handle events raised by
893 * this calibrator.
894 * @throws IllegalArgumentException if any of the provided values does
895 * not have proper size or if either
896 * turntable rotation rate or
897 * time interval is zero or negative.
898 */
899 protected RobustTurntableGyroscopeCalibrator(
900 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
901 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
902 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
903 final RobustTurntableGyroscopeCalibratorListener listener) {
904 this(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
905 accelerometerBias, accelerometerMa);
906 this.listener = listener;
907 }
908
909 /**
910 * Constructor.
911 *
912 * @param position position where body kinematics
913 * measures have been taken.
914 * @param turntableRotationRate constant rotation rate at which
915 * the turntable is spinning. Must
916 * be expressed in radians per
917 * second (rad/s).
918 * @param timeInterval time interval between measurements
919 * being captured expressed in
920 * seconds (s).
921 * @param measurements collection of body kinematics
922 * measurements with standard
923 * deviations taken at the same
924 * position with zero velocity
925 * and unknown different
926 * orientations.
927 * @param commonAxisUsed indicates whether z-axis is
928 * assumed to be common for
929 * accelerometer and gyroscope.
930 * @param estimateGDependentCrossBiases true if G-dependent cross biases
931 * will be estimated, false
932 * otherwise.
933 * @param initialBias initial gyroscope bias to be
934 * used to find a solution. This
935 * must be 3x1 and is expressed in
936 * radians per second (rad/s).
937 * @param initialMg initial gyroscope scale factors
938 * and cross coupling errors matrix.
939 * Must be 3x3.
940 * @param initialGg initial gyroscope G-dependent
941 * cross biases introduced on the
942 * gyroscope by the specific
943 * forces sensed by the
944 * accelerometer. Must be 3x3.
945 * @throws IllegalArgumentException if any of the provided values does
946 * not have proper size or if either
947 * turntable rotation rate or
948 * time interval is zero or negative.
949 */
950 protected RobustTurntableGyroscopeCalibrator(
951 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
952 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
953 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
954 final Matrix initialGg) {
955 this(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg);
956 this.commonAxisUsed = commonAxisUsed;
957 this.estimateGDependentCrossBiases = estimateGDependentCrossBiases;
958 }
959
960 /**
961 * Constructor.
962 *
963 * @param position position where body kinematics
964 * measures have been taken.
965 * @param turntableRotationRate constant rotation rate at which
966 * the turntable is spinning. Must
967 * be expressed in radians per
968 * second (rad/s).
969 * @param timeInterval time interval between measurements
970 * being captured expressed in
971 * seconds (s).
972 * @param measurements collection of body kinematics
973 * measurements with standard
974 * deviations taken at the same
975 * position with zero velocity and
976 * unknown different orientations.
977 * @param commonAxisUsed indicates whether z-axis is
978 * assumed to be common for
979 * accelerometer and gyroscope.
980 * @param estimateGDependentCrossBiases true if G-dependent cross
981 * biases will be estimated, false
982 * otherwise.
983 * @param initialBias initial gyroscope bias to be
984 * used to find a solution. This
985 * must be 3x1 and is expressed in
986 * radians per second (rad/s).
987 * @param initialMg initial gyroscope scale factors
988 * and cross coupling errors
989 * matrix. Must be 3x3.
990 * @param initialGg initial gyroscope G-dependent
991 * cross biases introduced on the
992 * gyroscope by the specific
993 * forces sensed by the
994 * accelerometer. Must be 3x3.
995 * @param listener listener to handle events
996 * raised by this calibrator.
997 * @throws IllegalArgumentException if any of the provided values does
998 * not have proper size or if either
999 * turntable rotation rate or
1000 * time interval is zero or negative.
1001 */
1002 protected RobustTurntableGyroscopeCalibrator(
1003 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
1004 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1005 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
1006 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
1007 this(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1008 estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
1009 this.listener = listener;
1010 }
1011
1012 /**
1013 * Constructor.
1014 *
1015 * @param position position where body kinematics
1016 * measures have been taken.
1017 * @param turntableRotationRate constant rotation rate at which
1018 * the turntable is spinning. Must
1019 * be expressed in radians per
1020 * second (rad/s).
1021 * @param timeInterval time interval between measurements
1022 * being captured expressed in
1023 * seconds (s).
1024 * @param measurements collection of body kinematics
1025 * measurements with standard
1026 * deviations taken at the same
1027 * position with zero velocity
1028 * and unknown different
1029 * orientations.
1030 * @param commonAxisUsed indicates whether z-axis is
1031 * assumed to be common for
1032 * accelerometer and gyroscope.
1033 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1034 * will be estimated, false
1035 * otherwise.
1036 * @param initialBias initial gyroscope bias to be
1037 * used to find a solution. This
1038 * must have length 3 and is
1039 * expressed in radians per second
1040 * (rad/s).
1041 * @param initialMg initial gyroscope scale factors
1042 * and cross coupling errors matrix.
1043 * Must be 3x3.
1044 * @param initialGg initial gyroscope G-dependent
1045 * cross biases introduced on the
1046 * gyroscope by the specific forces
1047 * sensed by the accelerometer.
1048 * Must be 3x3.
1049 * @throws IllegalArgumentException if any of the provided values does
1050 * not have proper size or if either
1051 * turntable rotation rate or
1052 * time interval is zero or negative.
1053 */
1054 protected RobustTurntableGyroscopeCalibrator(
1055 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
1056 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1057 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
1058 final Matrix initialGg) {
1059 this(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg);
1060 this.commonAxisUsed = commonAxisUsed;
1061 this.estimateGDependentCrossBiases = estimateGDependentCrossBiases;
1062 }
1063
1064 /**
1065 * Constructor.
1066 *
1067 * @param position position where body kinematics
1068 * measures have been taken.
1069 * @param turntableRotationRate constant rotation rate at which
1070 * the turntable is spinning. Must
1071 * be expressed in radians per
1072 * second (rad/s).
1073 * @param timeInterval time interval between measurements
1074 * being captured expressed in
1075 * seconds (s).
1076 * @param measurements collection of body kinematics
1077 * measurements with standard
1078 * deviations taken at the same
1079 * position with zero velocity
1080 * and unknown different
1081 * orientations.
1082 * @param commonAxisUsed indicates whether z-axis is
1083 * assumed to be common for
1084 * accelerometer and gyroscope.
1085 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1086 * will be estimated, false
1087 * otherwise.
1088 * @param initialBias initial gyroscope bias to be
1089 * used to find a solution. This
1090 * must have length 3 and is
1091 * expressed in radians per second
1092 * (rad/s).
1093 * @param initialMg initial gyroscope scale factors
1094 * and cross coupling errors
1095 * matrix. Must be 3x3.
1096 * @param initialGg initial gyroscope G-dependent
1097 * cross biases introduced on the
1098 * gyroscope by the specific forces
1099 * sensed by the accelerometer.
1100 * Must be 3x3.
1101 * @param listener listener to handle events raised
1102 * by this calibrator.
1103 * @throws IllegalArgumentException if any of the provided values does
1104 * not have proper size or if either
1105 * turntable rotation rate or
1106 * time interval is zero or negative.
1107 */
1108 protected RobustTurntableGyroscopeCalibrator(
1109 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
1110 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1111 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
1112 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
1113 this(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1114 estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
1115 this.listener = listener;
1116 }
1117
1118 /**
1119 * Constructor.
1120 *
1121 * @param position position where body kinematics
1122 * measures have been taken.
1123 * @param turntableRotationRate constant rotation rate at which
1124 * the turntable is spinning. Must
1125 * be expressed in radians per
1126 * second (rad/s).
1127 * @param timeInterval time interval between measurements
1128 * being captured expressed in
1129 * seconds (s).
1130 * @param measurements collection of body kinematics
1131 * measurements with standard
1132 * deviations taken at the same
1133 * position with zero velocity
1134 * and unknown different
1135 * orientations.
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
1140 * biases will be estimated,
1141 * false otherwise.
1142 * @param initialBias initial gyroscope bias to be
1143 * used to find a solution. This
1144 * must have length 3 and is
1145 * expressed in radians per second
1146 * (rad/s).
1147 * @param initialMg initial gyroscope scale factors
1148 * and cross coupling errors
1149 * matrix. Must be 3x3.
1150 * @param initialGg initial gyroscope G-dependent
1151 * cross biases introduced on the
1152 * gyroscope by the specific forces
1153 * sensed by the accelerometer.
1154 * Must be 3x3.
1155 * @param accelerometerBias known accelerometer bias. This
1156 * must have length 3 and is
1157 * expressed in meters per squared
1158 * second (m/s^2).
1159 * @param accelerometerMa known accelerometer scale factors
1160 * and cross coupling matrix. Must
1161 * be 3x3.
1162 * @throws IllegalArgumentException if any of the provided values does
1163 * not have proper size or if either
1164 * turntable rotation rate or
1165 * time interval is zero or negative.
1166 */
1167 protected RobustTurntableGyroscopeCalibrator(
1168 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
1169 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1170 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
1171 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa) {
1172 this(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
1173 accelerometerBias, accelerometerMa);
1174 this.commonAxisUsed = commonAxisUsed;
1175 this.estimateGDependentCrossBiases = estimateGDependentCrossBiases;
1176 }
1177
1178 /**
1179 * Constructor.
1180 *
1181 * @param position position where body kinematics
1182 * measures have been taken.
1183 * @param turntableRotationRate constant rotation rate at which
1184 * the turntable is spinning. Must
1185 * be expressed in radians per
1186 * second (rad/s).
1187 * @param timeInterval time interval between measurements
1188 * being captured expressed in
1189 * seconds (s).
1190 * @param measurements collection of body kinematics
1191 * measurements with standard
1192 * deviations taken at the same
1193 * position with zero velocity
1194 * and unknown different
1195 * orientations.
1196 * @param commonAxisUsed indicates whether z-axis is
1197 * assumed to be common for
1198 * accelerometer and gyroscope.
1199 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1200 * will be estimated, false
1201 * otherwise.
1202 * @param initialBias initial gyroscope bias to be used
1203 * to find a solution. This must
1204 * have length 3 and is expressed
1205 * in radians per second (rad/s).
1206 * @param initialMg initial gyroscope scale factors
1207 * and cross coupling errors matrix.
1208 * Must be 3x3.
1209 * @param initialGg initial gyroscope G-dependent
1210 * cross biases introduced on the
1211 * gyroscope by the specific forces
1212 * sensed by the accelerometer. Must
1213 * be 3x3.
1214 * @param accelerometerBias known accelerometer bias. This
1215 * must have length 3 and is
1216 * expressed in meters per squared
1217 * second (m/s^2).
1218 * @param accelerometerMa known accelerometer scale factors
1219 * and cross coupling matrix. Must
1220 * be 3x3.
1221 * @param listener listener to handle events raised
1222 * by this calibrator.
1223 * @throws IllegalArgumentException if any of the provided values does
1224 * not have proper size or if either
1225 * turntable rotation rate or
1226 * time interval is zero or negative.
1227 */
1228 protected RobustTurntableGyroscopeCalibrator(
1229 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
1230 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1231 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
1232 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
1233 final RobustTurntableGyroscopeCalibratorListener listener) {
1234 this(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1235 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
1236 this.listener = listener;
1237 }
1238
1239 /**
1240 * Constructor.
1241 *
1242 * @param position position where body kinematics
1243 * measures have been taken.
1244 * @param turntableRotationRate constant rotation rate at which
1245 * the turntable is spinning. Must
1246 * be expressed in radians per
1247 * second (rad/s).
1248 * @param timeInterval time interval between measurements
1249 * being captured expressed in
1250 * seconds (s).
1251 * @param measurements collection of body kinematics
1252 * measurements with standard
1253 * deviations taken at the same
1254 * position with zero velocity and
1255 * unknown different orientations.
1256 * @param commonAxisUsed indicates whether z-axis is
1257 * assumed to be common for
1258 * accelerometer and gyroscope.
1259 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1260 * will be estimated, false
1261 * otherwise.
1262 * @param initialBias initial gyroscope bias to be
1263 * used to find a solution. This
1264 * must be 3x1 and is expressed in
1265 * radians per second (rad/s).
1266 * @param initialMg initial gyroscope scale factors
1267 * and cross coupling errors matrix.
1268 * Must be 3x3.
1269 * @param initialGg initial gyroscope G-dependent
1270 * cross biases introduced on the
1271 * gyroscope by the specific forces
1272 * sensed by the accelerometer. Must
1273 * be 3x3.
1274 * @param accelerometerBias known accelerometer bias. This
1275 * must have length 3 and is
1276 * expressed in meters per squared
1277 * second (m/s^2).
1278 * @param accelerometerMa known accelerometer scale factors
1279 * and cross coupling matrix. Must
1280 * be 3x3.
1281 * @throws IllegalArgumentException if any of the provided values does
1282 * not have proper size or if either
1283 * turntable rotation rate or
1284 * time interval is zero or negative.
1285 */
1286 protected RobustTurntableGyroscopeCalibrator(
1287 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
1288 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1289 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
1290 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa) {
1291 this(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
1292 accelerometerBias, accelerometerMa);
1293 this.commonAxisUsed = commonAxisUsed;
1294 this.estimateGDependentCrossBiases = estimateGDependentCrossBiases;
1295 }
1296
1297 /**
1298 * Constructor.
1299 *
1300 * @param position position where body kinematics
1301 * measures have been taken.
1302 * @param turntableRotationRate constant rotation rate at which
1303 * the turntable is spinning. Must
1304 * be expressed in radians per
1305 * second (rad/s).
1306 * @param timeInterval time interval between measurements
1307 * being captured expressed in
1308 * seconds (s).
1309 * @param measurements collection of body kinematics
1310 * measurements with standard
1311 * deviations taken at the same
1312 * position with zero velocity and
1313 * unknown different orientations.
1314 * @param commonAxisUsed indicates whether z-axis is
1315 * assumed to be common for
1316 * accelerometer and gyroscope.
1317 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1318 * will be estimated, false
1319 * otherwise.
1320 * @param initialBias initial gyroscope bias to be used
1321 * to find a solution. This must be
1322 * 3x1 and is expressed in radians
1323 * per second (rad/s).
1324 * @param initialMg initial gyroscope scale factors
1325 * and cross coupling errors matrix.
1326 * Must be 3x3.
1327 * @param initialGg initial gyroscope G-dependent
1328 * cross biases introduced on the
1329 * gyroscope by the specific forces
1330 * sensed by the accelerometer. Must
1331 * be 3x3.
1332 * @param accelerometerBias known accelerometer bias. This
1333 * must have length 3 and is
1334 * expressed in meters per squared
1335 * second (m/s^2).
1336 * @param accelerometerMa known accelerometer scale factors
1337 * and cross coupling matrix. Must
1338 * be 3x3.
1339 * @param listener listener to handle events raised
1340 * by this calibrator.
1341 * @throws IllegalArgumentException if any of the provided values does
1342 * not have proper size or if either
1343 * turntable rotation rate or
1344 * time interval is zero or negative.
1345 */
1346 protected RobustTurntableGyroscopeCalibrator(
1347 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
1348 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1349 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
1350 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
1351 final RobustTurntableGyroscopeCalibratorListener listener) {
1352 this(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1353 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
1354 this.listener = listener;
1355 }
1356
1357 /**
1358 * Constructor.
1359 *
1360 * @param position position where body kinematics measures
1361 * have been taken.
1362 * @param turntableRotationRate constant rotation rate at which the
1363 * turntable is spinning. Must be
1364 * expressed in radians per second (rad/s).
1365 * @param timeInterval time interval between measurements being
1366 * captured expressed in seconds (s).
1367 * @param measurements collection of body kinematics
1368 * measurements with standard deviations
1369 * taken at the same position with zero
1370 * velocity and unknown different
1371 * orientations.
1372 * @param initialBias initial gyroscope bias to be used to
1373 * find a solution. This must be 3x1 and
1374 * is expressed in radians per second
1375 * (rad/s).
1376 * @param initialMg initial gyroscope scale factors and
1377 * cross coupling errors matrix. Must
1378 * be 3x3.
1379 * @param initialGg initial gyroscope G-dependent cross
1380 * biases introduced on the gyroscope by
1381 * the specific forces sensed by the
1382 * accelerometer. Must be 3x3.
1383 * @throws IllegalArgumentException if any of the provided values does
1384 * not have proper size or if either
1385 * turntable rotation rate or
1386 * time interval is zero or negative.
1387 */
1388 protected RobustTurntableGyroscopeCalibrator(
1389 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1390 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
1391 final Matrix initialGg) {
1392 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, initialBias, initialMg,
1393 initialGg);
1394 }
1395
1396 /**
1397 * Constructor.
1398 *
1399 * @param position position where body kinematics measures
1400 * have been taken.
1401 * @param turntableRotationRate constant rotation rate at which the
1402 * turntable is spinning. Must be
1403 * expressed in radians per second (rad/s).
1404 * @param timeInterval time interval between measurements being
1405 * captured expressed in seconds (s).
1406 * @param measurements collection of body kinematics
1407 * measurements with standard deviations
1408 * taken at the same position with zero
1409 * velocity and unknown different
1410 * orientations.
1411 * @param initialBias initial gyroscope bias to be used to
1412 * find a solution. This must be 3x1 and
1413 * is expressed in radians per second
1414 * (rad/s).
1415 * @param initialMg initial gyroscope scale factors and
1416 * cross coupling errors matrix. Must
1417 * be 3x3.
1418 * @param initialGg initial gyroscope G-dependent cross
1419 * biases introduced on the gyroscope by
1420 * the specific forces sensed by the
1421 * accelerometer. Must be 3x3.
1422 * @param listener listener to handle events raised by this
1423 * calibrator.
1424 * @throws IllegalArgumentException if any of the provided values does
1425 * not have proper size or if either
1426 * turntable rotation rate or
1427 * time interval is zero or negative.
1428 */
1429 protected RobustTurntableGyroscopeCalibrator(
1430 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1431 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
1432 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
1433 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, initialBias, initialMg,
1434 initialGg, listener);
1435 }
1436
1437 /**
1438 * Constructor.
1439 *
1440 * @param position position where body kinematics measures
1441 * have been taken.
1442 * @param turntableRotationRate constant rotation rate at which the
1443 * turntable is spinning. Must be
1444 * expressed in radians per second (rad/s).
1445 * @param timeInterval time interval between measurements being
1446 * captured expressed in seconds (s).
1447 * @param measurements collection of body kinematics
1448 * measurements with standard deviations
1449 * taken at the same position with zero
1450 * velocity and unknown different
1451 * orientations.
1452 * @param initialBias initial gyroscope bias to be used to
1453 * find a solution. This must have
1454 * length 3 and is expressed in radians
1455 * per second (rad/s).
1456 * @param initialMg initial gyroscope scale factors and
1457 * cross coupling errors matrix. Must
1458 * be 3x3.
1459 * @param initialGg initial gyroscope G-dependent cross
1460 * biases introduced on the gyroscope by
1461 * the specific forces sensed by the
1462 * accelerometer. Must be 3x3.
1463 * @throws IllegalArgumentException if any of the provided values does
1464 * not have proper size or if either
1465 * turntable rotation rate or
1466 * time interval is zero or negative.
1467 */
1468 protected RobustTurntableGyroscopeCalibrator(
1469 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1470 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
1471 final Matrix initialMg, final Matrix initialGg) {
1472 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, initialBias, initialMg,
1473 initialGg);
1474 }
1475
1476 /**
1477 * Constructor.
1478 *
1479 * @param position position where body kinematics measures
1480 * have been taken.
1481 * @param turntableRotationRate constant rotation rate at which the
1482 * turntable is spinning. Must be
1483 * expressed in radians per second (rad/s).
1484 * @param timeInterval time interval between measurements being
1485 * captured expressed in seconds (s).
1486 * @param measurements collection of body kinematics
1487 * measurements with standard deviations
1488 * taken at the same position with zero
1489 * velocity and unknown different
1490 * orientations.
1491 * @param initialBias initial gyroscope bias to be used to
1492 * find a solution. This must have length
1493 * 3 and is expressed in radians
1494 * per second (rad/s).
1495 * @param initialMg initial gyroscope scale factors and
1496 * cross coupling errors matrix. Must
1497 * be 3x3.
1498 * @param initialGg initial gyroscope G-dependent cross
1499 * biases introduced on the gyroscope by
1500 * the specific forces sensed by the
1501 * accelerometer. Must be 3x3.
1502 * @param listener listener to handle events raised by
1503 * this calibrator.
1504 * @throws IllegalArgumentException if any of the provided values does
1505 * not have proper size or if either
1506 * turntable rotation rate or
1507 * time interval is zero or negative.
1508 */
1509 protected RobustTurntableGyroscopeCalibrator(
1510 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1511 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
1512 final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
1513 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, initialBias, initialMg,
1514 initialGg, listener);
1515 }
1516
1517 /**
1518 * Constructor.
1519 *
1520 * @param position position where body kinematics measures
1521 * have been taken.
1522 * @param turntableRotationRate constant rotation rate at which the
1523 * turntable is spinning. Must be
1524 * expressed in radians per second (rad/s).
1525 * @param timeInterval time interval between measurements being
1526 * captured expressed in seconds (s).
1527 * @param measurements collection of body kinematics
1528 * measurements with standard deviations
1529 * taken at the same position with zero
1530 * velocity and unknown different
1531 * orientations.
1532 * @param initialBias initial gyroscope bias to be used to
1533 * find a solution. This must have length
1534 * 3 and is expressed in radians per
1535 * second (rad/s).
1536 * @param initialMg initial gyroscope scale factors and
1537 * cross coupling errors matrix. Must
1538 * be 3x3.
1539 * @param initialGg initial gyroscope G-dependent cross
1540 * biases introduced on the gyroscope by
1541 * the specific forces sensed by the
1542 * accelerometer. Must be 3x3.
1543 * @param accelerometerBias known accelerometer bias. This must
1544 * have length 3 and is expressed in
1545 * meters per squared second
1546 * (m/s^2).
1547 * @param accelerometerMa known accelerometer scale factors and
1548 * cross coupling matrix. Must be 3x3.
1549 * @throws IllegalArgumentException if any of the provided values does
1550 * not have proper size or if either
1551 * turntable rotation rate or
1552 * time interval is zero or negative.
1553 */
1554 protected RobustTurntableGyroscopeCalibrator(
1555 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1556 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
1557 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
1558 final Matrix accelerometerMa) {
1559 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, initialBias, initialMg,
1560 initialGg, accelerometerBias, accelerometerMa);
1561 }
1562
1563 /**
1564 * Constructor.
1565 *
1566 * @param position position where body kinematics measures
1567 * have been taken.
1568 * @param turntableRotationRate constant rotation rate at which the
1569 * turntable is spinning. Must be
1570 * expressed in radians per second (rad/s).
1571 * @param timeInterval time interval between measurements being
1572 * captured expressed in seconds (s).
1573 * @param measurements collection of body kinematics
1574 * measurements with standard deviations
1575 * taken at the same position with zero
1576 * velocity and unknown different
1577 * orientations.
1578 * @param initialBias initial gyroscope bias to be used to
1579 * find a solution. This must have length
1580 * 3 and is expressed in radians per
1581 * second (rad/s).
1582 * @param initialMg initial gyroscope scale factors and
1583 * cross coupling errors matrix. Must
1584 * be 3x3.
1585 * @param initialGg initial gyroscope G-dependent cross
1586 * biases introduced on the gyroscope by
1587 * the specific forces sensed by the
1588 * accelerometer. Must be 3x3.
1589 * @param accelerometerBias known accelerometer bias. This must
1590 * have length 3 and is expressed in
1591 * meters per squared second (m/s^2).
1592 * @param accelerometerMa known accelerometer scale factors and
1593 * cross coupling matrix. Must be 3x3.
1594 * @param listener listener to handle events raised by
1595 * this calibrator.
1596 * @throws IllegalArgumentException if any of the provided values does
1597 * not have proper size or if either
1598 * turntable rotation rate or
1599 * time interval is zero or negative.
1600 */
1601 protected RobustTurntableGyroscopeCalibrator(
1602 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1603 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
1604 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
1605 final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener) {
1606 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, initialBias, initialMg,
1607 initialGg, accelerometerBias, accelerometerMa, listener);
1608 }
1609
1610 /**
1611 * Constructor.
1612 *
1613 * @param position position where body kinematics measures
1614 * have been taken.
1615 * @param turntableRotationRate constant rotation rate at which the
1616 * turntable is spinning. Must be
1617 * expressed in radians per second (rad/s).
1618 * @param timeInterval time interval between measurements being
1619 * captured expressed in seconds (s).
1620 * @param measurements collection of body kinematics
1621 * measurements with standard deviations
1622 * taken at the same position with zero
1623 * velocity and unknown different
1624 * orientations.
1625 * @param initialBias initial gyroscope bias to be used to
1626 * find a solution. This must be 3x1 and
1627 * is expressed in radians per second
1628 * (rad/s).
1629 * @param initialMg initial gyroscope scale factors and
1630 * cross coupling errors matrix. Must
1631 * be 3x3.
1632 * @param initialGg initial gyroscope G-dependent cross
1633 * biases introduced on the gyroscope by
1634 * the specific forces sensed by the
1635 * accelerometer. Must be 3x3.
1636 * @param accelerometerBias known accelerometer bias. This must
1637 * have length 3 and is expressed in
1638 * meters per squared second
1639 * (m/s^2).
1640 * @param accelerometerMa known accelerometer scale factors and
1641 * cross coupling matrix. Must be 3x3.
1642 * @throws IllegalArgumentException if any of the provided values does
1643 * not have proper size or if either
1644 * turntable rotation rate or
1645 * time interval is zero or negative.
1646 */
1647 protected RobustTurntableGyroscopeCalibrator(
1648 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1649 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
1650 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa) {
1651 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, initialBias, initialMg,
1652 initialGg, accelerometerBias, accelerometerMa);
1653 }
1654
1655 /**
1656 * Constructor.
1657 *
1658 * @param position position where body kinematics measures
1659 * have been taken.
1660 * @param turntableRotationRate constant rotation rate at which the
1661 * turntable is spinning. Must be
1662 * expressed in radians per second (rad/s).
1663 * @param timeInterval time interval between measurements being
1664 * captured expressed in seconds (s).
1665 * @param measurements collection of body kinematics
1666 * measurements with standard deviations
1667 * taken at the same position with zero
1668 * velocity and unknown different
1669 * orientations.
1670 * @param initialBias initial gyroscope bias to be used to
1671 * find a solution. This must be 3x1 and
1672 * is expressed in radians per second
1673 * (rad/s).
1674 * @param initialMg initial gyroscope scale factors and
1675 * cross coupling errors matrix. Must
1676 * be 3x3.
1677 * @param initialGg initial gyroscope G-dependent cross
1678 * biases introduced on the gyroscope by
1679 * the specific forces sensed by the
1680 * accelerometer. Must be 3x3.
1681 * @param accelerometerBias known accelerometer bias. This must
1682 * have length 3 and is expressed in
1683 * meters per squared second (m/s^2).
1684 * @param accelerometerMa known accelerometer scale factors and
1685 * cross coupling matrix. Must be 3x3.
1686 * @param listener listener to handle events raised by
1687 * this calibrator.
1688 * @throws IllegalArgumentException if any of the provided values does
1689 * not have proper size or if either
1690 * turntable rotation rate or
1691 * time interval is zero or negative.
1692 */
1693 protected RobustTurntableGyroscopeCalibrator(
1694 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1695 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
1696 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
1697 final RobustTurntableGyroscopeCalibratorListener listener) {
1698 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, initialBias, initialMg,
1699 initialGg, accelerometerBias, accelerometerMa, listener);
1700 }
1701
1702 /**
1703 * Constructor.
1704 *
1705 * @param position position where body kinematics
1706 * measures have been taken.
1707 * @param turntableRotationRate constant rotation rate at which
1708 * the turntable is spinning. Must
1709 * be expressed in radians per
1710 * second (rad/s).
1711 * @param timeInterval time interval between measurements
1712 * being captured expressed in
1713 * seconds (s).
1714 * @param measurements collection of body kinematics
1715 * measurements with standard
1716 * deviations taken at the same
1717 * position with zero velocity
1718 * and unknown different
1719 * orientations.
1720 * @param commonAxisUsed indicates whether z-axis is
1721 * assumed to be common for
1722 * accelerometer and gyroscope.
1723 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1724 * will be estimated, false
1725 * otherwise.
1726 * @param initialBias initial gyroscope bias to be
1727 * used to find a solution. This
1728 * must be 3x1 and is expressed in
1729 * radians per second (rad/s).
1730 * @param initialMg initial gyroscope scale factors
1731 * and cross coupling errors matrix.
1732 * Must be 3x3.
1733 * @param initialGg initial gyroscope G-dependent
1734 * cross biases introduced on the
1735 * gyroscope by the specific
1736 * forces sensed by the
1737 * accelerometer. Must be 3x3.
1738 * @throws IllegalArgumentException if any of the provided values does
1739 * not have proper size or if either
1740 * turntable rotation rate or
1741 * time interval is zero or negative.
1742 */
1743 protected RobustTurntableGyroscopeCalibrator(
1744 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1745 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1746 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
1747 final Matrix initialGg) {
1748 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1749 estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
1750 }
1751
1752 /**
1753 * Constructor.
1754 *
1755 * @param position position where body kinematics
1756 * measures have been taken.
1757 * @param turntableRotationRate constant rotation rate at which
1758 * the turntable is spinning. Must
1759 * be expressed in radians per
1760 * second (rad/s).
1761 * @param timeInterval time interval between measurements
1762 * being captured expressed in
1763 * seconds (s).
1764 * @param measurements collection of body kinematics
1765 * measurements with standard
1766 * deviations taken at the same
1767 * position with zero velocity and
1768 * unknown different orientations.
1769 * @param commonAxisUsed indicates whether z-axis is
1770 * assumed to be common for
1771 * accelerometer and gyroscope.
1772 * @param estimateGDependentCrossBiases true if G-dependent cross
1773 * biases will be estimated, false
1774 * otherwise.
1775 * @param initialBias initial gyroscope bias to be
1776 * used to find a solution. This
1777 * must be 3x1 and is expressed in
1778 * radians per second (rad/s).
1779 * @param initialMg initial gyroscope scale factors
1780 * and cross coupling errors
1781 * matrix. Must be 3x3.
1782 * @param initialGg initial gyroscope G-dependent
1783 * cross biases introduced on the
1784 * gyroscope by the specific
1785 * forces sensed by the
1786 * accelerometer. Must be 3x3.
1787 * @param listener listener to handle events
1788 * raised by this calibrator.
1789 * @throws IllegalArgumentException if any of the provided values does
1790 * not have proper size or if either
1791 * turntable rotation rate or
1792 * time interval is zero or negative.
1793 */
1794 protected RobustTurntableGyroscopeCalibrator(
1795 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1796 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1797 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
1798 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
1799 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1800 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
1801 }
1802
1803 /**
1804 * Constructor.
1805 *
1806 * @param position position where body kinematics
1807 * measures have been taken.
1808 * @param turntableRotationRate constant rotation rate at which
1809 * the turntable is spinning. Must
1810 * be expressed in radians per
1811 * second (rad/s).
1812 * @param timeInterval time interval between measurements
1813 * being captured expressed in
1814 * seconds (s).
1815 * @param measurements collection of body kinematics
1816 * measurements with standard
1817 * deviations taken at the same
1818 * position with zero velocity
1819 * and unknown different
1820 * orientations.
1821 * @param commonAxisUsed indicates whether z-axis is
1822 * assumed to be common for
1823 * accelerometer and gyroscope.
1824 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1825 * will be estimated, false
1826 * otherwise.
1827 * @param initialBias initial gyroscope bias to be
1828 * used to find a solution. This
1829 * must have length 3 and is
1830 * expressed in radians per second
1831 * (rad/s).
1832 * @param initialMg initial gyroscope scale factors
1833 * and cross coupling errors matrix.
1834 * Must be 3x3.
1835 * @param initialGg initial gyroscope G-dependent
1836 * cross biases introduced on the
1837 * gyroscope by the specific forces
1838 * sensed by the accelerometer.
1839 * Must be 3x3.
1840 * @throws IllegalArgumentException if any of the provided values does
1841 * not have proper size or if either
1842 * turntable rotation rate or
1843 * time interval is zero or negative.
1844 */
1845 protected RobustTurntableGyroscopeCalibrator(
1846 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1847 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1848 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
1849 final Matrix initialGg) {
1850 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1851 estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
1852 }
1853
1854 /**
1855 * Constructor.
1856 *
1857 * @param position position where body kinematics
1858 * measures have been taken.
1859 * @param turntableRotationRate constant rotation rate at which
1860 * the turntable is spinning. Must
1861 * be expressed in radians per
1862 * second (rad/s).
1863 * @param timeInterval time interval between measurements
1864 * being captured expressed in
1865 * seconds (s).
1866 * @param measurements collection of body kinematics
1867 * measurements with standard
1868 * deviations taken at the same
1869 * position with zero velocity
1870 * and unknown different
1871 * orientations.
1872 * @param commonAxisUsed indicates whether z-axis is
1873 * assumed to be common for
1874 * accelerometer and gyroscope.
1875 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1876 * will be estimated, false
1877 * otherwise.
1878 * @param initialBias initial gyroscope bias to be
1879 * used to find a solution. This
1880 * must have length 3 and is
1881 * expressed in radians per second
1882 * (rad/s).
1883 * @param initialMg initial gyroscope scale factors
1884 * and cross coupling errors
1885 * matrix. Must be 3x3.
1886 * @param initialGg initial gyroscope G-dependent
1887 * cross biases introduced on the
1888 * gyroscope by the specific forces
1889 * sensed by the accelerometer.
1890 * Must be 3x3.
1891 * @param listener listener to handle events raised
1892 * by this calibrator.
1893 * @throws IllegalArgumentException if any of the provided values does
1894 * not have proper size or if either
1895 * turntable rotation rate or
1896 * time interval is zero or negative.
1897 */
1898 protected RobustTurntableGyroscopeCalibrator(
1899 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1900 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1901 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
1902 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
1903 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1904 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
1905 }
1906
1907 /**
1908 * Constructor.
1909 *
1910 * @param position position where body kinematics
1911 * measures have been taken.
1912 * @param turntableRotationRate constant rotation rate at which
1913 * the turntable is spinning. Must
1914 * be expressed in radians per
1915 * second (rad/s).
1916 * @param timeInterval time interval between measurements
1917 * being captured expressed in
1918 * seconds (s).
1919 * @param measurements collection of body kinematics
1920 * measurements with standard
1921 * deviations taken at the same
1922 * position with zero velocity
1923 * and unknown different
1924 * orientations.
1925 * @param commonAxisUsed indicates whether z-axis is
1926 * assumed to be common for
1927 * accelerometer and gyroscope.
1928 * @param estimateGDependentCrossBiases true if G-dependent cross
1929 * biases will be estimated,
1930 * false otherwise.
1931 * @param initialBias initial gyroscope bias to be
1932 * used to find a solution. This
1933 * must have length 3 and is
1934 * expressed in radians per second
1935 * (rad/s).
1936 * @param initialMg initial gyroscope scale factors
1937 * and cross coupling errors
1938 * matrix. Must be 3x3.
1939 * @param initialGg initial gyroscope G-dependent
1940 * cross biases introduced on the
1941 * gyroscope by the specific forces
1942 * sensed by the accelerometer.
1943 * Must be 3x3.
1944 * @param accelerometerBias known accelerometer bias. This
1945 * must have length 3 and is
1946 * expressed in meters per squared
1947 * second (m/s^2).
1948 * @param accelerometerMa known accelerometer scale factors
1949 * and cross coupling matrix. Must
1950 * be 3x3.
1951 * @throws IllegalArgumentException if any of the provided values does
1952 * not have proper size or if either
1953 * turntable rotation rate or
1954 * time interval is zero or negative.
1955 */
1956 protected RobustTurntableGyroscopeCalibrator(
1957 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1958 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1959 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
1960 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa) {
1961 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1962 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
1963 }
1964
1965 /**
1966 * Constructor.
1967 *
1968 * @param position position where body kinematics
1969 * measures have been taken.
1970 * @param turntableRotationRate constant rotation rate at which
1971 * the turntable is spinning. Must
1972 * be expressed in radians per
1973 * second (rad/s).
1974 * @param timeInterval time interval between measurements
1975 * being captured expressed in
1976 * seconds (s).
1977 * @param measurements collection of body kinematics
1978 * measurements with standard
1979 * deviations taken at the same
1980 * position with zero velocity
1981 * and unknown different
1982 * orientations.
1983 * @param commonAxisUsed indicates whether z-axis is
1984 * assumed to be common for
1985 * accelerometer and gyroscope.
1986 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1987 * will be estimated, false
1988 * otherwise.
1989 * @param initialBias initial gyroscope bias to be used
1990 * to find a solution. This must
1991 * have length 3 and is expressed
1992 * in radians per second (rad/s).
1993 * @param initialMg initial gyroscope scale factors
1994 * and cross coupling errors matrix.
1995 * Must be 3x3.
1996 * @param initialGg initial gyroscope G-dependent
1997 * cross biases introduced on the
1998 * gyroscope by the specific forces
1999 * sensed by the accelerometer. Must
2000 * be 3x3.
2001 * @param accelerometerBias known accelerometer bias. This
2002 * must have length 3 and is
2003 * expressed in meters per squared
2004 * second (m/s^2).
2005 * @param accelerometerMa known accelerometer scale factors
2006 * and cross coupling matrix. Must
2007 * be 3x3.
2008 * @param listener listener to handle events raised
2009 * by this calibrator.
2010 * @throws IllegalArgumentException if any of the provided values does
2011 * not have proper size or if either
2012 * turntable rotation rate or
2013 * time interval is zero or negative.
2014 */
2015 protected RobustTurntableGyroscopeCalibrator(
2016 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
2017 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
2018 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
2019 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
2020 final RobustTurntableGyroscopeCalibratorListener listener) {
2021 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, commonAxisUsed,
2022 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
2023 listener);
2024 }
2025
2026 /**
2027 * Constructor.
2028 *
2029 * @param position position where body kinematics
2030 * measures have been taken.
2031 * @param turntableRotationRate constant rotation rate at which
2032 * the turntable is spinning. Must
2033 * be expressed in radians per
2034 * second (rad/s).
2035 * @param timeInterval time interval between measurements
2036 * being captured expressed in
2037 * seconds (s).
2038 * @param measurements collection of body kinematics
2039 * measurements with standard
2040 * deviations taken at the same
2041 * position with zero velocity and
2042 * unknown different orientations.
2043 * @param commonAxisUsed indicates whether z-axis is
2044 * assumed to be common for
2045 * accelerometer and gyroscope.
2046 * @param estimateGDependentCrossBiases true if G-dependent cross biases
2047 * will be estimated, false
2048 * otherwise.
2049 * @param initialBias initial gyroscope bias to be
2050 * used to find a solution. This
2051 * must be 3x1 and is expressed in
2052 * radians per second (rad/s).
2053 * @param initialMg initial gyroscope scale factors
2054 * and cross coupling errors matrix.
2055 * Must be 3x3.
2056 * @param initialGg initial gyroscope G-dependent
2057 * cross biases introduced on the
2058 * gyroscope by the specific forces
2059 * sensed by the accelerometer. Must
2060 * be 3x3.
2061 * @param accelerometerBias known accelerometer bias. This
2062 * must have length 3 and is
2063 * expressed in meters per squared
2064 * second (m/s^2).
2065 * @param accelerometerMa known accelerometer scale factors
2066 * and cross coupling matrix. Must
2067 * be 3x3.
2068 * @throws IllegalArgumentException if any of the provided values does
2069 * not have proper size or if either
2070 * turntable rotation rate or
2071 * time interval is zero or negative.
2072 */
2073 protected RobustTurntableGyroscopeCalibrator(
2074 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
2075 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
2076 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
2077 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa) {
2078 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, commonAxisUsed,
2079 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
2080 }
2081
2082 /**
2083 * Constructor.
2084 *
2085 * @param position position where body kinematics
2086 * measures have been taken.
2087 * @param turntableRotationRate constant rotation rate at which
2088 * the turntable is spinning. Must
2089 * be expressed in radians per
2090 * second (rad/s).
2091 * @param timeInterval time interval between measurements
2092 * being captured expressed in
2093 * seconds (s).
2094 * @param measurements collection of body kinematics
2095 * measurements with standard
2096 * deviations taken at the same
2097 * position with zero velocity and
2098 * unknown different orientations.
2099 * @param commonAxisUsed indicates whether z-axis is
2100 * assumed to be common for
2101 * accelerometer and gyroscope.
2102 * @param estimateGDependentCrossBiases true if G-dependent cross biases
2103 * will be estimated, false
2104 * otherwise.
2105 * @param initialBias initial gyroscope bias to be used
2106 * to find a solution. This must be
2107 * 3x1 and is expressed in radians
2108 * per second (rad/s).
2109 * @param initialMg initial gyroscope scale factors
2110 * and cross coupling errors matrix.
2111 * Must be 3x3.
2112 * @param initialGg initial gyroscope G-dependent
2113 * cross biases introduced on the
2114 * gyroscope by the specific forces
2115 * sensed by the accelerometer. Must
2116 * be 3x3.
2117 * @param accelerometerBias known accelerometer bias. This
2118 * must have length 3 and is
2119 * expressed in meters per squared
2120 * second (m/s^2).
2121 * @param accelerometerMa known accelerometer scale factors
2122 * and cross coupling matrix. Must
2123 * be 3x3.
2124 * @param listener listener to handle events raised
2125 * by this calibrator.
2126 * @throws IllegalArgumentException if any of the provided values does
2127 * not have proper size or if either
2128 * turntable rotation rate or
2129 * time interval is zero or negative.
2130 */
2131 protected RobustTurntableGyroscopeCalibrator(
2132 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
2133 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
2134 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
2135 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
2136 final RobustTurntableGyroscopeCalibratorListener listener) {
2137 this(convertPosition(position), turntableRotationRate, timeInterval, measurements, commonAxisUsed,
2138 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
2139 listener);
2140 }
2141
2142 /**
2143 * Gets known x-coordinate of accelerometer bias to be used to fix
2144 * measured specific force and find cross biases introduced by the
2145 * accelerometer.
2146 * This is expressed in meters per squared second (m/s^2).
2147 *
2148 * @return known x-coordinate of accelerometer bias.
2149 */
2150 @Override
2151 public double getAccelerometerBiasX() {
2152 return accelerometerBiasX;
2153 }
2154
2155 /**
2156 * Sets known x-coordinate of accelerometer bias to be used to fix
2157 * measured specific force and find cross biases introduced by the
2158 * accelerometer.
2159 * This is expressed in meters per squared second (m/s^2).
2160 *
2161 * @param accelerometerBiasX known x-coordinate of accelerometer bias.
2162 * @throws LockedException if calibrator is currently running.
2163 */
2164 @Override
2165 public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
2166 if (running) {
2167 throw new LockedException();
2168 }
2169 this.accelerometerBiasX = accelerometerBiasX;
2170 }
2171
2172 /**
2173 * Gets known y-coordinate of accelerometer bias to be used to fix
2174 * measured specific force and find cross biases introduced by the
2175 * accelerometer.
2176 * This is expressed in meters per squared second (m/s^2).
2177 *
2178 * @return known y-coordinate of accelerometer bias.
2179 */
2180 @Override
2181 public double getAccelerometerBiasY() {
2182 return accelerometerBiasY;
2183 }
2184
2185 /**
2186 * Sets known y-coordinate of accelerometer bias to be used to fix
2187 * measured specific force and find cross biases introduced by the
2188 * accelerometer.
2189 * This is expressed in meters per squared second (m/s^2).
2190 *
2191 * @param accelerometerBiasY known y-coordinate of accelerometer bias.
2192 * @throws LockedException if calibrator is currently running.
2193 */
2194 @Override
2195 public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
2196 if (running) {
2197 throw new LockedException();
2198 }
2199 this.accelerometerBiasY = accelerometerBiasY;
2200 }
2201
2202 /**
2203 * Gets known z-coordinate of accelerometer bias to be used to fix
2204 * measured specific force and find cross biases introduced by the
2205 * accelerometer.
2206 * This is expressed in meters per squared second (m/s^2).
2207 *
2208 * @return known z-coordinate of accelerometer bias.
2209 */
2210 @Override
2211 public double getAccelerometerBiasZ() {
2212 return accelerometerBiasZ;
2213 }
2214
2215 /**
2216 * Sets known z-coordinate of accelerometer bias to be used to fix
2217 * measured specific force and find cross biases introduced by the
2218 * accelerometer.
2219 * This is expressed in meters per squared second (m/s^2).
2220 *
2221 * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
2222 * @throws LockedException if calibrator is currently running.
2223 */
2224 @Override
2225 public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
2226 if (running) {
2227 throw new LockedException();
2228 }
2229 this.accelerometerBiasZ = accelerometerBiasZ;
2230 }
2231
2232 /**
2233 * Gets known x-coordinate of accelerometer bias to be used to fix
2234 * measured specific force and find cross biases introduced by the
2235 * accelerometer.
2236 *
2237 * @return known x-coordinate of accelerometer bias.
2238 */
2239 @Override
2240 public Acceleration getAccelerometerBiasXAsAcceleration() {
2241 return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2242 }
2243
2244 /**
2245 * Gets known x-coordinate of accelerometer bias to be used to fix
2246 * measured specific force and find cross biases introduced by the
2247 * accelerometer.
2248 *
2249 * @param result instance where result data will be stored.
2250 */
2251 @Override
2252 public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
2253 result.setValue(accelerometerBiasX);
2254 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
2255 }
2256
2257 /**
2258 * Sets known x-coordinate of accelerometer bias to be used to fix
2259 * measured specific force and find cross biases introduced by the
2260 * accelerometer.
2261 *
2262 * @param accelerometerBiasX x-coordinate of accelerometer bias.
2263 * @throws LockedException if calibrator is currently running.
2264 */
2265 @Override
2266 public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
2267 if (running) {
2268 throw new LockedException();
2269 }
2270 this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
2271 }
2272
2273 /**
2274 * Gets known y-coordinate of accelerometer bias to be used to fix
2275 * measured specific force and find cross biases introduced by the
2276 * accelerometer.
2277 *
2278 * @return known y-coordinate of accelerometer bias.
2279 */
2280 @Override
2281 public Acceleration getAccelerometerBiasYAsAcceleration() {
2282 return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2283 }
2284
2285 /**
2286 * Gets known y-coordinate of accelerometer bias to be used to fix
2287 * measured specific force and find cross biases introduced by the
2288 * accelerometer.
2289 *
2290 * @param result instance where result data will be stored.
2291 */
2292 @Override
2293 public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
2294 result.setValue(accelerometerBiasY);
2295 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
2296 }
2297
2298 /**
2299 * Sets known y-coordinate of accelerometer bias to be used to fix
2300 * measured specific force and find cross biases introduced by the
2301 * accelerometer.
2302 *
2303 * @param accelerometerBiasY y-coordinate of accelerometer bias.
2304 * @throws LockedException if calibrator is currently running.
2305 */
2306 @Override
2307 public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
2308 if (running) {
2309 throw new LockedException();
2310 }
2311 this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
2312 }
2313
2314 /**
2315 * Gets known z-coordinate of accelerometer bias to be used to fix
2316 * measured specific force and find cross biases introduced by the
2317 * accelerometer.
2318 *
2319 * @return known z-coordinate of accelerometer bias.
2320 */
2321 @Override
2322 public Acceleration getAccelerometerBiasZAsAcceleration() {
2323 return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2324 }
2325
2326 /**
2327 * Gets known z-coordinate of accelerometer bias to be used to fix
2328 * measured specific force and find cross biases introduced by the
2329 * accelerometer.
2330 *
2331 * @param result instance where result data will be stored.
2332 */
2333 @Override
2334 public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
2335 result.setValue(accelerometerBiasZ);
2336 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
2337 }
2338
2339 /**
2340 * Sets known z-coordinate of accelerometer bias to be used to fix
2341 * measured specific force and find cross biases introduced by the
2342 * accelerometer.
2343 *
2344 * @param accelerometerBiasZ z-coordinate of accelerometer bias.
2345 * @throws LockedException if calibrator is currently running.
2346 */
2347 @Override
2348 public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
2349 if (running) {
2350 throw new LockedException();
2351 }
2352 this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
2353 }
2354
2355 /**
2356 * Sets known accelerometer bias to be used to fix measured specific
2357 * force and find cross biases introduced by the accelerometer.
2358 * This is expressed in meters per squared second (m/s^2).
2359 *
2360 * @param accelerometerBiasX x-coordinate of accelerometer bias.
2361 * @param accelerometerBiasY y-coordinate of accelerometer bias.
2362 * @param accelerometerBiasZ z-coordinate of accelerometer bias.
2363 * @throws LockedException if calibrator is currently running.
2364 */
2365 @Override
2366 public void setAccelerometerBias(
2367 final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
2368 throws LockedException {
2369 if (running) {
2370 throw new LockedException();
2371 }
2372
2373 this.accelerometerBiasX = accelerometerBiasX;
2374 this.accelerometerBiasY = accelerometerBiasY;
2375 this.accelerometerBiasZ = accelerometerBiasZ;
2376 }
2377
2378 /**
2379 * Sets known accelerometer bias to be used to fix measured specific
2380 * force and find cross biases introduced by the accelerometer.
2381 *
2382 * @param accelerometerBiasX x-coordinate of accelerometer bias.
2383 * @param accelerometerBiasY y-coordinate of accelerometer bias.
2384 * @param accelerometerBiasZ z-coordinate of accelerometer bias.
2385 * @throws LockedException if calibrator is currently running.
2386 */
2387 @Override
2388 public void setAccelerometerBias(
2389 final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
2390 final Acceleration accelerometerBiasZ) throws LockedException {
2391 if (running) {
2392 throw new LockedException();
2393 }
2394
2395 this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
2396 this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
2397 this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
2398 }
2399
2400 /**
2401 * Gets known accelerometer bias to be used to fix measured specific
2402 * force and find cross biases introduced by the accelerometer.
2403 * This is expressed in meters per squared second (m/s^2).
2404 *
2405 * @return known accelerometer bias.
2406 */
2407 @Override
2408 public double[] getAccelerometerBias() {
2409 final var result = new double[BodyKinematics.COMPONENTS];
2410 getAccelerometerBias(result);
2411 return result;
2412 }
2413
2414 /**
2415 * Gets known accelerometer bias to be used to fix measured specific
2416 * force and find cross biases introduced by the accelerometer.
2417 * This is expressed in meters per squared second (m/s^2).
2418 *
2419 * @param result instance where result data will be copied to.
2420 * @throws IllegalArgumentException if provided array does not have
2421 * length 3.
2422 */
2423 @Override
2424 public void getAccelerometerBias(final double[] result) {
2425 if (result.length != BodyKinematics.COMPONENTS) {
2426 throw new IllegalArgumentException();
2427 }
2428
2429 result[0] = accelerometerBiasX;
2430 result[1] = accelerometerBiasY;
2431 result[2] = accelerometerBiasZ;
2432 }
2433
2434 /**
2435 * Sets known accelerometer bias to be used to fix measured specific
2436 * force and find cross biases introduced by the accelerometer.
2437 * This is expressed in meters per squared second (m/s^2).
2438 *
2439 * @param accelerometerBias known accelerometer bias.
2440 * @throws LockedException if calibrator is currently running.
2441 * @throws IllegalArgumentException if provided array does not have
2442 * length 3.
2443 */
2444 @Override
2445 public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
2446 if (running) {
2447 throw new LockedException();
2448 }
2449
2450 if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
2451 throw new IllegalArgumentException();
2452 }
2453
2454 accelerometerBiasX = accelerometerBias[0];
2455 accelerometerBiasY = accelerometerBias[1];
2456 accelerometerBiasZ = accelerometerBias[2];
2457 }
2458
2459 /**
2460 * Gets known accelerometer bias to be used to fix measured specific
2461 * force and find cross biases introduced by the accelerometer.
2462 * This is expressed in meters per squared second (m/s^2).
2463 *
2464 * @return known accelerometer bias.
2465 */
2466 @Override
2467 public Matrix getAccelerometerBiasAsMatrix() {
2468 Matrix result;
2469 try {
2470 result = new Matrix(BodyKinematics.COMPONENTS, 1);
2471 getAccelerometerBiasAsMatrix(result);
2472 } catch (final WrongSizeException ignore) {
2473 // never happens
2474 result = null;
2475 }
2476 return result;
2477 }
2478
2479 /**
2480 * Gets known accelerometer bias to be used to fix measured specific
2481 * force and find cross biases introduced by the accelerometer.
2482 * This is expressed in meters per squared second (m/s^2).
2483 *
2484 * @param result instance where result data will be copied to.
2485 * @throws IllegalArgumentException if provided matrix is not 3x1.
2486 */
2487 @Override
2488 public void getAccelerometerBiasAsMatrix(final Matrix result) {
2489 if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
2490 throw new IllegalArgumentException();
2491 }
2492 result.setElementAtIndex(0, accelerometerBiasX);
2493 result.setElementAtIndex(1, accelerometerBiasY);
2494 result.setElementAtIndex(2, accelerometerBiasZ);
2495 }
2496
2497 /**
2498 * Sets known accelerometer bias to be used to fix measured specific
2499 * force and find cross biases introduced by the accelerometer.
2500 * This is expressed in meters per squared second (m/s^2).
2501 *
2502 * @param accelerometerBias known accelerometer bias. Must be 3x1.
2503 * @throws LockedException if calibrator is currently running.
2504 * @throws IllegalArgumentException if provided matrix is not 3x1.
2505 */
2506 @Override
2507 public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
2508 if (running) {
2509 throw new LockedException();
2510 }
2511 if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
2512 throw new IllegalArgumentException();
2513 }
2514
2515 accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
2516 accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
2517 accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
2518 }
2519
2520 /**
2521 * Gets known accelerometer x scaling factor to be used to fix measured
2522 * specific force and find cross biases introduced by the accelerometer.
2523 *
2524 * @return known accelerometer x scaling factor.
2525 */
2526 @Override
2527 public double getAccelerometerSx() {
2528 return accelerometerSx;
2529 }
2530
2531 /**
2532 * Sets known accelerometer x scaling factor to be used to fix measured
2533 * specific force and find cross biases introduced by the accelerometer.
2534 *
2535 * @param accelerometerSx known accelerometer x scaling factor.
2536 * @throws LockedException if calibrator is currently running.
2537 */
2538 @Override
2539 public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
2540 if (running) {
2541 throw new LockedException();
2542 }
2543 this.accelerometerSx = accelerometerSx;
2544 }
2545
2546 /**
2547 * Gets known accelerometer y scaling factor to be used to fix measured
2548 * specific force and find cross biases introduced by the accelerometer.
2549 *
2550 * @return known accelerometer y scaling factor.
2551 */
2552 @Override
2553 public double getAccelerometerSy() {
2554 return accelerometerSy;
2555 }
2556
2557 /**
2558 * Sets known accelerometer y scaling factor to be used to fix measured
2559 * specific force and find cross biases introduced by the accelerometer.
2560 *
2561 * @param accelerometerSy known accelerometer y scaling factor.
2562 * @throws LockedException if calibrator is currently running.
2563 */
2564 @Override
2565 public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
2566 if (running) {
2567 throw new LockedException();
2568 }
2569 this.accelerometerSy = accelerometerSy;
2570 }
2571
2572 /**
2573 * Gets known accelerometer z scaling factor to be used to fix measured
2574 * specific force and find cross biases introduced by the accelerometer.
2575 *
2576 * @return known accelerometer z scaling factor.
2577 */
2578 @Override
2579 public double getAccelerometerSz() {
2580 return accelerometerSz;
2581 }
2582
2583 /**
2584 * Sets known accelerometer z scaling factor to be used to fix measured
2585 * specific force and find cross biases introduced by the accelerometer.
2586 *
2587 * @param accelerometerSz known accelerometer z scaling factor.
2588 * @throws LockedException if calibrator is currently running.
2589 */
2590 @Override
2591 public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
2592 if (running) {
2593 throw new LockedException();
2594 }
2595 this.accelerometerSz = accelerometerSz;
2596 }
2597
2598 /**
2599 * Gets known accelerometer x-y cross coupling error to be used to fix
2600 * measured specific force and find cross biases introduced by the
2601 * accelerometer.
2602 *
2603 * @return known accelerometer x-y cross coupling error.
2604 */
2605 @Override
2606 public double getAccelerometerMxy() {
2607 return accelerometerMxy;
2608 }
2609
2610 /**
2611 * Sets known accelerometer x-y cross coupling error to be used to fix
2612 * measured specific force and find cross biases introduced by the
2613 * accelerometer.
2614 *
2615 * @param accelerometerMxy known accelerometer x-y cross coupling error.
2616 * @throws LockedException if calibrator is currently running.
2617 */
2618 @Override
2619 public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
2620 if (running) {
2621 throw new LockedException();
2622 }
2623 this.accelerometerMxy = accelerometerMxy;
2624 }
2625
2626 /**
2627 * Gets known accelerometer x-z cross coupling error to be used to fix
2628 * measured specific force and find cross biases introduced by the
2629 * accelerometer.
2630 *
2631 * @return known accelerometer x-z cross coupling error.
2632 */
2633 @Override
2634 public double getAccelerometerMxz() {
2635 return accelerometerMxz;
2636 }
2637
2638 /**
2639 * Sets known accelerometer x-z cross coupling error to be used to fix
2640 * measured specific force and find cross biases introduced by the
2641 * accelerometer.
2642 *
2643 * @param accelerometerMxz known accelerometer x-z cross coupling error.
2644 * @throws LockedException if calibrator is currently running.
2645 */
2646 @Override
2647 public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
2648 if (running) {
2649 throw new LockedException();
2650 }
2651 this.accelerometerMxz = accelerometerMxz;
2652 }
2653
2654 /**
2655 * Gets known accelerometer y-x cross coupling error to be used to fix
2656 * measured specific force and find cross biases introduced by the
2657 * accelerometer.
2658 *
2659 * @return known accelerometer y-x cross coupling error.
2660 */
2661 @Override
2662 public double getAccelerometerMyx() {
2663 return accelerometerMyx;
2664 }
2665
2666 /**
2667 * Sets known accelerometer y-x cross coupling error to be used to fix
2668 * measured specific force and find cross biases introduced by the
2669 * accelerometer.
2670 *
2671 * @param accelerometerMyx known accelerometer y-x cross coupling
2672 * error.
2673 * @throws LockedException if calibrator is currently running.
2674 */
2675 @Override
2676 public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
2677 if (running) {
2678 throw new LockedException();
2679 }
2680 this.accelerometerMyx = accelerometerMyx;
2681 }
2682
2683 /**
2684 * Gets known accelerometer y-z cross coupling error to be used to fix
2685 * measured specific force and find cross biases introduced by the
2686 * accelerometer.
2687 *
2688 * @return known accelerometer y-z cross coupling error.
2689 */
2690 @Override
2691 public double getAccelerometerMyz() {
2692 return accelerometerMyz;
2693 }
2694
2695 /**
2696 * Sets known accelerometer y-z cross coupling error to be used to fix
2697 * measured specific force and find cross biases introduced by the
2698 * accelerometer.
2699 *
2700 * @param accelerometerMyz known accelerometer y-z cross coupling
2701 * error.
2702 * @throws LockedException if calibrator is currently running.
2703 */
2704 @Override
2705 public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
2706 if (running) {
2707 throw new LockedException();
2708 }
2709 this.accelerometerMyz = accelerometerMyz;
2710 }
2711
2712 /**
2713 * Gets known accelerometer z-x cross coupling error to be used to fix
2714 * measured specific force and find cross biases introduced by the
2715 * accelerometer.
2716 *
2717 * @return known accelerometer z-x cross coupling error.
2718 */
2719 @Override
2720 public double getAccelerometerMzx() {
2721 return accelerometerMzx;
2722 }
2723
2724 /**
2725 * Sets known accelerometer z-x cross coupling error to be used to fix
2726 * measured specific force and find cross biases introduced by the
2727 * accelerometer.
2728 *
2729 * @param accelerometerMzx known accelerometer z-x cross coupling
2730 * error.
2731 * @throws LockedException if calibrator is currently running.
2732 */
2733 @Override
2734 public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
2735 if (running) {
2736 throw new LockedException();
2737 }
2738 this.accelerometerMzx = accelerometerMzx;
2739 }
2740
2741 /**
2742 * Gets known accelerometer z-y cross coupling error to be used to fix
2743 * measured specific force and find cross biases introduced by the
2744 * accelerometer.
2745 *
2746 * @return known accelerometer z-y cross coupling error.
2747 */
2748 @Override
2749 public double getAccelerometerMzy() {
2750 return accelerometerMzy;
2751 }
2752
2753 /**
2754 * Sets known accelerometer z-y cross coupling error to be used to fix
2755 * measured specific force and find cross biases introduced by the
2756 * accelerometer.
2757 *
2758 * @param accelerometerMzy known accelerometer z-y cross coupling
2759 * error.
2760 * @throws LockedException if calibrator is currently running.
2761 */
2762 @Override
2763 public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
2764 if (running) {
2765 throw new LockedException();
2766 }
2767 this.accelerometerMzy = accelerometerMzy;
2768 }
2769
2770 /**
2771 * Sets known accelerometer scaling factors to be used to fix measured
2772 * specific force and find cross biases introduced by the
2773 * accelerometer.
2774 *
2775 * @param accelerometerSx known accelerometer x scaling factor.
2776 * @param accelerometerSy known accelerometer y scaling factor.
2777 * @param accelerometerSz known accelerometer z scaling factor.
2778 * @throws LockedException if calibrator is currently running.
2779 */
2780 @Override
2781 public void setAccelerometerScalingFactors(
2782 final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
2783 throws LockedException {
2784 if (running) {
2785 throw new LockedException();
2786 }
2787 this.accelerometerSx = accelerometerSx;
2788 this.accelerometerSy = accelerometerSy;
2789 this.accelerometerSz = accelerometerSz;
2790 }
2791
2792 /**
2793 * Sets known accelerometer cross coupling errors to be used to fix
2794 * measured specific force and find cross biases introduced by the
2795 * accelerometer.
2796 *
2797 * @param accelerometerMxy known accelerometer x-y cross coupling
2798 * error.
2799 * @param accelerometerMxz known accelerometer x-z cross coupling
2800 * error.
2801 * @param accelerometerMyx known accelerometer y-x cross coupling
2802 * error.
2803 * @param accelerometerMyz known accelerometer y-z cross coupling
2804 * error.
2805 * @param accelerometerMzx known accelerometer z-x cross coupling
2806 * error.
2807 * @param accelerometerMzy known accelerometer z-y cross coupling
2808 * error.
2809 * @throws LockedException if calibrator is currently running.
2810 */
2811 @Override
2812 public void setAccelerometerCrossCouplingErrors(
2813 final double accelerometerMxy, final double accelerometerMxz,
2814 final double accelerometerMyx, final double accelerometerMyz,
2815 final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
2816 if (running) {
2817 throw new LockedException();
2818 }
2819 this.accelerometerMxy = accelerometerMxy;
2820 this.accelerometerMxz = accelerometerMxz;
2821 this.accelerometerMyx = accelerometerMyx;
2822 this.accelerometerMyz = accelerometerMyz;
2823 this.accelerometerMzx = accelerometerMzx;
2824 this.accelerometerMzy = accelerometerMzy;
2825 }
2826
2827 /**
2828 * Sets known accelerometer scaling factors and cross coupling errors
2829 * to be used to fix measured specific force and find cross biases
2830 * introduced by the accelerometer.
2831 *
2832 * @param accelerometerSx known accelerometer x scaling factor.
2833 * @param accelerometerSy known accelerometer y scaling factor.
2834 * @param accelerometerSz known accelerometer z scaling factor.
2835 * @param accelerometerMxy known accelerometer x-y cross coupling
2836 * error.
2837 * @param accelerometerMxz known accelerometer x-z cross coupling
2838 * error.
2839 * @param accelerometerMyx known accelerometer y-x cross coupling
2840 * error.
2841 * @param accelerometerMyz known accelerometer y-z cross coupling
2842 * error.
2843 * @param accelerometerMzx known accelerometer z-x cross coupling
2844 * error.
2845 * @param accelerometerMzy known accelerometer z-y cross coupling
2846 * error.
2847 * @throws LockedException if calibrator is currently running.
2848 */
2849 @Override
2850 public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
2851 final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
2852 final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
2853 final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
2854 throws LockedException {
2855 if (running) {
2856 throw new LockedException();
2857 }
2858 setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
2859 setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
2860 accelerometerMyz, accelerometerMzx, accelerometerMzy);
2861 }
2862
2863 /**
2864 * Gets known accelerometer scale factors and cross coupling
2865 * errors matrix.
2866 *
2867 * @return known accelerometer scale factors and cross coupling
2868 * errors matrix.
2869 */
2870 @Override
2871 public Matrix getAccelerometerMa() {
2872 Matrix result;
2873 try {
2874 result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
2875 getAccelerometerMa(result);
2876 } catch (final WrongSizeException ignore) {
2877 // never happens
2878 result = null;
2879 }
2880 return result;
2881 }
2882
2883 /**
2884 * Gets known accelerometer scale factors and cross coupling
2885 * errors matrix.
2886 *
2887 * @param result instance where data will be stored.
2888 * @throws IllegalArgumentException if provided matrix is not 3x3.
2889 */
2890 @Override
2891 public void getAccelerometerMa(final Matrix result) {
2892 if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
2893 throw new IllegalArgumentException();
2894 }
2895 result.setElementAtIndex(0, accelerometerSx);
2896 result.setElementAtIndex(1, accelerometerMyx);
2897 result.setElementAtIndex(2, accelerometerMzx);
2898
2899 result.setElementAtIndex(3, accelerometerMxy);
2900 result.setElementAtIndex(4, accelerometerSy);
2901 result.setElementAtIndex(5, accelerometerMzy);
2902
2903 result.setElementAtIndex(6, accelerometerMxz);
2904 result.setElementAtIndex(7, accelerometerMyz);
2905 result.setElementAtIndex(8, accelerometerSz);
2906 }
2907
2908 /**
2909 * Sets known accelerometer scale factors and cross coupling
2910 * errors matrix.
2911 *
2912 * @param accelerometerMa known accelerometer scale factors and
2913 * cross coupling errors matrix. Must be 3x3.
2914 * @throws LockedException if calibrator is currently running.
2915 * @throws IllegalArgumentException if provided matrix is not 3x3.
2916 */
2917 @Override
2918 public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
2919 if (running) {
2920 throw new LockedException();
2921 }
2922 if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
2923 || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
2924 throw new IllegalArgumentException();
2925 }
2926
2927 accelerometerSx = accelerometerMa.getElementAtIndex(0);
2928 accelerometerMyx = accelerometerMa.getElementAtIndex(1);
2929 accelerometerMzx = accelerometerMa.getElementAtIndex(2);
2930
2931 accelerometerMxy = accelerometerMa.getElementAtIndex(3);
2932 accelerometerSy = accelerometerMa.getElementAtIndex(4);
2933 accelerometerMzy = accelerometerMa.getElementAtIndex(5);
2934
2935 accelerometerMxz = accelerometerMa.getElementAtIndex(6);
2936 accelerometerMyz = accelerometerMa.getElementAtIndex(7);
2937 accelerometerSz = accelerometerMa.getElementAtIndex(8);
2938 }
2939
2940 /**
2941 * Gets initial x-coordinate of gyroscope bias to be used to find
2942 * a solution.
2943 * This is expressed in radians per second (rad/s).
2944 *
2945 * @return initial x-coordinate of gyroscope bias.
2946 */
2947 public double getInitialBiasX() {
2948 return initialBiasX;
2949 }
2950
2951 /**
2952 * Sets initial x-coordinate of gyroscope bias to be used to find
2953 * a solution.
2954 * This is expressed in radians per second (rad/s).
2955 *
2956 * @param initialBiasX initial x-coordinate of gyroscope bias.
2957 * @throws LockedException if calibrator is currently running.
2958 */
2959 public void setInitialBiasX(final double initialBiasX) throws LockedException {
2960 if (running) {
2961 throw new LockedException();
2962 }
2963 this.initialBiasX = initialBiasX;
2964 }
2965
2966 /**
2967 * Gets initial y-coordinate of gyroscope bias to be used to find
2968 * a solution.
2969 * This is expressed in radians per second (rad/s).
2970 *
2971 * @return initial y-coordinate of gyroscope bias.
2972 */
2973 public double getInitialBiasY() {
2974 return initialBiasY;
2975 }
2976
2977 /**
2978 * Sets initial y-coordinate of gyroscope bias to be used to find
2979 * a solution.
2980 * This is expressed in radians per second (rad/s).
2981 *
2982 * @param initialBiasY initial y-coordinate of gyroscope bias.
2983 * @throws LockedException if calibrator is currently running.
2984 */
2985 public void setInitialBiasY(final double initialBiasY) throws LockedException {
2986 if (running) {
2987 throw new LockedException();
2988 }
2989 this.initialBiasY = initialBiasY;
2990 }
2991
2992 /**
2993 * Gets initial z-coordinate of gyroscope bias ot be used to find
2994 * a solution.
2995 * This is expressed in radians per second (rad/s).
2996 *
2997 * @return initial z-coordinate of gyroscope bias.
2998 */
2999 public double getInitialBiasZ() {
3000 return initialBiasZ;
3001 }
3002
3003 /**
3004 * Sets initial z-coordinate of gyroscope bias to be used to find
3005 * a solution.
3006 * This is expressed in radians per second (rad/s).
3007 *
3008 * @param initialBiasZ initial z-coordinate of gyroscope bias.
3009 * @throws LockedException if calibrator is currently running.
3010 */
3011 public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
3012 if (running) {
3013 throw new LockedException();
3014 }
3015 this.initialBiasZ = initialBiasZ;
3016 }
3017
3018 /**
3019 * Gets initial x-coordinate of gyroscope bias to be used to find a
3020 * solution.
3021 *
3022 * @return initial x-coordinate of gyroscope bias.
3023 */
3024 public AngularSpeed getInitialBiasAngularSpeedX() {
3025 return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
3026 }
3027
3028 /**
3029 * Gets initial x-coordinate of gyroscope bias to be used to find a
3030 * solution.
3031 *
3032 * @param result instance where result data will be stored.
3033 */
3034 public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
3035 result.setValue(initialBiasX);
3036 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
3037 }
3038
3039 /**
3040 * Sets initial x-coordinate of gyroscope bias to be used to find a
3041 * solution.
3042 *
3043 * @param initialBiasX initial x-coordinate of gyroscope bias.
3044 * @throws LockedException if calibrator is currently running.
3045 */
3046 public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
3047 if (running) {
3048 throw new LockedException();
3049 }
3050 this.initialBiasX = convertAngularSpeed(initialBiasX);
3051 }
3052
3053 /**
3054 * Gets initial y-coordinate of gyroscope bias to be used to find a
3055 * solution.
3056 *
3057 * @return initial y-coordinate of gyroscope bias.
3058 */
3059 public AngularSpeed getInitialBiasAngularSpeedY() {
3060 return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
3061 }
3062
3063 /**
3064 * Gets initial y-coordinate of gyroscope bias to be used to find a
3065 * solution.
3066 *
3067 * @param result instance where result data will be stored.
3068 */
3069 public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
3070 result.setValue(initialBiasY);
3071 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
3072 }
3073
3074 /**
3075 * Sets initial y-coordinate of gyroscope bias to be used to find a
3076 * solution.
3077 *
3078 * @param initialBiasY initial y-coordinate of gyroscope bias.
3079 * @throws LockedException if calibrator is currently running.
3080 */
3081 public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
3082 if (running) {
3083 throw new LockedException();
3084 }
3085 this.initialBiasY = convertAngularSpeed(initialBiasY);
3086 }
3087
3088 /**
3089 * Gets initial z-coordinate of gyroscope bias to be used to find a
3090 * solution.
3091 *
3092 * @return initial z-coordinate of gyroscope bias.
3093 */
3094 public AngularSpeed getInitialBiasAngularSpeedZ() {
3095 return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
3096 }
3097
3098 /**
3099 * Gets initial z-coordinate of gyroscope bias to be used to find a
3100 * solution.
3101 *
3102 * @param result instance where result data will be stored.
3103 */
3104 public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
3105 result.setValue(initialBiasZ);
3106 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
3107 }
3108
3109 /**
3110 * Sets initial z-coordinate of gyroscope bias to be used to find a
3111 * solution.
3112 *
3113 * @param initialBiasZ initial z-coordinate of gyroscope bias.
3114 * @throws LockedException if calibrator is currently running.
3115 */
3116 public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
3117 if (running) {
3118 throw new LockedException();
3119 }
3120 this.initialBiasZ = convertAngularSpeed(initialBiasZ);
3121 }
3122
3123 /**
3124 * Sets initial bias coordinates of gyroscope used to find a solution
3125 * expressed in radians per second (rad/s).
3126 *
3127 * @param initialBiasX initial x-coordinate of gyroscope bias.
3128 * @param initialBiasY initial y-coordinate of gyroscope bias.
3129 * @param initialBiasZ initial z-coordinate of gyroscope bias.
3130 * @throws LockedException if calibrator is currently running.
3131 */
3132 public void setInitialBias(
3133 final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
3134 if (running) {
3135 throw new LockedException();
3136 }
3137 this.initialBiasX = initialBiasX;
3138 this.initialBiasY = initialBiasY;
3139 this.initialBiasZ = initialBiasZ;
3140 }
3141
3142 /**
3143 * Sets initial bias coordinates of gyroscope used to find a solution.
3144 *
3145 * @param initialBiasX initial x-coordinate of gyroscope bias.
3146 * @param initialBiasY initial y-coordinate of gyroscope bias.
3147 * @param initialBiasZ initial z-coordinate of gyroscope bias.
3148 * @throws LockedException if calibrator is currently running.
3149 */
3150 public void setInitialBias(
3151 final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
3152 throws LockedException {
3153 if (running) {
3154 throw new LockedException();
3155 }
3156 this.initialBiasX = convertAngularSpeed(initialBiasX);
3157 this.initialBiasY = convertAngularSpeed(initialBiasY);
3158 this.initialBiasZ = convertAngularSpeed(initialBiasZ);
3159 }
3160
3161 /**
3162 * Gets initial x scaling factor of gyroscope.
3163 *
3164 * @return initial x scaling factor of gyroscope.
3165 */
3166 @Override
3167 public double getInitialSx() {
3168 return initialSx;
3169 }
3170
3171 /**
3172 * Sets initial x scaling factor of gyroscope.
3173 *
3174 * @param initialSx initial x scaling factor of gyroscope.
3175 * @throws LockedException if calibrator is currently running.
3176 */
3177 @Override
3178 public void setInitialSx(final double initialSx) throws LockedException {
3179 if (running) {
3180 throw new LockedException();
3181 }
3182 this.initialSx = initialSx;
3183 }
3184
3185 /**
3186 * Gets initial y scaling factor of gyroscope.
3187 *
3188 * @return initial y scaling factor of gyroscope.
3189 */
3190 @Override
3191 public double getInitialSy() {
3192 return initialSy;
3193 }
3194
3195 /**
3196 * Sets initial y scaling factor of gyroscope.
3197 *
3198 * @param initialSy initial y scaling factor of gyroscope.
3199 * @throws LockedException if calibrator is currently running.
3200 */
3201 @Override
3202 public void setInitialSy(final double initialSy) throws LockedException {
3203 if (running) {
3204 throw new LockedException();
3205 }
3206 this.initialSy = initialSy;
3207 }
3208
3209 /**
3210 * Gets initial z scaling factor of gyroscope.
3211 *
3212 * @return initial z scaling factor of gyroscope.
3213 */
3214 @Override
3215 public double getInitialSz() {
3216 return initialSz;
3217 }
3218
3219 /**
3220 * Sets initial z scaling factor of gyroscope.
3221 *
3222 * @param initialSz initial z scaling factor of gyroscope.
3223 * @throws LockedException if calibrator is currently running.
3224 */
3225 @Override
3226 public void setInitialSz(final double initialSz) throws LockedException {
3227 if (running) {
3228 throw new LockedException();
3229 }
3230 this.initialSz = initialSz;
3231 }
3232
3233 /**
3234 * Gets initial x-y cross coupling error of gyroscope.
3235 *
3236 * @return initial x-y cross coupling error of gyroscope.
3237 */
3238 @Override
3239 public double getInitialMxy() {
3240 return initialMxy;
3241 }
3242
3243 /**
3244 * Sets initial x-y cross coupling error of gyroscope.
3245 *
3246 * @param initialMxy initial x-y cross coupling error of gyroscope.
3247 * @throws LockedException if calibrator is currently running.
3248 */
3249 @Override
3250 public void setInitialMxy(final double initialMxy) throws LockedException {
3251 if (running) {
3252 throw new LockedException();
3253 }
3254 this.initialMxy = initialMxy;
3255 }
3256
3257 /**
3258 * Gets initial x-z cross coupling error of gyroscope.
3259 *
3260 * @return initial x-z cross coupling error of gyroscope.
3261 */
3262 @Override
3263 public double getInitialMxz() {
3264 return initialMxz;
3265 }
3266
3267 /**
3268 * Sets initial x-z cross coupling error of gyroscope.
3269 *
3270 * @param initialMxz initial x-z cross coupling error of gyroscope.
3271 * @throws LockedException if calibrator is currently running.
3272 */
3273 @Override
3274 public void setInitialMxz(final double initialMxz) throws LockedException {
3275 if (running) {
3276 throw new LockedException();
3277 }
3278 this.initialMxz = initialMxz;
3279 }
3280
3281 /**
3282 * Gets initial y-x cross coupling error of gyroscope.
3283 *
3284 * @return initial y-x cross coupling error of gyroscope.
3285 */
3286 @Override
3287 public double getInitialMyx() {
3288 return initialMyx;
3289 }
3290
3291 /**
3292 * Sets initial y-x cross coupling error of gyroscope.
3293 *
3294 * @param initialMyx initial y-x cross coupling error of gyroscope.
3295 * @throws LockedException if calibrator is currently running.
3296 */
3297 @Override
3298 public void setInitialMyx(final double initialMyx) throws LockedException {
3299 if (running) {
3300 throw new LockedException();
3301 }
3302 this.initialMyx = initialMyx;
3303 }
3304
3305 /**
3306 * Gets initial y-z cross coupling error of gyroscope.
3307 *
3308 * @return initial y-z cross coupling error of gyroscope.
3309 */
3310 @Override
3311 public double getInitialMyz() {
3312 return initialMyz;
3313 }
3314
3315 /**
3316 * Sets initial y-z cross coupling error of gyroscope.
3317 *
3318 * @param initialMyz initial y-z cross coupling error of gyroscope.
3319 * @throws LockedException if calibrator is currently running.
3320 */
3321 @Override
3322 public void setInitialMyz(final double initialMyz) throws LockedException {
3323 if (running) {
3324 throw new LockedException();
3325 }
3326 this.initialMyz = initialMyz;
3327 }
3328
3329 /**
3330 * Gets initial z-x cross coupling error of gyroscope.
3331 *
3332 * @return initial z-x cross coupling error of gyroscope.
3333 */
3334 @Override
3335 public double getInitialMzx() {
3336 return initialMzx;
3337 }
3338
3339 /**
3340 * Sets initial z-x cross coupling error of gyroscope.
3341 *
3342 * @param initialMzx initial z-x cross coupling error of gyroscope.
3343 * @throws LockedException if calibrator is currently running.
3344 */
3345 @Override
3346 public void setInitialMzx(final double initialMzx) throws LockedException {
3347 if (running) {
3348 throw new LockedException();
3349 }
3350 this.initialMzx = initialMzx;
3351 }
3352
3353 /**
3354 * Gets initial z-y cross coupling error of gyroscope.
3355 *
3356 * @return initial z-y cross coupling error of gyroscope.
3357 */
3358 @Override
3359 public double getInitialMzy() {
3360 return initialMzy;
3361 }
3362
3363 /**
3364 * Sets initial z-y cross coupling error of gyroscope.
3365 *
3366 * @param initialMzy initial z-y cross coupling error of gyroscope.
3367 * @throws LockedException if calibrator is currently running.
3368 */
3369 @Override
3370 public void setInitialMzy(final double initialMzy) throws LockedException {
3371 if (running) {
3372 throw new LockedException();
3373 }
3374 this.initialMzy = initialMzy;
3375 }
3376
3377 /**
3378 * Sets initial scaling factors of gyroscope.
3379 *
3380 * @param initialSx initial x scaling factor of gyroscope.
3381 * @param initialSy initial y scaling factor of gyroscope.
3382 * @param initialSz initial z scaling factor of gyroscope.
3383 * @throws LockedException if calibrator is currently running.
3384 */
3385 @Override
3386 public void setInitialScalingFactors(
3387 final double initialSx, final double initialSy, final double initialSz) throws LockedException {
3388 if (running) {
3389 throw new LockedException();
3390 }
3391 this.initialSx = initialSx;
3392 this.initialSy = initialSy;
3393 this.initialSz = initialSz;
3394 }
3395
3396 /**
3397 * Sets initial cross coupling errors of gyroscope.
3398 *
3399 * @param initialMxy initial x-y cross coupling error of gyroscope.
3400 * @param initialMxz initial x-z cross coupling error of gyroscope.
3401 * @param initialMyx initial y-x cross coupling error of gyroscope.
3402 * @param initialMyz initial y-z cross coupling error of gyroscope.
3403 * @param initialMzx initial z-x cross coupling error of gyroscope.
3404 * @param initialMzy initial z-y cross coupling error of gyroscope.
3405 * @throws LockedException if calibrator is currently running.
3406 */
3407 @Override
3408 public void setInitialCrossCouplingErrors(
3409 final double initialMxy, final double initialMxz, final double initialMyx,
3410 final double initialMyz, final double initialMzx, final double initialMzy)
3411 throws LockedException {
3412 if (running) {
3413 throw new LockedException();
3414 }
3415 this.initialMxy = initialMxy;
3416 this.initialMxz = initialMxz;
3417 this.initialMyx = initialMyx;
3418 this.initialMyz = initialMyz;
3419 this.initialMzx = initialMzx;
3420 this.initialMzy = initialMzy;
3421 }
3422
3423 /**
3424 * Sets initial scaling factors and cross coupling errors of
3425 * gyroscope.
3426 *
3427 * @param initialSx initial x scaling factor of gyroscope.
3428 * @param initialSy initial y scaling factor of gyroscope.
3429 * @param initialSz initial z scaling factor of gyroscope.
3430 * @param initialMxy initial x-y cross coupling error of gyroscope.
3431 * @param initialMxz initial x-z cross coupling error of gyroscope.
3432 * @param initialMyx initial y-x cross coupling error of gyroscope.
3433 * @param initialMyz initial y-z cross coupling error of gyroscope.
3434 * @param initialMzx initial z-x cross coupling error of gyroscope.
3435 * @param initialMzy initial z-y cross coupling error of gyroscope.
3436 * @throws LockedException if calibrator is currently running.
3437 */
3438 @Override
3439 public void setInitialScalingFactorsAndCrossCouplingErrors(
3440 final double initialSx, final double initialSy, final double initialSz,
3441 final double initialMxy, final double initialMxz, final double initialMyx,
3442 final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
3443 if (running) {
3444 throw new LockedException();
3445 }
3446 setInitialScalingFactors(initialSx, initialSy, initialSz);
3447 setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
3448 }
3449
3450 /**
3451 * Gets initial gyroscope bias to be used to find a solution as
3452 * an array.
3453 * Array values are expressed in radians per second (rad/s).
3454 *
3455 * @return array containing coordinates of initial gyroscope bias.
3456 */
3457 public double[] getInitialBias() {
3458 final double[] result = new double[BodyKinematics.COMPONENTS];
3459 getInitialBias(result);
3460 return result;
3461 }
3462
3463 /**
3464 * Gets initial gyroscope bias to be used to find a solution as
3465 * an array.
3466 * Array values are expressed in radians per second (rad/s).
3467 *
3468 * @param result instance where result data will be copied to.
3469 * @throws IllegalArgumentException if provided array does not have length 3.
3470 */
3471 public void getInitialBias(final double[] result) {
3472 if (result.length != BodyKinematics.COMPONENTS) {
3473 throw new IllegalArgumentException();
3474 }
3475 result[0] = initialBiasX;
3476 result[1] = initialBiasY;
3477 result[2] = initialBiasZ;
3478 }
3479
3480 /**
3481 * Sets initial gyroscope bias to be used to find a solution as
3482 * an array.
3483 * Array values are expressed in radians per second (rad/s).
3484 *
3485 * @param initialBias initial bias to find a solution.
3486 * @throws LockedException if calibrator is currently running.
3487 * @throws IllegalArgumentException if provided array does not have length 3.
3488 */
3489 public void setInitialBias(final double[] initialBias) throws LockedException {
3490 if (running) {
3491 throw new LockedException();
3492 }
3493
3494 if (initialBias.length != BodyKinematics.COMPONENTS) {
3495 throw new IllegalArgumentException();
3496 }
3497 initialBiasX = initialBias[0];
3498 initialBiasY = initialBias[1];
3499 initialBiasZ = initialBias[2];
3500 }
3501
3502 /**
3503 * Gets initial gyroscope bias to be used to find a solution as a
3504 * column matrix.
3505 * Values are expressed in radians per second (rad/s).
3506 *
3507 * @return initial gyroscope bias to be used to find a solution as a
3508 * column matrix.
3509 */
3510 public Matrix getInitialBiasAsMatrix() {
3511 Matrix result;
3512 try {
3513 result = new Matrix(BodyKinematics.COMPONENTS, 1);
3514 getInitialBiasAsMatrix(result);
3515 } catch (final WrongSizeException ignore) {
3516 // never happens
3517 result = null;
3518 }
3519 return result;
3520 }
3521
3522 /**
3523 * Gets initial gyroscope bias to be used to find a solution as a
3524 * column matrix.
3525 * Values are expressed in radians per second (rad/s).
3526 *
3527 * @param result instance where result data will be copied to.
3528 * @throws IllegalArgumentException if provided matrix is not 3x1.
3529 */
3530 public void getInitialBiasAsMatrix(final Matrix result) {
3531 if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
3532 throw new IllegalArgumentException();
3533 }
3534 result.setElementAtIndex(0, initialBiasX);
3535 result.setElementAtIndex(1, initialBiasY);
3536 result.setElementAtIndex(2, initialBiasZ);
3537 }
3538
3539 /**
3540 * Sets initial gyroscope bias to be used to find a solution as
3541 * a column matrix with values expressed in radians per second (rad/s).
3542 *
3543 * @param initialBias initial gyroscope bias to find a solution.
3544 * @throws LockedException if calibrator is currently running.
3545 * @throws IllegalArgumentException if provided matrix is not 3x1.
3546 */
3547 public void setInitialBias(final Matrix initialBias) throws LockedException {
3548 if (running) {
3549 throw new LockedException();
3550 }
3551 if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
3552 throw new IllegalArgumentException();
3553 }
3554
3555 initialBiasX = initialBias.getElementAtIndex(0);
3556 initialBiasY = initialBias.getElementAtIndex(1);
3557 initialBiasZ = initialBias.getElementAtIndex(2);
3558 }
3559
3560 /**
3561 * Gets initial bias coordinates of gyroscope used to find a solution.
3562 *
3563 * @return initial bias coordinates.
3564 */
3565 public AngularSpeedTriad getInitialBiasAsTriad() {
3566 return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
3567 }
3568
3569 /**
3570 * Gets initial bias coordinates of gyroscope used to find a solution.
3571 *
3572 * @param result instance where result will be stored.
3573 */
3574 public void getInitialBiasAsTriad(final AngularSpeedTriad result) {
3575 result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
3576 AngularSpeedUnit.RADIANS_PER_SECOND);
3577 }
3578
3579 /**
3580 * Sets initial bias coordinates of gyroscope used to find a solution.
3581 *
3582 * @param initialBias initial bias coordinates to be set.
3583 * @throws LockedException if calibrator is currently running.
3584 */
3585 public void setInitialBias(final AngularSpeedTriad initialBias) throws LockedException {
3586 if (running) {
3587 throw new LockedException();
3588 }
3589
3590 initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
3591 initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
3592 initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
3593 }
3594
3595 /**
3596 * Gets initial gyroscope scale factors and cross coupling errors
3597 * matrix.
3598 *
3599 * @return initial gyroscope scale factors and cross coupling errors
3600 * matrix.
3601 */
3602 @Override
3603 public Matrix getInitialMg() {
3604 Matrix result;
3605 try {
3606 result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
3607 getInitialMg(result);
3608 } catch (final WrongSizeException ignore) {
3609 // never happens
3610 result = null;
3611 }
3612 return result;
3613 }
3614
3615 /**
3616 * Gets initial gyroscope scale factors and cross coupling errors
3617 * matrix.
3618 *
3619 * @param result instance where data will be stored.
3620 * @throws IllegalArgumentException if provided matrix is not 3x3.
3621 */
3622 @Override
3623 public void getInitialMg(final Matrix result) {
3624 if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
3625 throw new IllegalArgumentException();
3626 }
3627 result.setElementAtIndex(0, initialSx);
3628 result.setElementAtIndex(1, initialMyx);
3629 result.setElementAtIndex(2, initialMzx);
3630
3631 result.setElementAtIndex(3, initialMxy);
3632 result.setElementAtIndex(4, initialSy);
3633 result.setElementAtIndex(5, initialMzy);
3634
3635 result.setElementAtIndex(6, initialMxz);
3636 result.setElementAtIndex(7, initialMyz);
3637 result.setElementAtIndex(8, initialSz);
3638 }
3639
3640 /**
3641 * Sets initial gyroscope scale factors and cross coupling errors matrix.
3642 *
3643 * @param initialMg initial scale factors and cross coupling errors matrix.
3644 * @throws IllegalArgumentException if provided matrix is not 3x3.
3645 * @throws LockedException if calibrator is currently running.
3646 */
3647 @Override
3648 public void setInitialMg(final Matrix initialMg) throws LockedException {
3649 if (running) {
3650 throw new LockedException();
3651 }
3652 if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
3653 throw new IllegalArgumentException();
3654 }
3655
3656 initialSx = initialMg.getElementAtIndex(0);
3657 initialMyx = initialMg.getElementAtIndex(1);
3658 initialMzx = initialMg.getElementAtIndex(2);
3659
3660 initialMxy = initialMg.getElementAtIndex(3);
3661 initialSy = initialMg.getElementAtIndex(4);
3662 initialMzy = initialMg.getElementAtIndex(5);
3663
3664 initialMxz = initialMg.getElementAtIndex(6);
3665 initialMyz = initialMg.getElementAtIndex(7);
3666 initialSz = initialMg.getElementAtIndex(8);
3667 }
3668
3669 /**
3670 * Gets initial G-dependent cross biases introduced on the gyroscope by the
3671 * specific forces sensed by the accelerometer.
3672 *
3673 * @return a 3x3 matrix containing initial g-dependent cross biases.
3674 */
3675 @Override
3676 public Matrix getInitialGg() {
3677 return new Matrix(initialGg);
3678 }
3679
3680 /**
3681 * Gets initial G-dependent cross biases introduced on the gyroscope by the
3682 * specific forces sensed by the accelerometer.
3683 *
3684 * @param result instance where data will be stored.
3685 * @throws IllegalArgumentException if provided matrix is not 3x3.
3686 */
3687 @Override
3688 public void getInitialGg(final Matrix result) {
3689
3690 if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
3691 throw new IllegalArgumentException();
3692 }
3693
3694 result.copyFrom(initialGg);
3695 }
3696
3697 /**
3698 * Sets initial G-dependent cross biases introduced on the gyroscope by the
3699 * specific forces sensed by the accelerometer.
3700 *
3701 * @param initialGg g-dependent cross biases.
3702 * @throws LockedException if calibrator is currently running.
3703 * @throws IllegalArgumentException if provided matrix is not 3x3.
3704 */
3705 @Override
3706 public void setInitialGg(final Matrix initialGg) throws LockedException {
3707 if (running) {
3708 throw new LockedException();
3709 }
3710
3711 if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
3712 throw new IllegalArgumentException();
3713 }
3714
3715 initialGg.copyTo(this.initialGg);
3716 }
3717
3718 /**
3719 * Gets constant rotation rate at which the turntable is spinning.
3720 * This is expressed in radians per second (rad/s).
3721 *
3722 * @return constant rotation rate of turntable.
3723 */
3724 public double getTurntableRotationRate() {
3725 return turntableRotationRate;
3726 }
3727
3728 /**
3729 * Sets constant rotation rate at which the turntable is spinning.
3730 * This is expressed in radians per second (rad/s).
3731 *
3732 * @param turntableRotationRate constant rotation rate of turntable.
3733 * @throws LockedException if calibrator is currently running
3734 * @throws IllegalArgumentException if provided value is zero or
3735 * negative.
3736 */
3737 public void setTurntableRotationRate(final double turntableRotationRate) throws LockedException {
3738 if (running) {
3739 throw new LockedException();
3740 }
3741 if (turntableRotationRate <= 0.0) {
3742 throw new IllegalArgumentException();
3743 }
3744
3745 this.turntableRotationRate = turntableRotationRate;
3746 }
3747
3748 /**
3749 * Gets constant rotation rate at which the turntable is spinning.
3750 *
3751 * @return constant rotation rate of turntable.
3752 */
3753 public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
3754 return new AngularSpeed(turntableRotationRate, AngularSpeedUnit.RADIANS_PER_SECOND);
3755 }
3756
3757 /**
3758 * Gets constant rotation rate at which the turntable is spinning.
3759 *
3760 * @param result instance where result will be stored.
3761 */
3762 public void getTurntableRotationRateAsAngularSpeed(final AngularSpeed result) {
3763 result.setValue(turntableRotationRate);
3764 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
3765 }
3766
3767 /**
3768 * Sets constant rotation rate at which the turntable is spinning.
3769 *
3770 * @param turntableRotationRate constant rotation rate of turntable.
3771 * @throws LockedException if calibrator is currently running.
3772 * @throws IllegalArgumentException if provided value is zero or
3773 * negative.
3774 */
3775 public void setTurntableRotationRate(final AngularSpeed turntableRotationRate) throws LockedException {
3776 if (running) {
3777 throw new LockedException();
3778 }
3779 setTurntableRotationRate(convertAngularSpeed(turntableRotationRate));
3780 }
3781
3782 /**
3783 * Gets time interval between measurements being captured expressed in
3784 * seconds (s).
3785 *
3786 * @return time interval between measurements.
3787 */
3788 public double getTimeInterval() {
3789 return timeInterval;
3790 }
3791
3792 /**
3793 * Sets time interval between measurements being captured expressed in
3794 * seconds (s).
3795 *
3796 * @param timeInterval time interval between measurements.
3797 * @throws LockedException if calibrator is currently running.
3798 * @throws IllegalArgumentException if provided value is zero or
3799 * negative.
3800 */
3801 public void setTimeInterval(final double timeInterval) throws LockedException {
3802 if (running) {
3803 throw new LockedException();
3804 }
3805
3806 if (timeInterval <= 0.0) {
3807 throw new IllegalArgumentException();
3808 }
3809 this.timeInterval = timeInterval;
3810 }
3811
3812 /**
3813 * Gets time interval between measurements being captured.
3814 *
3815 * @return time interval between measurements.
3816 */
3817 public Time getTimeIntervalAsTime() {
3818 return new Time(timeInterval, TimeUnit.SECOND);
3819 }
3820
3821 /**
3822 * Gets time interval between measurements being captured.
3823 *
3824 * @param result instance where result will be stored.
3825 */
3826 public void getTimeIntervalAsTime(final Time result) {
3827 result.setValue(timeInterval);
3828 result.setUnit(TimeUnit.SECOND);
3829 }
3830
3831 /**
3832 * Sets time interval between measurements being captured.
3833 *
3834 * @param timeInterval time interval between measurements.
3835 * @throws LockedException if calibrator is currently running.
3836 */
3837 public void setTimeInterval(final Time timeInterval) throws LockedException {
3838 if (running) {
3839 throw new LockedException();
3840 }
3841 setTimeInterval(convertTime(timeInterval));
3842 }
3843
3844 /**
3845 * Gets a collection of body kinematics measurements taken at
3846 * a given position with different unknown orientations and containing
3847 * the standard deviations of accelerometer and gyroscope measurements.
3848 *
3849 * @return collection of body kinematics measurements at a known position
3850 * with unknown orientations.
3851 */
3852 @Override
3853 public List<StandardDeviationBodyKinematics> getMeasurements() {
3854 return measurements;
3855 }
3856
3857 /**
3858 * Sets a collection of body kinematics measurements taken at
3859 * a given position with different unknown orientations and containing
3860 * the standard deviations of accelerometer and gyroscope measurements.
3861 *
3862 * @param measurements collection of body kinematics measurements at a
3863 * known position with unknown orientations.
3864 * @throws LockedException if calibrator is currently running.
3865 */
3866 @Override
3867 public void setMeasurements(final List<StandardDeviationBodyKinematics> measurements) throws LockedException {
3868 if (running) {
3869 throw new LockedException();
3870 }
3871 this.measurements = measurements;
3872 }
3873
3874 /**
3875 * Gets position where body kinematics measures have been taken expressed in
3876 * ECEF coordinates.
3877 *
3878 * @return position where body kinematics measures have been taken.
3879 */
3880 public ECEFPosition getEcefPosition() {
3881 return position;
3882 }
3883
3884 /**
3885 * Gets position where body kinematics measures have been taken expressed in
3886 * ECEF coordinates.
3887 *
3888 * @param position position where body kinematics measures have been taken.
3889 * @throws LockedException if calibrator is currently running.
3890 */
3891 public void setPosition(final ECEFPosition position) throws LockedException {
3892 if (running) {
3893 throw new LockedException();
3894 }
3895
3896 this.position = position;
3897 }
3898
3899 /**
3900 * Gets position where body kinematics measures have been taken expressed in
3901 * NED coordinates.
3902 *
3903 * @return position where body kinematics measures have been taken or null if
3904 * not available.
3905 */
3906 public NEDPosition getNedPosition() {
3907 final var result = new NEDPosition();
3908 return getNedPosition(result) ? result : null;
3909 }
3910
3911 /**
3912 * Gets position where body kinematics measures have been taken expressed in
3913 * NED coordinates.
3914 *
3915 * @param result instance where result will be stored.
3916 * @return true if NED position could be computed, false otherwise.
3917 */
3918 public boolean getNedPosition(final NEDPosition result) {
3919 if (position != null) {
3920 final var velocity = new NEDVelocity();
3921 ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
3922 position.getX(), position.getY(), position.getZ(),
3923 0.0, 0.0, 0.0, result, velocity);
3924 return true;
3925 } else {
3926 return false;
3927 }
3928 }
3929
3930 /**
3931 * Sets position where body kinematics measures have been taken expressed in
3932 * NED coordinates.
3933 *
3934 * @param position position where body kinematics measures have been taken.
3935 * @throws LockedException if calibrator is currently running.
3936 */
3937 public void setPosition(final NEDPosition position) throws LockedException {
3938 if (running) {
3939 throw new LockedException();
3940 }
3941
3942 this.position = convertPosition(position);
3943 }
3944
3945 /**
3946 * Indicates the type of measurement or sequence used by this calibrator.
3947 *
3948 * @return type of measurement or sequence used by this calibrator.
3949 */
3950 @Override
3951 public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
3952 return GyroscopeCalibratorMeasurementOrSequenceType.STANDARD_DEVIATION_BODY_KINEMATICS_MEASUREMENT;
3953 }
3954
3955 /**
3956 * Indicates whether this calibrator requires ordered measurements or sequences
3957 * in a list or not.
3958 *
3959 * @return true if measurements or sequences must be ordered, false otherwise.
3960 */
3961 @Override
3962 public boolean isOrderedMeasurementsOrSequencesRequired() {
3963 return true;
3964 }
3965
3966 /**
3967 * Indicates whether z-axis is assumed to be common for accelerometer and
3968 * gyroscope.
3969 * When enabled, this eliminates 3 variables from Ma matrix.
3970 *
3971 * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
3972 * false otherwise.
3973 */
3974 @Override
3975 public boolean isCommonAxisUsed() {
3976 return commonAxisUsed;
3977 }
3978
3979 /**
3980 * Specifies whether z-axis is assumed to be common for accelerometer and
3981 * gyroscope.
3982 * When enabled, this eliminates 3 variables from Ma matrix.
3983 *
3984 * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
3985 * and gyroscope, false otherwise.
3986 * @throws LockedException if calibrator is currently running.
3987 */
3988 @Override
3989 public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
3990 if (running) {
3991 throw new LockedException();
3992 }
3993
3994 this.commonAxisUsed = commonAxisUsed;
3995 }
3996
3997 /**
3998 * Indicates whether G-dependent cross biases are being estimated
3999 * or not.
4000 * When enabled, this adds 9 variables from Gg matrix.
4001 *
4002 * @return true if G-dependent cross biases will be estimated,
4003 * false otherwise.
4004 */
4005 public boolean isGDependentCrossBiasesEstimated() {
4006 return estimateGDependentCrossBiases;
4007 }
4008
4009 /**
4010 * Specifies whether G-dependent cross biases are being estimated
4011 * or not.
4012 * When enabled, this adds 9 variables from Gg matrix.
4013 *
4014 * @param estimateGDependentCrossBiases true if G-dependent cross
4015 * biases will be estimated,
4016 * false otherwise.
4017 * @throws LockedException if calibrator is currently running.
4018 */
4019 public void setGDependentCrossBiasesEstimated(final boolean estimateGDependentCrossBiases) throws LockedException {
4020 if (running) {
4021 throw new LockedException();
4022 }
4023
4024 this.estimateGDependentCrossBiases = estimateGDependentCrossBiases;
4025 }
4026
4027 /**
4028 * Gets listener to handle events raised by this estimator.
4029 *
4030 * @return listener to handle events raised by this estimator.
4031 */
4032 public RobustTurntableGyroscopeCalibratorListener getListener() {
4033 return listener;
4034 }
4035
4036 /**
4037 * Sets listener to handle events raised by this estimator.
4038 *
4039 * @param listener listener to handle events raised by this estimator.
4040 * @throws LockedException if calibrator is currently running.
4041 */
4042 public void setListener(final RobustTurntableGyroscopeCalibratorListener listener) throws LockedException {
4043 if (running) {
4044 throw new LockedException();
4045 }
4046
4047 this.listener = listener;
4048 }
4049
4050 /**
4051 * Gets minimum number of required measurements.
4052 *
4053 * @return minimum number of required measurements.
4054 */
4055 @Override
4056 public int getMinimumRequiredMeasurementsOrSequences() {
4057 if (commonAxisUsed) {
4058 if (estimateGDependentCrossBiases) {
4059 return TurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS_AND_CROSS_BIASES;
4060 } else {
4061 return TurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS;
4062 }
4063 } else {
4064 if (estimateGDependentCrossBiases) {
4065 return TurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_GENERAL_AND_CROSS_BIASES;
4066 } else {
4067 return TurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_GENERAL;
4068 }
4069 }
4070 }
4071
4072 /**
4073 * Indicates whether calibrator is ready to start.
4074 *
4075 * @return true if calibrator is ready, false otherwise.
4076 */
4077 @Override
4078 public boolean isReady() {
4079 return measurements != null && measurements.size() >= getMinimumRequiredMeasurementsOrSequences();
4080 }
4081
4082 /**
4083 * Indicates whether calibrator is currently running or not.
4084 *
4085 * @return true if calibrator is running, false otherwise.
4086 */
4087 @Override
4088 public boolean isRunning() {
4089 return running;
4090 }
4091
4092 /**
4093 * Returns amount of progress variation before notifying a progress change during
4094 * calibration.
4095 *
4096 * @return amount of progress variation before notifying a progress change during
4097 * calibration.
4098 */
4099 public float getProgressDelta() {
4100 return progressDelta;
4101 }
4102
4103 /**
4104 * Sets amount of progress variation before notifying a progress change during
4105 * calibration.
4106 *
4107 * @param progressDelta amount of progress variation before notifying a progress
4108 * change during calibration.
4109 * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
4110 * @throws LockedException if calibrator is currently running.
4111 */
4112 public void setProgressDelta(final float progressDelta) throws LockedException {
4113 if (running) {
4114 throw new LockedException();
4115 }
4116 if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
4117 throw new IllegalArgumentException();
4118 }
4119 this.progressDelta = progressDelta;
4120 }
4121
4122 /**
4123 * Returns amount of confidence expressed as a value between 0.0 and 1.0
4124 * (which is equivalent to 100%). The amount of confidence indicates the probability
4125 * that the estimated result is correct. Usually this value will be close to 1.0, but
4126 * not exactly 1.0.
4127 *
4128 * @return amount of confidence as a value between 0.0 and 1.0.
4129 */
4130 public double getConfidence() {
4131 return confidence;
4132 }
4133
4134 /**
4135 * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
4136 * equivalent to 100%). The amount of confidence indicates the probability that
4137 * the estimated result is correct. Usually this value will be close to 1.0, but
4138 * not exactly 1.0.
4139 *
4140 * @param confidence confidence to be set as a value between 0.0 and 1.0.
4141 * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
4142 * @throws LockedException if calibrator is currently running.
4143 */
4144 public void setConfidence(final double confidence) throws LockedException {
4145 if (running) {
4146 throw new LockedException();
4147 }
4148 if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
4149 throw new IllegalArgumentException();
4150 }
4151 this.confidence = confidence;
4152 }
4153
4154 /**
4155 * Returns maximum allowed number of iterations. If maximum allowed number of
4156 * iterations is achieved without converging to a result when calling calibrate(),
4157 * a RobustEstimatorException will be raised.
4158 *
4159 * @return maximum allowed number of iterations.
4160 */
4161 public int getMaxIterations() {
4162 return maxIterations;
4163 }
4164
4165 /**
4166 * Sets maximum allowed number of iterations. When the maximum number of iterations
4167 * is exceeded, result will not be available, however an approximate result will be
4168 * available for retrieval.
4169 *
4170 * @param maxIterations maximum allowed number of iterations to be set.
4171 * @throws IllegalArgumentException if provided value is less than 1.
4172 * @throws LockedException if calibrator is currently running.
4173 */
4174 public void setMaxIterations(final int maxIterations) throws LockedException {
4175 if (running) {
4176 throw new LockedException();
4177 }
4178 if (maxIterations < MIN_ITERATIONS) {
4179 throw new IllegalArgumentException();
4180 }
4181 this.maxIterations = maxIterations;
4182 }
4183
4184 /**
4185 * Gets data related to inliers found after estimation.
4186 *
4187 * @return data related to inliers found after estimation.
4188 */
4189 public InliersData getInliersData() {
4190 return inliersData;
4191 }
4192
4193 /**
4194 * Indicates whether result must be refined using a non-linear solver over found inliers.
4195 *
4196 * @return true to refine result, false to simply use result found by robust estimator
4197 * without further refining.
4198 */
4199 public boolean isResultRefined() {
4200 return refineResult;
4201 }
4202
4203 /**
4204 * Specifies whether result must be refined using a non-linear solver over found inliers.
4205 *
4206 * @param refineResult true to refine result, false to simply use result found by robust
4207 * estimator without further refining.
4208 * @throws LockedException if calibrator is currently running.
4209 */
4210 public void setResultRefined(final boolean refineResult) throws LockedException {
4211 if (running) {
4212 throw new LockedException();
4213 }
4214 this.refineResult = refineResult;
4215 }
4216
4217 /**
4218 * Indicates whether covariance must be kept after refining result.
4219 * This setting is only taken into account if result is refined.
4220 *
4221 * @return true if covariance must be kept after refining result, false otherwise.
4222 */
4223 public boolean isCovarianceKept() {
4224 return keepCovariance;
4225 }
4226
4227 /**
4228 * Specifies whether covariance must be kept after refining result.
4229 * This setting is only taken into account if result is refined.
4230 *
4231 * @param keepCovariance true if covariance must be kept after refining result,
4232 * false otherwise.
4233 * @throws LockedException if calibrator is currently running.
4234 */
4235 public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
4236 if (running) {
4237 throw new LockedException();
4238 }
4239 this.keepCovariance = keepCovariance;
4240 }
4241
4242 /**
4243 * Returns quality scores corresponding to each measurement.
4244 * The larger the score value the better the quality of the sample.
4245 * This implementation always returns null.
4246 * Subclasses using quality scores must implement proper behavior.
4247 *
4248 * @return quality scores corresponding to each sample.
4249 */
4250 @Override
4251 public double[] getQualityScores() {
4252 return null;
4253 }
4254
4255 /**
4256 * Sets quality scores corresponding to each measurement.
4257 * The larger the score value the better the quality of the sample.
4258 * This implementation makes no action.
4259 * Subclasses using quality scores must implement proper behaviour.
4260 *
4261 * @param qualityScores quality scores corresponding to each sample.
4262 * @throws IllegalArgumentException if provided quality scores length
4263 * is smaller than minimum required samples.
4264 * @throws LockedException if calibrator is currently running.
4265 */
4266 @Override
4267 public void setQualityScores(final double[] qualityScores) throws LockedException {
4268 }
4269
4270 /**
4271 * Gets array containing x,y,z components of estimated gyroscope biases
4272 * expressed in radians per second (rad/s).
4273 *
4274 * @return array containing x,y,z components of estimated gyroscope biases.
4275 */
4276 @Override
4277 public double[] getEstimatedBiases() {
4278 return estimatedBiases;
4279 }
4280
4281 /**
4282 * Gets array containing x,y,z components of estimated gyroscope biases
4283 * expressed in radians per second (rad/s).
4284 *
4285 * @param result instance where estimated gyroscope biases will be stored.
4286 * @return true if result instance was updated, false otherwise (when estimation
4287 * is not yet available).
4288 */
4289 @Override
4290 public boolean getEstimatedBiases(final double[] result) {
4291 if (estimatedBiases != null) {
4292 System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
4293 return true;
4294 } else {
4295 return false;
4296 }
4297 }
4298
4299 /**
4300 * Gets column matrix containing x,y,z components of estimated gyroscope biases
4301 * expressed in radians per second (rad/s).
4302 *
4303 * @return column matrix containing x,y,z components of estimated gyroscope
4304 * biases.
4305 */
4306 @Override
4307 public Matrix getEstimatedBiasesAsMatrix() {
4308 return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
4309 }
4310
4311 /**
4312 * Gets column matrix containing x,y,z components of estimated gyroscope biases
4313 * expressed in radians per second (rad/s).
4314 *
4315 * @param result instance where result data will be stored.
4316 * @return true if result was updated, false otherwise.
4317 * @throws WrongSizeException if provided result instance has invalid size.
4318 */
4319 @Override
4320 public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
4321 if (estimatedBiases != null) {
4322 result.fromArray(estimatedBiases);
4323 return true;
4324 } else {
4325 return false;
4326 }
4327 }
4328
4329 /**
4330 * Gets x coordinate of estimated gyroscope bias expressed in radians per
4331 * second (rad/s).
4332 *
4333 * @return x coordinate of estimated gyroscope bias or null if not available.
4334 */
4335 @Override
4336 public Double getEstimatedBiasX() {
4337 return estimatedBiases != null ? estimatedBiases[0] : null;
4338 }
4339
4340 /**
4341 * Gets y coordinate of estimated gyroscope bias expressed in radians per
4342 * second (rad/s).
4343 *
4344 * @return y coordinate of estimated gyroscope bias or null if not available.
4345 */
4346 @Override
4347 public Double getEstimatedBiasY() {
4348 return estimatedBiases != null ? estimatedBiases[1] : null;
4349 }
4350
4351 /**
4352 * Gets z coordinate of estimated gyroscope bias expressed in radians per
4353 * second (rad/s).
4354 *
4355 * @return z coordinate of estimated gyroscope bias or null if not available.
4356 */
4357 @Override
4358 public Double getEstimatedBiasZ() {
4359 return estimatedBiases != null ? estimatedBiases[2] : null;
4360 }
4361
4362 /**
4363 * Gets x coordinate of estimated gyroscope bias.
4364 *
4365 * @return x coordinate of estimated gyroscope bias or null if not available.
4366 */
4367 @Override
4368 public AngularSpeed getEstimatedBiasAngularSpeedX() {
4369 return estimatedBiases != null
4370 ? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
4371 }
4372
4373 /**
4374 * Gets x coordinate of estimated gyroscope bias.
4375 *
4376 * @param result instance where result will be stored.
4377 * @return true if result was updated, false if estimation is not available.
4378 */
4379 @Override
4380 public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
4381 if (estimatedBiases != null) {
4382 result.setValue(estimatedBiases[0]);
4383 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
4384 return true;
4385 } else {
4386 return false;
4387 }
4388 }
4389
4390 /**
4391 * Gets y coordinate of estimated gyroscope bias.
4392 *
4393 * @return y coordinate of estimated gyroscope bias or null if not available.
4394 */
4395 @Override
4396 public AngularSpeed getEstimatedBiasAngularSpeedY() {
4397 return estimatedBiases != null
4398 ? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
4399 }
4400
4401 /**
4402 * Gets y coordinate of estimated gyroscope bias.
4403 *
4404 * @param result instance where result will be stored.
4405 * @return true if result was updated, false if estimation is not available.
4406 */
4407 @Override
4408 public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
4409 if (estimatedBiases != null) {
4410 result.setValue(estimatedBiases[1]);
4411 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
4412 return true;
4413 } else {
4414 return false;
4415 }
4416 }
4417
4418 /**
4419 * Gets z coordinate of estimated gyroscope bias.
4420 *
4421 * @return z coordinate of estimated gyroscope bias or null if not available.
4422 */
4423 @Override
4424 public AngularSpeed getEstimatedBiasAngularSpeedZ() {
4425 return estimatedBiases != null
4426 ? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
4427 }
4428
4429 /**
4430 * Gets z coordinate of estimated gyroscope bias.
4431 *
4432 * @param result instance where result will be stored.
4433 * @return true if result was updated, false if estimation is not available.
4434 */
4435 @Override
4436 public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
4437 if (estimatedBiases != null) {
4438 result.setValue(estimatedBiases[2]);
4439 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
4440 return true;
4441 } else {
4442 return false;
4443 }
4444 }
4445
4446 /**
4447 * Gets estimated gyroscope bias.
4448 *
4449 * @return estimated gyroscope bias or null if not available.
4450 */
4451 @Override
4452 public AngularSpeedTriad getEstimatedBiasAsTriad() {
4453 return estimatedBiases != null
4454 ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
4455 estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
4456 : null;
4457 }
4458
4459 /**
4460 * Gets estimated gyroscope bias.
4461 *
4462 * @param result instance where result will be stored.
4463 * @return true if estimated gyroscope bias is available and result was
4464 * modified, false otherwise.
4465 */
4466 @Override
4467 public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
4468 if (estimatedBiases != null) {
4469 result.setValueCoordinatesAndUnit(
4470 estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
4471 return true;
4472 } else {
4473 return false;
4474 }
4475 }
4476
4477 /**
4478 * Gets estimated gyroscope scale factors and cross coupling errors.
4479 * This is the product of matrix Tg containing cross coupling errors and Kg
4480 * containing scaling factors.
4481 * So that:
4482 * <pre>
4483 * Mg = [sx mxy mxz] = Tg*Kg
4484 * [myx sy myz]
4485 * [mzx mzy sz ]
4486 * </pre>
4487 * Where:
4488 * <pre>
4489 * Kg = [sx 0 0 ]
4490 * [0 sy 0 ]
4491 * [0 0 sz]
4492 * </pre>
4493 * and
4494 * <pre>
4495 * Tg = [1 -alphaXy alphaXz ]
4496 * [alphaYx 1 -alphaYz]
4497 * [-alphaZx alphaZy 1 ]
4498 * </pre>
4499 * Hence:
4500 * <pre>
4501 * Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
4502 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
4503 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
4504 * </pre>
4505 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
4506 * are considered to be zero if the gyroscope z-axis is assumed to be the same
4507 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
4508 * becomes upper diagonal:
4509 * <pre>
4510 * Mg = [sx mxy mxz]
4511 * [0 sy myz]
4512 * [0 0 sz ]
4513 * </pre>
4514 * Values of this matrix are unit-less.
4515 *
4516 * @return estimated gyroscope scale factors and cross coupling errors, or null
4517 * if not available.
4518 */
4519 @Override
4520 public Matrix getEstimatedMg() {
4521 return estimatedMg;
4522 }
4523
4524 /**
4525 * Gets estimated gyroscope x-axis scale factor.
4526 *
4527 * @return estimated gyroscope x-axis scale factor or null
4528 * if not available.
4529 */
4530 @Override
4531 public Double getEstimatedSx() {
4532 return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
4533 }
4534
4535 /**
4536 * Gets estimated gyroscope y-axis scale factor.
4537 *
4538 * @return estimated gyroscope y-axis scale factor or null
4539 * if not available.
4540 */
4541 @Override
4542 public Double getEstimatedSy() {
4543 return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
4544 }
4545
4546 /**
4547 * Gets estimated gyroscope z-axis scale factor.
4548 *
4549 * @return estimated gyroscope z-axis scale factor or null
4550 * if not available.
4551 */
4552 @Override
4553 public Double getEstimatedSz() {
4554 return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
4555 }
4556
4557 /**
4558 * Gets estimated gyroscope x-y cross-coupling error.
4559 *
4560 * @return estimated gyroscope x-y cross-coupling error or null
4561 * if not available.
4562 */
4563 @Override
4564 public Double getEstimatedMxy() {
4565 return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
4566 }
4567
4568 /**
4569 * Gets estimated gyroscope x-z cross-coupling error.
4570 *
4571 * @return estimated gyroscope x-z cross-coupling error or null
4572 * if not available.
4573 */
4574 @Override
4575 public Double getEstimatedMxz() {
4576 return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
4577 }
4578
4579 /**
4580 * Gets estimated gyroscope y-x cross-coupling error.
4581 *
4582 * @return estimated gyroscope y-x cross-coupling error or null
4583 * if not available.
4584 */
4585 @Override
4586 public Double getEstimatedMyx() {
4587 return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
4588 }
4589
4590 /**
4591 * Gets estimated gyroscope y-z cross-coupling error.
4592 *
4593 * @return estimated gyroscope y-z cross-coupling error or null
4594 * if not available.
4595 */
4596 @Override
4597 public Double getEstimatedMyz() {
4598 return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
4599 }
4600
4601 /**
4602 * Gets estimated gyroscope z-x cross-coupling error.
4603 *
4604 * @return estimated gyroscope z-x cross-coupling error or null
4605 * if not available.
4606 */
4607 @Override
4608 public Double getEstimatedMzx() {
4609 return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
4610 }
4611
4612 /**
4613 * Gets estimated gyroscope z-y cross-coupling error.
4614 *
4615 * @return estimated gyroscope z-y cross-coupling error or null
4616 * if not available.
4617 */
4618 @Override
4619 public Double getEstimatedMzy() {
4620 return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
4621 }
4622
4623 /**
4624 * Gets estimated G-dependent cross biases introduced on the gyroscope by the
4625 * specific forces sensed by the accelerometer.
4626 * This instance allows any 3x3 matrix.
4627 *
4628 * @return estimated G-dependent cross biases.
4629 */
4630 @Override
4631 public Matrix getEstimatedGg() {
4632 return estimatedGg;
4633 }
4634
4635 /**
4636 * Gets estimated mean square error respect to provided measurements.
4637 *
4638 * @return estimated mean square error respect to provided measurements.
4639 */
4640 @Override
4641 public double getEstimatedMse() {
4642 return estimatedMse;
4643 }
4644
4645 /**
4646 * Gets estimated chi square value.
4647 *
4648 * @return estimated chi square value.
4649 */
4650 @Override
4651 public double getEstimatedChiSq() {
4652 return estimatedChiSq;
4653 }
4654
4655 /**
4656 * Gets estimated chi square degrees of freedom. Degrees of freedom is equal to the number of sampled data minus the
4657 * number of estimated parameters.
4658 *
4659 * @return estimated degrees of freedom of chi square value
4660 */
4661 @Override
4662 public int getEstimatedChiSqDegreesOfFreedom() {
4663 return estimatedChiSqDegreesOfFreedom;
4664 }
4665
4666 /**
4667 * Gets estimated reduced chi square value. This is equal to estimated chi square value divided by its degrees of
4668 * freedom. Ideally this value should be close to 1.0, indicating that fit is optimal.
4669 * A value larger than 1.0 indicates that fit is not good or noise has been underestimated, and a value smaller than
4670 * 1.0 indicates that there is overfitting or noise has been overestimated.
4671 *
4672 * @return estimated reduced chi square value
4673 */
4674 @Override
4675 public double getEstimatedReducedChiSq() {
4676 return estimatedReducedChiSq;
4677 }
4678
4679 /**
4680 * Gets estimated probability of finding a smaller chi square value expressed as a value between 0.0 and 1.0. The
4681 * smaller the found chi square value is, the better the fit of the estimated parameters to the actual parameter.
4682 * Thus, the smaller the chance of finding a smaller chi square value, then the better the estimated fit is.
4683 *
4684 * @return estimated probability of finding a smaller chi square value.
4685 */
4686 @Override
4687 public double getEstimatedP() {
4688 return estimatedP;
4689 }
4690
4691 /**
4692 * Gets estimated measure of quality of estimated fit as a value between 0.0 and 1.0. The larger the quality value
4693 * is, the better the fit that has been estimated.
4694 *
4695 * @return estimated measure of quality of estimated fit.
4696 */
4697 @Override
4698 public double getEstimatedQ() {
4699 return estimatedQ;
4700 }
4701
4702 /**
4703 * Gets estimated covariance matrix for estimated parameters.
4704 * Diagonal elements of the matrix contains variance for the following
4705 * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
4706 * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
4707 * gg13, gg23, gg33.
4708 *
4709 * @return estimated covariance matrix for estimated parameters.
4710 */
4711 @Override
4712 public Matrix getEstimatedCovariance() {
4713 return estimatedCovariance;
4714 }
4715
4716 /**
4717 * Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
4718 *
4719 * @return variance of estimated x coordinate of gyroscope bias or null if not available.
4720 */
4721 public Double getEstimatedBiasXVariance() {
4722 return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
4723 }
4724
4725 /**
4726 * Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
4727 * radians per second (rad/s).
4728 *
4729 * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
4730 * available.
4731 */
4732 public Double getEstimatedBiasXStandardDeviation() {
4733 final var variance = getEstimatedBiasXVariance();
4734 return variance != null ? Math.sqrt(variance) : null;
4735 }
4736
4737 /**
4738 * Gets standard deviation of estimated x coordinate of gyroscope bias.
4739 *
4740 * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
4741 * available.
4742 */
4743 public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
4744 return estimatedCovariance != null
4745 ? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
4746 }
4747
4748 /**
4749 * Gets standard deviation of estimated x coordinate of gyroscope bias.
4750 *
4751 * @param result instance where result will be stored.
4752 * @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
4753 * false otherwise.
4754 */
4755 public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
4756 if (estimatedCovariance != null) {
4757 result.setValue(getEstimatedBiasXStandardDeviation());
4758 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
4759 return true;
4760 } else {
4761 return false;
4762 }
4763 }
4764
4765 /**
4766 * Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
4767 *
4768 * @return variance of estimated y coordinate of gyroscope bias or null if not available.
4769 */
4770 public Double getEstimatedBiasYVariance() {
4771 return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
4772 }
4773
4774 /**
4775 * Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
4776 * radians per second (rad/s).
4777 *
4778 * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
4779 * available.
4780 */
4781 public Double getEstimatedBiasYStandardDeviation() {
4782 final var variance = getEstimatedBiasYVariance();
4783 return variance != null ? Math.sqrt(variance) : null;
4784 }
4785
4786 /**
4787 * Gets standard deviation of estimated y coordinate of gyroscope bias.
4788 *
4789 * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
4790 * available.
4791 */
4792 public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
4793 return estimatedCovariance != null
4794 ? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
4795 }
4796
4797 /**
4798 * Gets standard deviation of estimated y coordinate of gyroscope bias.
4799 *
4800 * @param result instance where result will be stored.
4801 * @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
4802 * false otherwise.
4803 */
4804 public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
4805 if (estimatedCovariance != null) {
4806 result.setValue(getEstimatedBiasYStandardDeviation());
4807 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
4808 return true;
4809 } else {
4810 return false;
4811 }
4812 }
4813
4814 /**
4815 * Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
4816 *
4817 * @return variance of estimated z coordinate of gyroscope bias or null if not available.
4818 */
4819 public Double getEstimatedBiasZVariance() {
4820 return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
4821 }
4822
4823 /**
4824 * Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
4825 * radians per second (rad/s).
4826 *
4827 * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
4828 * available.
4829 */
4830 public Double getEstimatedBiasZStandardDeviation() {
4831 final var variance = getEstimatedBiasZVariance();
4832 return variance != null ? Math.sqrt(variance) : null;
4833 }
4834
4835 /**
4836 * Gets standard deviation of estimated z coordinate of gyroscope bias.
4837 *
4838 * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
4839 * available.
4840 */
4841 public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
4842 return estimatedCovariance != null ?
4843 new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) :
4844 null;
4845 }
4846
4847 /**
4848 * Gets standard deviation of estimated z coordinate of gyroscope bias.
4849 *
4850 * @param result instance where result will be stored.
4851 * @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
4852 * false otherwise.
4853 */
4854 public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
4855 if (estimatedCovariance != null) {
4856 result.setValue(getEstimatedBiasZStandardDeviation());
4857 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
4858 return true;
4859 } else {
4860 return false;
4861 }
4862 }
4863
4864 /**
4865 * Gets standard deviation of estimated gyroscope bias coordinates.
4866 *
4867 * @return standard deviation of estimated gyroscope bias coordinates.
4868 */
4869 public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
4870 return estimatedCovariance != null
4871 ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
4872 getEstimatedBiasXStandardDeviation(),
4873 getEstimatedBiasYStandardDeviation(),
4874 getEstimatedBiasZStandardDeviation())
4875 : null;
4876 }
4877
4878 /**
4879 * Gets standard deviation of estimated gyroscope bias coordinates.
4880 *
4881 * @param result instance where result will be stored.
4882 * @return true if standard deviation of gyroscope bias was available, false
4883 * otherwise.
4884 */
4885 public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
4886 if (estimatedCovariance != null) {
4887 result.setValueCoordinatesAndUnit(
4888 getEstimatedBiasXStandardDeviation(),
4889 getEstimatedBiasYStandardDeviation(),
4890 getEstimatedBiasZStandardDeviation(),
4891 AngularSpeedUnit.RADIANS_PER_SECOND);
4892 return true;
4893 } else {
4894 return false;
4895 }
4896 }
4897
4898 /**
4899 * Gets average of estimated standard deviation of gyroscope bias coordinates expressed
4900 * in radians per second (rad/s).
4901 *
4902 * @return average of estimated standard deviation of gyroscope bias coordinates or null
4903 * if not available.
4904 */
4905 public Double getEstimatedBiasStandardDeviationAverage() {
4906 return estimatedCovariance != null
4907 ? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
4908 + getEstimatedBiasZStandardDeviation()) / 3.0
4909 : null;
4910 }
4911
4912 /**
4913 * Gets average of estimated standard deviation of gyroscope bias coordinates.
4914 *
4915 * @return average of estimated standard deviation of gyroscope bias coordinates or null.
4916 */
4917 public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
4918 return estimatedCovariance != null
4919 ? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
4920 : null;
4921 }
4922
4923 /**
4924 * Gets average of estimated standard deviation of gyroscope bias coordinates.
4925 *
4926 * @param result instance where result will be stored.
4927 * @return true if average of estimated standard deviation of gyroscope bias is available,
4928 * false otherwise.
4929 */
4930 public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
4931 if (estimatedCovariance != null) {
4932 result.setValue(getEstimatedBiasStandardDeviationAverage());
4933 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
4934 return true;
4935 } else {
4936 return false;
4937 }
4938 }
4939
4940 /**
4941 * Gets norm of estimated standard deviation of gyroscope bias expressed in
4942 * radians per second (rad/s).
4943 * This can be used as the initial gyroscope bias uncertainty for
4944 * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
4945 *
4946 * @return norm of estimated standard deviation of gyroscope bias or null
4947 * if not available.
4948 */
4949 @Override
4950 public Double getEstimatedBiasStandardDeviationNorm() {
4951 return estimatedCovariance != null
4952 ? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
4953 : null;
4954 }
4955
4956 /**
4957 * Gets norm of estimated standard deviation of gyroscope bias.
4958 * This can be used as the initial gyroscope bias uncertainty for
4959 * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
4960 *
4961 * @return norm of estimated standard deviation of gyroscope bias or null
4962 * if not available.
4963 */
4964 public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
4965 return estimatedCovariance != null
4966 ? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
4967 : null;
4968 }
4969
4970 /**
4971 * Gets norm of estimated standard deviation of gyroscope bias coordinates.
4972 * This can be used as the initial gyroscope bias uncertainty for
4973 * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
4974 *
4975 * @param result instance where result will be stored.
4976 * @return true if norm of estimated standard deviation of gyroscope bias is
4977 * available, false otherwise.
4978 */
4979 public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
4980 if (estimatedCovariance != null) {
4981 result.setValue(getEstimatedBiasStandardDeviationNorm());
4982 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
4983 return true;
4984 } else {
4985 return false;
4986 }
4987 }
4988
4989 /**
4990 * Gets size of subsets to be checked during robust estimation.
4991 * This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences()}.
4992 *
4993 * @return size of subsets to be checked during robust estimation.
4994 */
4995 public int getPreliminarySubsetSize() {
4996 return preliminarySubsetSize;
4997 }
4998
4999 /**
5000 * Sets size of subsets to be checked during robust estimation.
5001 * This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences}.
5002 *
5003 * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
5004 * @throws LockedException if calibrator is currently running.
5005 * @throws IllegalArgumentException if provided value is less than
5006 * {@link #getMinimumRequiredMeasurementsOrSequences}.
5007 */
5008 public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
5009 if (running) {
5010 throw new LockedException();
5011 }
5012 if (preliminarySubsetSize < getMinimumRequiredMeasurementsOrSequences()) {
5013 throw new IllegalArgumentException();
5014 }
5015
5016 this.preliminarySubsetSize = preliminarySubsetSize;
5017 }
5018
5019 /**
5020 * Returns method being used for robust estimation.
5021 *
5022 * @return method being used for robust estimation.
5023 */
5024 public abstract RobustEstimatorMethod getMethod();
5025
5026 /**
5027 * Creates a robust gyroscope calibrator.
5028 *
5029 * @param method robust estimator method.
5030 * @return a robust gyroscope calibrator.
5031 */
5032 public static RobustTurntableGyroscopeCalibrator create(final RobustEstimatorMethod method) {
5033 return switch (method) {
5034 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator();
5035 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator();
5036 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator();
5037 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator();
5038 default -> new PROMedSRobustTurntableGyroscopeCalibrator();
5039 };
5040 }
5041
5042 /**
5043 * Creates a robust gyroscope calibrator.
5044 *
5045 * @param position position where body kinematics measures
5046 * have been taken.
5047 * @param turntableRotationRate constant rotation rate at which the
5048 * turntable is spinning. Must be
5049 * expressed in radians per second (rad/s).
5050 * @param timeInterval time interval between measurements being
5051 * captured expressed in seconds (s).
5052 * @param measurements collection of body kinematics
5053 * measurements with standard deviations
5054 * taken at the same position with zero
5055 * velocity and unknown different
5056 * orientations.
5057 * @param initialBias initial gyroscope bias to be used to
5058 * find a solution. This must be 3x1 and
5059 * is expressed in radians per second
5060 * (rad/s).
5061 * @param initialMg initial gyroscope scale factors and
5062 * cross coupling errors matrix. Must
5063 * be 3x3.
5064 * @param initialGg initial gyroscope G-dependent cross
5065 * biases introduced on the gyroscope by
5066 * the specific forces sensed by the
5067 * accelerometer. Must be 3x3.
5068 * @param method robust estimator method.
5069 * @return a robust gyroscope calibrator.
5070 * @throws IllegalArgumentException if any of the provided values does
5071 * not have proper size or if either
5072 * turntable rotation rate or
5073 * time interval is zero or negative.
5074 */
5075 public static RobustTurntableGyroscopeCalibrator create(
5076 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5077 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
5078 final Matrix initialGg, final RobustEstimatorMethod method) {
5079 return switch (method) {
5080 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5081 measurements, initialBias, initialMg, initialGg);
5082 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5083 measurements, initialBias, initialMg, initialGg);
5084 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5085 measurements, initialBias, initialMg, initialGg);
5086 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5087 measurements, initialBias, initialMg, initialGg);
5088 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5089 measurements, initialBias, initialMg, initialGg);
5090 };
5091 }
5092
5093 /**
5094 * Creates a robust gyroscope calibrator.
5095 *
5096 * @param position position where body kinematics measures
5097 * have been taken.
5098 * @param turntableRotationRate constant rotation rate at which the
5099 * turntable is spinning. Must be
5100 * expressed in radians per second (rad/s).
5101 * @param timeInterval time interval between measurements being
5102 * captured expressed in seconds (s).
5103 * @param measurements collection of body kinematics
5104 * measurements with standard deviations
5105 * taken at the same position with zero
5106 * velocity and unknown different
5107 * orientations.
5108 * @param initialBias initial gyroscope bias to be used to
5109 * find a solution. This must be 3x1 and
5110 * is expressed in radians per second
5111 * (rad/s).
5112 * @param initialMg initial gyroscope scale factors and
5113 * cross coupling errors matrix. Must
5114 * be 3x3.
5115 * @param initialGg initial gyroscope G-dependent cross
5116 * biases introduced on the gyroscope by
5117 * the specific forces sensed by the
5118 * accelerometer. Must be 3x3.
5119 * @param listener listener to handle events raised by this
5120 * calibrator.
5121 * @param method robust estimator method.
5122 * @return a robust gyroscope calibrator.
5123 * @throws IllegalArgumentException if any of the provided values does
5124 * not have proper size or if either
5125 * turntable rotation rate or
5126 * time interval is zero or negative.
5127 */
5128 public static RobustTurntableGyroscopeCalibrator create(
5129 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5130 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
5131 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
5132 final RobustEstimatorMethod method) {
5133 return switch (method) {
5134 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5135 measurements, initialBias, initialMg, initialGg, listener);
5136 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5137 measurements, initialBias, initialMg, initialGg, listener);
5138 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5139 measurements, initialBias, initialMg, initialGg, listener);
5140 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5141 measurements, initialBias, initialMg, initialGg, listener);
5142 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5143 measurements, initialBias, initialMg, initialGg, listener);
5144 };
5145 }
5146
5147 /**
5148 * Creates a robust gyroscope calibrator.
5149 *
5150 * @param position position where body kinematics measures
5151 * have been taken.
5152 * @param turntableRotationRate constant rotation rate at which the
5153 * turntable is spinning. Must be
5154 * expressed in radians per second (rad/s).
5155 * @param timeInterval time interval between measurements being
5156 * captured expressed in seconds (s).
5157 * @param measurements collection of body kinematics
5158 * measurements with standard deviations
5159 * taken at the same position with zero
5160 * velocity and unknown different
5161 * orientations.
5162 * @param initialBias initial gyroscope bias to be used to
5163 * find a solution. This must have
5164 * length 3 and is expressed in radians
5165 * per second (rad/s).
5166 * @param initialMg initial gyroscope scale factors and
5167 * cross coupling errors matrix. Must
5168 * be 3x3.
5169 * @param initialGg initial gyroscope G-dependent cross
5170 * biases introduced on the gyroscope by
5171 * the specific forces sensed by the
5172 * accelerometer. Must be 3x3.
5173 * @param method robust estimator method.
5174 * @return a robust gyroscope calibrator.
5175 * @throws IllegalArgumentException if any of the provided values does
5176 * not have proper size or if either
5177 * turntable rotation rate or
5178 * time interval is zero or negative.
5179 */
5180 public static RobustTurntableGyroscopeCalibrator create(
5181 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5182 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
5183 final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
5184 return switch (method) {
5185 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5186 measurements, initialBias, initialMg, initialGg);
5187 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5188 measurements, initialBias, initialMg, initialGg);
5189 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5190 measurements, initialBias, initialMg, initialGg);
5191 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5192 measurements, initialBias, initialMg, initialGg);
5193 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5194 measurements, initialBias, initialMg, initialGg);
5195 };
5196 }
5197
5198 /**
5199 * Creates a robust gyroscope calibrator.
5200 *
5201 * @param position position where body kinematics measures
5202 * have been taken.
5203 * @param turntableRotationRate constant rotation rate at which the
5204 * turntable is spinning. Must be
5205 * expressed in radians per second (rad/s).
5206 * @param timeInterval time interval between measurements being
5207 * captured expressed in seconds (s).
5208 * @param measurements collection of body kinematics
5209 * measurements with standard deviations
5210 * taken at the same position with zero
5211 * velocity and unknown different
5212 * orientations.
5213 * @param initialBias initial gyroscope bias to be used to
5214 * find a solution. This must have
5215 * length 3 and is expressed in radians
5216 * per second (rad/s).
5217 * @param initialMg initial gyroscope scale factors and
5218 * cross coupling errors matrix. Must
5219 * be 3x3.
5220 * @param initialGg initial gyroscope G-dependent cross
5221 * biases introduced on the gyroscope by
5222 * the specific forces sensed by the
5223 * accelerometer. Must be 3x3.
5224 * @param listener listener to handle events raised by
5225 * this calibrator.
5226 * @param method robust estimator method.
5227 * @return a robust gyroscope calibrator.
5228 * @throws IllegalArgumentException if any of the provided values does
5229 * not have proper size or if either
5230 * turntable rotation rate or
5231 * time interval is zero or negative.
5232 */
5233 public static RobustTurntableGyroscopeCalibrator create(
5234 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5235 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
5236 final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
5237 final RobustEstimatorMethod method) {
5238 return switch (method) {
5239 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5240 measurements, initialBias, initialMg, initialGg, listener);
5241 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5242 measurements, initialBias, initialMg, initialGg, listener);
5243 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5244 measurements, initialBias, initialMg, initialGg, listener);
5245 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5246 measurements, initialBias, initialMg, initialGg, listener);
5247 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5248 measurements, initialBias, initialMg, initialGg, listener);
5249 };
5250 }
5251
5252 /**
5253 * Creates a robust gyroscope calibrator.
5254 *
5255 * @param position position where body kinematics measures
5256 * have been taken.
5257 * @param turntableRotationRate constant rotation rate at which the
5258 * turntable is spinning. Must be
5259 * expressed in radians per second (rad/s).
5260 * @param timeInterval time interval between measurements being
5261 * captured expressed in seconds (s).
5262 * @param measurements collection of body kinematics
5263 * measurements with standard deviations
5264 * taken at the same position with zero
5265 * velocity and unknown different
5266 * orientations.
5267 * @param initialBias initial gyroscope bias to be used to
5268 * find a solution. This must have length
5269 * 3 and is expressed in radians per
5270 * second (rad/s).
5271 * @param initialMg initial gyroscope scale factors and
5272 * cross coupling errors matrix. Must
5273 * be 3x3.
5274 * @param initialGg initial gyroscope G-dependent cross
5275 * biases introduced on the gyroscope by
5276 * the specific forces sensed by the
5277 * accelerometer. Must be 3x3.
5278 * @param accelerometerBias known accelerometer bias. This must
5279 * have length 3 and is expressed in
5280 * meters per squared second
5281 * (m/s^2).
5282 * @param accelerometerMa known accelerometer scale factors and
5283 * cross coupling matrix. Must be 3x3.
5284 * @param method robust estimator method.
5285 * @return a robust gyroscope calibrator.
5286 * @throws IllegalArgumentException if any of the provided values does
5287 * not have proper size or if either
5288 * turntable rotation rate or
5289 * time interval is zero or negative.
5290 */
5291 public static RobustTurntableGyroscopeCalibrator create(
5292 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5293 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
5294 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
5295 final Matrix accelerometerMa, final RobustEstimatorMethod method) {
5296 return switch (method) {
5297 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5298 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
5299 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5300 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
5301 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5302 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
5303 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5304 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
5305 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5306 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
5307 };
5308 }
5309
5310 /**
5311 * Creates a robust gyroscope calibrator.
5312 *
5313 * @param position position where body kinematics measures
5314 * have been taken.
5315 * @param turntableRotationRate constant rotation rate at which the
5316 * turntable is spinning. Must be
5317 * expressed in radians per second (rad/s).
5318 * @param timeInterval time interval between measurements being
5319 * captured expressed in seconds (s).
5320 * @param measurements collection of body kinematics
5321 * measurements with standard deviations
5322 * taken at the same position with zero
5323 * velocity and unknown different
5324 * orientations.
5325 * @param initialBias initial gyroscope bias to be used to
5326 * find a solution. This must have length
5327 * 3 and is expressed in radians per
5328 * second (rad/s).
5329 * @param initialMg initial gyroscope scale factors and
5330 * cross coupling errors matrix. Must
5331 * be 3x3.
5332 * @param initialGg initial gyroscope G-dependent cross
5333 * biases introduced on the gyroscope by
5334 * the specific forces sensed by the
5335 * accelerometer. Must be 3x3.
5336 * @param accelerometerBias known accelerometer bias. This must
5337 * have length 3 and is expressed in
5338 * meters per squared second
5339 * (m/s^2).
5340 * @param accelerometerMa known accelerometer scale factors and
5341 * cross coupling matrix. Must be 3x3.
5342 * @param listener listener to handle events raised by
5343 * this calibrator.
5344 * @param method robust estimator method.
5345 * @return a robust gyroscope calibrator.
5346 * @throws IllegalArgumentException if any of the provided values does
5347 * not have proper size or if either
5348 * turntable rotation rate or
5349 * time interval is zero or negative.
5350 */
5351 public static RobustTurntableGyroscopeCalibrator create(
5352 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5353 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
5354 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
5355 final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
5356 final RobustEstimatorMethod method) {
5357 return switch (method) {
5358 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5359 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
5360 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5361 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
5362 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5363 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
5364 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5365 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
5366 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5367 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
5368 };
5369 }
5370
5371 /**
5372 * Creates a robust gyroscope calibrator.
5373 *
5374 * @param position position where body kinematics measures
5375 * have been taken.
5376 * @param turntableRotationRate constant rotation rate at which the
5377 * turntable is spinning. Must be
5378 * expressed in radians per second (rad/s).
5379 * @param timeInterval time interval between measurements being
5380 * captured expressed in seconds (s).
5381 * @param measurements collection of body kinematics
5382 * measurements with standard deviations
5383 * taken at the same position with zero
5384 * velocity and unknown different
5385 * orientations.
5386 * @param initialBias initial gyroscope bias to be used to
5387 * find a solution. This must be 3x1 and
5388 * is expressed in radians per second
5389 * (rad/s).
5390 * @param initialMg initial gyroscope scale factors and
5391 * cross coupling errors matrix. Must
5392 * be 3x3.
5393 * @param initialGg initial gyroscope G-dependent cross
5394 * biases introduced on the gyroscope by
5395 * the specific forces sensed by the
5396 * accelerometer. Must be 3x3.
5397 * @param accelerometerBias known accelerometer bias. This must
5398 * have length 3 and is expressed in
5399 * meters per squared second
5400 * (m/s^2).
5401 * @param accelerometerMa known accelerometer scale factors and
5402 * cross coupling matrix. Must be 3x3.
5403 * @param method robust estimator method.
5404 * @return a robust gyroscope calibrator.
5405 * @throws IllegalArgumentException if any of the provided values does
5406 * not have proper size or if either
5407 * turntable rotation rate or
5408 * time interval is zero or negative.
5409 */
5410 public static RobustTurntableGyroscopeCalibrator create(
5411 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5412 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
5413 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
5414 final RobustEstimatorMethod method) {
5415 return switch (method) {
5416 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5417 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
5418 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5419 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
5420 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5421 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
5422 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5423 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
5424 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5425 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
5426 };
5427 }
5428
5429 /**
5430 * Creates a robust gyroscope calibrator.
5431 *
5432 * @param position position where body kinematics measures
5433 * have been taken.
5434 * @param turntableRotationRate constant rotation rate at which the
5435 * turntable is spinning. Must be
5436 * expressed in radians per second (rad/s).
5437 * @param timeInterval time interval between measurements being
5438 * captured expressed in seconds (s).
5439 * @param measurements collection of body kinematics
5440 * measurements with standard deviations
5441 * taken at the same position with zero
5442 * velocity and unknown different
5443 * orientations.
5444 * @param initialBias initial gyroscope bias to be used to
5445 * find a solution. This must be 3x1 and
5446 * is expressed in radians per second
5447 * (rad/s).
5448 * @param initialMg initial gyroscope scale factors and
5449 * cross coupling errors matrix. Must
5450 * be 3x3.
5451 * @param initialGg initial gyroscope G-dependent cross
5452 * biases introduced on the gyroscope by
5453 * the specific forces sensed by the
5454 * accelerometer. Must be 3x3.
5455 * @param accelerometerBias known accelerometer bias. This must
5456 * have length 3 and is expressed in
5457 * meters per squared second
5458 * (m/s^2).
5459 * @param accelerometerMa known accelerometer scale factors and
5460 * cross coupling matrix. Must be 3x3.
5461 * @param listener listener to handle events raised by
5462 * this calibrator.
5463 * @param method robust estimator method.
5464 * @return a robust gyroscope calibrator.
5465 * @throws IllegalArgumentException if any of the provided values does
5466 * not have proper size or if either
5467 * turntable rotation rate or
5468 * time interval is zero or negative.
5469 */
5470 public static RobustTurntableGyroscopeCalibrator create(
5471 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5472 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
5473 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
5474 final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
5475 return switch (method) {
5476 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5477 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
5478 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5479 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
5480 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5481 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
5482 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5483 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
5484 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5485 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
5486 };
5487 }
5488
5489 /**
5490 * Creates a robust gyroscope calibrator.
5491 *
5492 * @param position position where body kinematics
5493 * measures have been taken.
5494 * @param turntableRotationRate constant rotation rate at which
5495 * the turntable is spinning. Must
5496 * be expressed in radians per
5497 * second (rad/s).
5498 * @param timeInterval time interval between measurements
5499 * being captured expressed in
5500 * seconds (s).
5501 * @param measurements collection of body kinematics
5502 * measurements with standard
5503 * deviations taken at the same
5504 * position with zero velocity
5505 * and unknown different
5506 * orientations.
5507 * @param commonAxisUsed indicates whether z-axis is
5508 * assumed to be common for
5509 * accelerometer and gyroscope.
5510 * @param estimateGDependentCrossBiases true if G-dependent cross biases
5511 * will be estimated, false
5512 * otherwise.
5513 * @param initialBias initial gyroscope bias to be
5514 * used to find a solution. This
5515 * must be 3x1 and is expressed in
5516 * radians per second (rad/s).
5517 * @param initialMg initial gyroscope scale factors
5518 * and cross coupling errors matrix.
5519 * Must be 3x3.
5520 * @param initialGg initial gyroscope G-dependent
5521 * cross biases introduced on the
5522 * gyroscope by the specific
5523 * forces sensed by the
5524 * accelerometer. Must be 3x3.
5525 * @param method robust estimator method.
5526 * @return a robust gyroscope calibrator.
5527 * @throws IllegalArgumentException if any of the provided values does
5528 * not have proper size or if either
5529 * turntable rotation rate or
5530 * time interval is zero or negative.
5531 */
5532 public static RobustTurntableGyroscopeCalibrator create(
5533 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5534 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
5535 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
5536 final Matrix initialGg, final RobustEstimatorMethod method) {
5537 return switch (method) {
5538 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5539 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
5540 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5541 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
5542 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5543 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
5544 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5545 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
5546 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5547 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
5548 };
5549 }
5550
5551 /**
5552 * Creates a robust gyroscope calibrator.
5553 *
5554 * @param position position where body kinematics
5555 * measures have been taken.
5556 * @param turntableRotationRate constant rotation rate at which
5557 * the turntable is spinning. Must
5558 * be expressed in radians per
5559 * second (rad/s).
5560 * @param timeInterval time interval between measurements
5561 * being captured expressed in
5562 * seconds (s).
5563 * @param measurements collection of body kinematics
5564 * measurements with standard
5565 * deviations taken at the same
5566 * position with zero velocity
5567 * and unknown different
5568 * orientations.
5569 * @param commonAxisUsed indicates whether z-axis is
5570 * assumed to be common for
5571 * accelerometer and gyroscope.
5572 * @param estimateGDependentCrossBiases true if G-dependent cross biases
5573 * will be estimated, false
5574 * otherwise.
5575 * @param initialBias initial gyroscope bias to be
5576 * used to find a solution. This
5577 * must be 3x1 and is expressed in
5578 * radians per second (rad/s).
5579 * @param initialMg initial gyroscope scale factors
5580 * and cross coupling errors matrix.
5581 * Must be 3x3.
5582 * @param initialGg initial gyroscope G-dependent
5583 * cross biases introduced on the
5584 * gyroscope by the specific
5585 * forces sensed by the
5586 * accelerometer. Must be 3x3.
5587 * @param listener listener to handle events raised by
5588 * this calibrator.
5589 * @param method robust estimator method.
5590 * @return a robust gyroscope calibrator.
5591 * @throws IllegalArgumentException if any of the provided values does
5592 * not have proper size or if either
5593 * turntable rotation rate or
5594 * time interval is zero or negative.
5595 */
5596 public static RobustTurntableGyroscopeCalibrator create(
5597 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5598 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
5599 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
5600 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
5601 final RobustEstimatorMethod method) {
5602 return switch (method) {
5603 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5604 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5605 listener);
5606 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5607 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5608 listener);
5609 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5610 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5611 listener);
5612 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5613 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5614 listener);
5615 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5616 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5617 listener);
5618 };
5619 }
5620
5621 /**
5622 * Creates a robust gyroscope calibrator.
5623 *
5624 * @param position position where body kinematics
5625 * measures have been taken.
5626 * @param turntableRotationRate constant rotation rate at which
5627 * the turntable is spinning. Must
5628 * be expressed in radians per
5629 * second (rad/s).
5630 * @param timeInterval time interval between measurements
5631 * being captured expressed in
5632 * seconds (s).
5633 * @param measurements collection of body kinematics
5634 * measurements with standard
5635 * deviations taken at the same
5636 * position with zero velocity
5637 * and unknown different
5638 * orientations.
5639 * @param commonAxisUsed indicates whether z-axis is
5640 * assumed to be common for
5641 * accelerometer and gyroscope.
5642 * @param estimateGDependentCrossBiases true if G-dependent cross biases
5643 * will be estimated, false
5644 * otherwise.
5645 * @param initialBias initial gyroscope bias to be
5646 * used to find a solution. This
5647 * must have length 3 and is
5648 * expressed in radians per second
5649 * (rad/s).
5650 * @param initialMg initial gyroscope scale factors
5651 * and cross coupling errors matrix.
5652 * Must be 3x3.
5653 * @param initialGg initial gyroscope G-dependent
5654 * cross biases introduced on the
5655 * gyroscope by the specific forces
5656 * sensed by the accelerometer.
5657 * Must be 3x3.
5658 * @param method robust estimator method.
5659 * @return a robust gyroscope calibrator.
5660 * @throws IllegalArgumentException if any of the provided values does
5661 * not have proper size or if either
5662 * turntable rotation rate or
5663 * time interval is zero or negative.
5664 */
5665 public static RobustTurntableGyroscopeCalibrator create(
5666 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5667 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
5668 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
5669 final Matrix initialGg, final RobustEstimatorMethod method) {
5670 return switch (method) {
5671 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5672 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
5673 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5674 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
5675 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5676 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
5677 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5678 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
5679 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5680 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
5681 };
5682 }
5683
5684 /**
5685 * Creates a robust gyroscope calibrator.
5686 *
5687 * @param position position where body kinematics
5688 * measures have been taken.
5689 * @param turntableRotationRate constant rotation rate at which
5690 * the turntable is spinning. Must
5691 * be expressed in radians per
5692 * second (rad/s).
5693 * @param timeInterval time interval between measurements
5694 * being captured expressed in
5695 * seconds (s).
5696 * @param measurements collection of body kinematics
5697 * measurements with standard
5698 * deviations taken at the same
5699 * position with zero velocity
5700 * and unknown different
5701 * orientations.
5702 * @param commonAxisUsed indicates whether z-axis is
5703 * assumed to be common for
5704 * accelerometer and gyroscope.
5705 * @param estimateGDependentCrossBiases true if G-dependent cross biases
5706 * will be estimated, false
5707 * otherwise.
5708 * @param initialBias initial gyroscope bias to be
5709 * used to find a solution. This
5710 * must have length 3 and is
5711 * expressed in radians per second
5712 * (rad/s).
5713 * @param initialMg initial gyroscope scale factors
5714 * and cross coupling errors matrix.
5715 * Must be 3x3.
5716 * @param initialGg initial gyroscope G-dependent
5717 * cross biases introduced on the
5718 * gyroscope by the specific forces
5719 * sensed by the accelerometer.
5720 * Must be 3x3.
5721 * @param listener listener to handle events raised
5722 * by this calibrator.
5723 * @param method robust estimator method.
5724 * @return a robust gyroscope calibrator.
5725 * @throws IllegalArgumentException if any of the provided values does
5726 * not have proper size or if either
5727 * turntable rotation rate or
5728 * time interval is zero or negative.
5729 */
5730 public static RobustTurntableGyroscopeCalibrator create(
5731 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5732 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
5733 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
5734 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
5735 final RobustEstimatorMethod method) {
5736 return switch (method) {
5737 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(
5738 position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
5739 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
5740 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(
5741 position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
5742 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
5743 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(
5744 position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
5745 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
5746 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(
5747 position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
5748 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
5749 default -> new PROMedSRobustTurntableGyroscopeCalibrator(
5750 position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
5751 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
5752 };
5753 }
5754
5755 /**
5756 * Creates a robust gyroscope calibrator.
5757 *
5758 * @param position position where body kinematics
5759 * measures have been taken.
5760 * @param turntableRotationRate constant rotation rate at which
5761 * the turntable is spinning. Must
5762 * be expressed in radians per
5763 * second (rad/s).
5764 * @param timeInterval time interval between measurements
5765 * being captured expressed in
5766 * seconds (s).
5767 * @param measurements collection of body kinematics
5768 * measurements with standard
5769 * deviations taken at the same
5770 * position with zero velocity
5771 * and unknown different
5772 * orientations.
5773 * @param commonAxisUsed indicates whether z-axis is
5774 * assumed to be common for
5775 * accelerometer and gyroscope.
5776 * @param estimateGDependentCrossBiases true if G-dependent cross
5777 * biases will be estimated,
5778 * false otherwise.
5779 * @param initialBias initial gyroscope bias to be
5780 * used to find a solution. This
5781 * must have length 3 and is
5782 * expressed in radians per second
5783 * (rad/s).
5784 * @param initialMg initial gyroscope scale factors
5785 * and cross coupling errors
5786 * matrix. Must be 3x3.
5787 * @param initialGg initial gyroscope G-dependent
5788 * cross biases introduced on the
5789 * gyroscope by the specific forces
5790 * sensed by the accelerometer.
5791 * Must be 3x3.
5792 * @param accelerometerBias known accelerometer bias. This
5793 * must have length 3 and is
5794 * expressed in meters per squared
5795 * second (m/s^2).
5796 * @param accelerometerMa known accelerometer scale factors
5797 * and cross coupling matrix. Must
5798 * be 3x3.
5799 * @param method robust estimator method.
5800 * @return a robust gyroscope calibrator.
5801 * @throws IllegalArgumentException if any of the provided values does
5802 * not have proper size or if either
5803 * turntable rotation rate or
5804 * time interval is zero or negative.
5805 */
5806 public static RobustTurntableGyroscopeCalibrator create(
5807 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5808 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
5809 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
5810 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
5811 final RobustEstimatorMethod method) {
5812 return switch (method) {
5813 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5814 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5815 accelerometerBias, accelerometerMa);
5816 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5817 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5818 accelerometerBias, accelerometerMa);
5819 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5820 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5821 accelerometerBias, accelerometerMa);
5822 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5823 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5824 accelerometerBias, accelerometerMa);
5825 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5826 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5827 accelerometerBias, accelerometerMa);
5828 };
5829 }
5830
5831 /**
5832 * Creates a robust gyroscope calibrator.
5833 *
5834 * @param position position where body kinematics
5835 * measures have been taken.
5836 * @param turntableRotationRate constant rotation rate at which
5837 * the turntable is spinning. Must
5838 * be expressed in radians per
5839 * second (rad/s).
5840 * @param timeInterval time interval between measurements
5841 * being captured expressed in
5842 * seconds (s).
5843 * @param measurements collection of body kinematics
5844 * measurements with standard
5845 * deviations taken at the same
5846 * position with zero velocity
5847 * and unknown different
5848 * orientations.
5849 * @param commonAxisUsed indicates whether z-axis is
5850 * assumed to be common for
5851 * accelerometer and gyroscope.
5852 * @param estimateGDependentCrossBiases true if G-dependent cross
5853 * biases will be estimated,
5854 * false otherwise.
5855 * @param initialBias initial gyroscope bias to be
5856 * used to find a solution. This
5857 * must have length 3 and is
5858 * expressed in radians per second
5859 * (rad/s).
5860 * @param initialMg initial gyroscope scale factors
5861 * and cross coupling errors
5862 * matrix. Must be 3x3.
5863 * @param initialGg initial gyroscope G-dependent
5864 * cross biases introduced on the
5865 * gyroscope by the specific forces
5866 * sensed by the accelerometer.
5867 * Must be 3x3.
5868 * @param accelerometerBias known accelerometer bias. This
5869 * must have length 3 and is
5870 * expressed in meters per squared
5871 * second (m/s^2).
5872 * @param accelerometerMa known accelerometer scale factors
5873 * and cross coupling matrix. Must
5874 * be 3x3.
5875 * @param listener listener to handle events raised
5876 * by this calibrator.
5877 * @param method robust estimator method.
5878 * @return a robust gyroscope calibrator.
5879 * @throws IllegalArgumentException if any of the provided values does
5880 * not have proper size or if either
5881 * turntable rotation rate or
5882 * time interval is zero or negative.
5883 */
5884 public static RobustTurntableGyroscopeCalibrator create(
5885 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5886 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
5887 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
5888 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
5889 final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
5890 return switch (method) {
5891 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5892 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5893 accelerometerBias, accelerometerMa, listener);
5894 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5895 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5896 accelerometerBias, accelerometerMa, listener);
5897 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5898 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5899 accelerometerBias, accelerometerMa, listener);
5900 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5901 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5902 accelerometerBias, accelerometerMa, listener);
5903 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5904 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5905 accelerometerBias, accelerometerMa, listener);
5906 };
5907 }
5908
5909 /**
5910 * Creates a robust gyroscope calibrator.
5911 *
5912 * @param position position where body kinematics
5913 * measures have been taken.
5914 * @param turntableRotationRate constant rotation rate at which
5915 * the turntable is spinning. Must
5916 * be expressed in radians per
5917 * second (rad/s).
5918 * @param timeInterval time interval between measurements
5919 * being captured expressed in
5920 * seconds (s).
5921 * @param measurements collection of body kinematics
5922 * measurements with standard
5923 * deviations taken at the same
5924 * position with zero velocity and
5925 * unknown different orientations.
5926 * @param commonAxisUsed indicates whether z-axis is
5927 * assumed to be common for
5928 * accelerometer and gyroscope.
5929 * @param estimateGDependentCrossBiases true if G-dependent cross biases
5930 * will be estimated, false
5931 * otherwise.
5932 * @param initialBias initial gyroscope bias to be
5933 * used to find a solution. This
5934 * must be 3x1 and is expressed in
5935 * radians per second (rad/s).
5936 * @param initialMg initial gyroscope scale factors
5937 * and cross coupling errors matrix.
5938 * Must be 3x3.
5939 * @param initialGg initial gyroscope G-dependent
5940 * cross biases introduced on the
5941 * gyroscope by the specific forces
5942 * sensed by the accelerometer. Must
5943 * be 3x3.
5944 * @param accelerometerBias known accelerometer bias. This
5945 * must have length 3 and is
5946 * expressed in meters per squared
5947 * second (m/s^2).
5948 * @param accelerometerMa known accelerometer scale factors
5949 * and cross coupling matrix. Must
5950 * be 3x3.
5951 * @param method robust estimator method.
5952 * @return a robust gyroscope calibrator.
5953 * @throws IllegalArgumentException if any of the provided values does
5954 * not have proper size or if either
5955 * turntable rotation rate or
5956 * time interval is zero or negative.
5957 */
5958 public static RobustTurntableGyroscopeCalibrator create(
5959 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
5960 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
5961 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
5962 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
5963 final RobustEstimatorMethod method) {
5964 return switch (method) {
5965 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5966 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5967 accelerometerBias, accelerometerMa);
5968 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5969 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5970 accelerometerBias, accelerometerMa);
5971 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5972 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5973 accelerometerBias, accelerometerMa);
5974 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5975 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5976 accelerometerBias, accelerometerMa);
5977 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
5978 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
5979 accelerometerBias, accelerometerMa);
5980 };
5981 }
5982
5983 /**
5984 * Creates a robust gyroscope calibrator.
5985 *
5986 * @param position position where body kinematics
5987 * measures have been taken.
5988 * @param turntableRotationRate constant rotation rate at which
5989 * the turntable is spinning. Must
5990 * be expressed in radians per
5991 * second (rad/s).
5992 * @param timeInterval time interval between measurements
5993 * being captured expressed in
5994 * seconds (s).
5995 * @param measurements collection of body kinematics
5996 * measurements with standard
5997 * deviations taken at the same
5998 * position with zero velocity and
5999 * unknown different orientations.
6000 * @param commonAxisUsed indicates whether z-axis is
6001 * assumed to be common for
6002 * accelerometer and gyroscope.
6003 * @param estimateGDependentCrossBiases true if G-dependent cross biases
6004 * will be estimated, false
6005 * otherwise.
6006 * @param initialBias initial gyroscope bias to be
6007 * used to find a solution. This
6008 * must be 3x1 and is expressed in
6009 * radians per second (rad/s).
6010 * @param initialMg initial gyroscope scale factors
6011 * and cross coupling errors matrix.
6012 * Must be 3x3.
6013 * @param initialGg initial gyroscope G-dependent
6014 * cross biases introduced on the
6015 * gyroscope by the specific forces
6016 * sensed by the accelerometer. Must
6017 * be 3x3.
6018 * @param accelerometerBias known accelerometer bias. This
6019 * must have length 3 and is
6020 * expressed in meters per squared
6021 * second (m/s^2).
6022 * @param accelerometerMa known accelerometer scale factors
6023 * and cross coupling matrix. Must
6024 * be 3x3.
6025 * @param listener listener to handle events raised
6026 * by this calibrator.
6027 * @param method robust estimator method.
6028 * @return a robust gyroscope calibrator.
6029 * @throws IllegalArgumentException if any of the provided values does
6030 * not have proper size or if either
6031 * turntable rotation rate or
6032 * time interval is zero or negative.
6033 */
6034 public static RobustTurntableGyroscopeCalibrator create(
6035 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
6036 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
6037 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
6038 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
6039 final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
6040 return switch (method) {
6041 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6042 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6043 accelerometerBias, accelerometerMa, listener);
6044 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6045 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6046 accelerometerBias, accelerometerMa, listener);
6047 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6048 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6049 accelerometerBias, accelerometerMa, listener);
6050 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6051 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6052 accelerometerBias, accelerometerMa, listener);
6053 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6054 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6055 accelerometerBias, accelerometerMa, listener);
6056 };
6057 }
6058
6059 /**
6060 * Creates a robust gyroscope calibrator.
6061 *
6062 * @param position position where body kinematics measures
6063 * have been taken.
6064 * @param turntableRotationRate constant rotation rate at which the
6065 * turntable is spinning. Must be
6066 * expressed in radians per second (rad/s).
6067 * @param timeInterval time interval between measurements being
6068 * captured expressed in seconds (s).
6069 * @param measurements collection of body kinematics
6070 * measurements with standard deviations
6071 * taken at the same position with zero
6072 * velocity and unknown different
6073 * orientations.
6074 * @param initialBias initial gyroscope bias to be used to
6075 * find a solution. This must be 3x1 and
6076 * is expressed in radians per second
6077 * (rad/s).
6078 * @param initialMg initial gyroscope scale factors and
6079 * cross coupling errors matrix. Must
6080 * be 3x3.
6081 * @param initialGg initial gyroscope G-dependent cross
6082 * biases introduced on the gyroscope by
6083 * the specific forces sensed by the
6084 * accelerometer. Must be 3x3.
6085 * @param method robust estimator method.
6086 * @return a robust gyroscope calibrator.
6087 * @throws IllegalArgumentException if any of the provided values does
6088 * not have proper size or if either
6089 * turntable rotation rate or
6090 * time interval is zero or negative.
6091 */
6092 public static RobustTurntableGyroscopeCalibrator create(
6093 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6094 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
6095 final Matrix initialGg, final RobustEstimatorMethod method) {
6096 return switch (method) {
6097 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6098 measurements, initialBias, initialMg, initialGg);
6099 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6100 measurements, initialBias, initialMg, initialGg);
6101 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6102 measurements, initialBias, initialMg, initialGg);
6103 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6104 measurements, initialBias, initialMg, initialGg);
6105 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6106 measurements, initialBias, initialMg, initialGg);
6107 };
6108 }
6109
6110 /**
6111 * Creates a robust gyroscope calibrator.
6112 *
6113 * @param position position where body kinematics measures
6114 * have been taken.
6115 * @param turntableRotationRate constant rotation rate at which the
6116 * turntable is spinning. Must be
6117 * expressed in radians per second (rad/s).
6118 * @param timeInterval time interval between measurements being
6119 * captured expressed in seconds (s).
6120 * @param measurements collection of body kinematics
6121 * measurements with standard deviations
6122 * taken at the same position with zero
6123 * velocity and unknown different
6124 * orientations.
6125 * @param initialBias initial gyroscope bias to be used to
6126 * find a solution. This must be 3x1 and
6127 * is expressed in radians per second
6128 * (rad/s).
6129 * @param initialMg initial gyroscope scale factors and
6130 * cross coupling errors matrix. Must
6131 * be 3x3.
6132 * @param initialGg initial gyroscope G-dependent cross
6133 * biases introduced on the gyroscope by
6134 * the specific forces sensed by the
6135 * accelerometer. Must be 3x3.
6136 * @param listener listener to handle events raised
6137 * by this calibrator.
6138 * @param method robust estimator method.
6139 * @return a robust gyroscope calibrator.
6140 * @throws IllegalArgumentException if any of the provided values does
6141 * not have proper size or if either
6142 * turntable rotation rate or
6143 * time interval is zero or negative.
6144 */
6145 public static RobustTurntableGyroscopeCalibrator create(
6146 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6147 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
6148 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
6149 final RobustEstimatorMethod method) {
6150 return switch (method) {
6151 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6152 measurements, initialBias, initialMg, initialGg, listener);
6153 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6154 measurements, initialBias, initialMg, initialGg, listener);
6155 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6156 measurements, initialBias, initialMg, initialGg, listener);
6157 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6158 measurements, initialBias, initialMg, initialGg, listener);
6159 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6160 measurements, initialBias, initialMg, initialGg, listener);
6161 };
6162 }
6163
6164 /**
6165 * Creates a robust gyroscope calibrator.
6166 *
6167 * @param position position where body kinematics measures
6168 * have been taken.
6169 * @param turntableRotationRate constant rotation rate at which the
6170 * turntable is spinning. Must be
6171 * expressed in radians per second (rad/s).
6172 * @param timeInterval time interval between measurements being
6173 * captured expressed in seconds (s).
6174 * @param measurements collection of body kinematics
6175 * measurements with standard deviations
6176 * taken at the same position with zero
6177 * velocity and unknown different
6178 * orientations.
6179 * @param initialBias initial gyroscope bias to be used to
6180 * find a solution. This must have
6181 * length 3 and is expressed in radians
6182 * per second (rad/s).
6183 * @param initialMg initial gyroscope scale factors and
6184 * cross coupling errors matrix. Must
6185 * be 3x3.
6186 * @param initialGg initial gyroscope G-dependent cross
6187 * biases introduced on the gyroscope by
6188 * the specific forces sensed by the
6189 * accelerometer. Must be 3x3.
6190 * @param method robust estimator method.
6191 * @return a robust gyroscope calibrator.
6192 * @throws IllegalArgumentException if any of the provided values does
6193 * not have proper size or if either
6194 * turntable rotation rate or
6195 * time interval is zero or negative.
6196 */
6197 public static RobustTurntableGyroscopeCalibrator create(
6198 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6199 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
6200 final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
6201 return switch (method) {
6202 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6203 measurements, initialBias, initialMg, initialGg);
6204 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6205 measurements, initialBias, initialMg, initialGg);
6206 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6207 measurements, initialBias, initialMg, initialGg);
6208 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6209 measurements, initialBias, initialMg, initialGg);
6210 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6211 measurements, initialBias, initialMg, initialGg);
6212 };
6213 }
6214
6215 /**
6216 * Creates a robust gyroscope calibrator.
6217 *
6218 * @param position position where body kinematics measures
6219 * have been taken.
6220 * @param turntableRotationRate constant rotation rate at which the
6221 * turntable is spinning. Must be
6222 * expressed in radians per second (rad/s).
6223 * @param timeInterval time interval between measurements being
6224 * captured expressed in seconds (s).
6225 * @param measurements collection of body kinematics
6226 * measurements with standard deviations
6227 * taken at the same position with zero
6228 * velocity and unknown different
6229 * orientations.
6230 * @param initialBias initial gyroscope bias to be used to
6231 * find a solution. This must have
6232 * length 3 and is expressed in radians
6233 * per second (rad/s).
6234 * @param initialMg initial gyroscope scale factors and
6235 * cross coupling errors matrix. Must
6236 * be 3x3.
6237 * @param initialGg initial gyroscope G-dependent cross
6238 * biases introduced on the gyroscope by
6239 * the specific forces sensed by the
6240 * accelerometer. Must be 3x3.
6241 * @param listener listener to handle events raised
6242 * by this calibrator.
6243 * @param method robust estimator method.
6244 * @return a robust gyroscope calibrator.
6245 * @throws IllegalArgumentException if any of the provided values does
6246 * not have proper size or if either
6247 * turntable rotation rate or
6248 * time interval is zero or negative.
6249 */
6250 public static RobustTurntableGyroscopeCalibrator create(
6251 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6252 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
6253 final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
6254 final RobustEstimatorMethod method) {
6255 return switch (method) {
6256 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6257 measurements, initialBias, initialMg, initialGg, listener);
6258 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6259 measurements, initialBias, initialMg, initialGg, listener);
6260 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6261 measurements, initialBias, initialMg, initialGg, listener);
6262 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6263 measurements, initialBias, initialMg, initialGg, listener);
6264 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6265 measurements, initialBias, initialMg, initialGg, listener);
6266 };
6267 }
6268
6269 /**
6270 * Creates a robust gyroscope calibrator.
6271 *
6272 * @param position position where body kinematics measures
6273 * have been taken.
6274 * @param turntableRotationRate constant rotation rate at which the
6275 * turntable is spinning. Must be
6276 * expressed in radians per second (rad/s).
6277 * @param timeInterval time interval between measurements being
6278 * captured expressed in seconds (s).
6279 * @param measurements collection of body kinematics
6280 * measurements with standard deviations
6281 * taken at the same position with zero
6282 * velocity and unknown different
6283 * orientations.
6284 * @param initialBias initial gyroscope bias to be used to
6285 * find a solution. This must have length
6286 * 3 and is expressed in radians per
6287 * second (rad/s).
6288 * @param initialMg initial gyroscope scale factors and
6289 * cross coupling errors matrix. Must
6290 * be 3x3.
6291 * @param initialGg initial gyroscope G-dependent cross
6292 * biases introduced on the gyroscope by
6293 * the specific forces sensed by the
6294 * accelerometer. Must be 3x3.
6295 * @param accelerometerBias known accelerometer bias. This must
6296 * have length 3 and is expressed in
6297 * meters per squared second
6298 * (m/s^2).
6299 * @param accelerometerMa known accelerometer scale factors and
6300 * cross coupling matrix. Must be 3x3.
6301 * @param method robust estimator method.
6302 * @return a robust gyroscope calibrator.
6303 * @throws IllegalArgumentException if any of the provided values does
6304 * not have proper size or if either
6305 * turntable rotation rate or
6306 * time interval is zero or negative.
6307 */
6308 public static RobustTurntableGyroscopeCalibrator create(
6309 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6310 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
6311 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
6312 final Matrix accelerometerMa, final RobustEstimatorMethod method) {
6313 return switch (method) {
6314 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6315 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
6316 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6317 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
6318 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6319 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
6320 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6321 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
6322 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6323 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
6324 };
6325 }
6326
6327 /**
6328 * Creates a robust gyroscope calibrator.
6329 *
6330 * @param position position where body kinematics measures
6331 * have been taken.
6332 * @param turntableRotationRate constant rotation rate at which the
6333 * turntable is spinning. Must be
6334 * expressed in radians per second (rad/s).
6335 * @param timeInterval time interval between measurements being
6336 * captured expressed in seconds (s).
6337 * @param measurements collection of body kinematics
6338 * measurements with standard deviations
6339 * taken at the same position with zero
6340 * velocity and unknown different
6341 * orientations.
6342 * @param initialBias initial gyroscope bias to be used to
6343 * find a solution. This must have length
6344 * 3 and is expressed in radians per
6345 * second (rad/s).
6346 * @param initialMg initial gyroscope scale factors and
6347 * cross coupling errors matrix. Must
6348 * be 3x3.
6349 * @param initialGg initial gyroscope G-dependent cross
6350 * biases introduced on the gyroscope by
6351 * the specific forces sensed by the
6352 * accelerometer. Must be 3x3.
6353 * @param accelerometerBias known accelerometer bias. This must
6354 * have length 3 and is expressed in
6355 * meters per squared second
6356 * (m/s^2).
6357 * @param accelerometerMa known accelerometer scale factors and
6358 * cross coupling matrix. Must be 3x3.
6359 * @param listener listener to handle events raised
6360 * by this calibrator.
6361 * @param method robust estimator method.
6362 * @return a robust gyroscope calibrator.
6363 * @throws IllegalArgumentException if any of the provided values does
6364 * not have proper size or if either
6365 * turntable rotation rate or
6366 * time interval is zero or negative.
6367 */
6368 public static RobustTurntableGyroscopeCalibrator create(
6369 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6370 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
6371 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
6372 final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
6373 final RobustEstimatorMethod method) {
6374 return switch (method) {
6375 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6376 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
6377 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6378 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
6379 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6380 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
6381 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6382 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
6383 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6384 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
6385 };
6386 }
6387
6388 /**
6389 * Creates a robust gyroscope calibrator.
6390 *
6391 * @param position position where body kinematics measures
6392 * have been taken.
6393 * @param turntableRotationRate constant rotation rate at which the
6394 * turntable is spinning. Must be
6395 * expressed in radians per second (rad/s).
6396 * @param timeInterval time interval between measurements being
6397 * captured expressed in seconds (s).
6398 * @param measurements collection of body kinematics
6399 * measurements with standard deviations
6400 * taken at the same position with zero
6401 * velocity and unknown different
6402 * orientations.
6403 * @param initialBias initial gyroscope bias to be used to
6404 * find a solution. This must be 3x1 and
6405 * is expressed in radians per second
6406 * (rad/s).
6407 * @param initialMg initial gyroscope scale factors and
6408 * cross coupling errors matrix. Must
6409 * be 3x3.
6410 * @param initialGg initial gyroscope G-dependent cross
6411 * biases introduced on the gyroscope by
6412 * the specific forces sensed by the
6413 * accelerometer. Must be 3x3.
6414 * @param accelerometerBias known accelerometer bias. This must
6415 * have length 3 and is expressed in
6416 * meters per squared second
6417 * (m/s^2).
6418 * @param accelerometerMa known accelerometer scale factors and
6419 * cross coupling matrix. Must be 3x3.
6420 * @param method robust estimator method.
6421 * @return a robust gyroscope calibrator.
6422 * @throws IllegalArgumentException if any of the provided values does
6423 * not have proper size or if either
6424 * turntable rotation rate or
6425 * time interval is zero or negative.
6426 */
6427 public static RobustTurntableGyroscopeCalibrator create(
6428 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6429 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
6430 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
6431 final RobustEstimatorMethod method) {
6432 return switch (method) {
6433 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6434 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
6435 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6436 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
6437 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6438 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
6439 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6440 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
6441 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6442 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
6443 };
6444 }
6445
6446 /**
6447 * Creates a robust gyroscope calibrator.
6448 *
6449 * @param position position where body kinematics measures
6450 * have been taken.
6451 * @param turntableRotationRate constant rotation rate at which the
6452 * turntable is spinning. Must be
6453 * expressed in radians per second (rad/s).
6454 * @param timeInterval time interval between measurements being
6455 * captured expressed in seconds (s).
6456 * @param measurements collection of body kinematics
6457 * measurements with standard deviations
6458 * taken at the same position with zero
6459 * velocity and unknown different
6460 * orientations.
6461 * @param initialBias initial gyroscope bias to be used to
6462 * find a solution. This must be 3x1 and
6463 * is expressed in radians per second
6464 * (rad/s).
6465 * @param initialMg initial gyroscope scale factors and
6466 * cross coupling errors matrix. Must
6467 * be 3x3.
6468 * @param initialGg initial gyroscope G-dependent cross
6469 * biases introduced on the gyroscope by
6470 * the specific forces sensed by the
6471 * accelerometer. Must be 3x3.
6472 * @param accelerometerBias known accelerometer bias. This must
6473 * have length 3 and is expressed in
6474 * meters per squared second
6475 * (m/s^2).
6476 * @param accelerometerMa known accelerometer scale factors and
6477 * cross coupling matrix. Must be 3x3.
6478 * @param listener listener to handle events raised
6479 * by this calibrator.
6480 * @param method robust estimator method.
6481 * @return a robust gyroscope calibrator.
6482 * @throws IllegalArgumentException if any of the provided values does
6483 * not have proper size or if either
6484 * turntable rotation rate or
6485 * time interval is zero or negative.
6486 */
6487 public static RobustTurntableGyroscopeCalibrator create(
6488 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6489 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
6490 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
6491 final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
6492 return switch (method) {
6493 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6494 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
6495 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6496 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
6497 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6498 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
6499 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6500 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
6501 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6502 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
6503 };
6504 }
6505
6506 /**
6507 * Creates a robust gyroscope calibrator.
6508 *
6509 * @param position position where body kinematics
6510 * measures have been taken.
6511 * @param turntableRotationRate constant rotation rate at which
6512 * the turntable is spinning. Must
6513 * be expressed in radians per
6514 * second (rad/s).
6515 * @param timeInterval time interval between measurements
6516 * being captured expressed in
6517 * seconds (s).
6518 * @param measurements collection of body kinematics
6519 * measurements with standard
6520 * deviations taken at the same
6521 * position with zero velocity
6522 * and unknown different
6523 * orientations.
6524 * @param commonAxisUsed indicates whether z-axis is
6525 * assumed to be common for
6526 * accelerometer and gyroscope.
6527 * @param estimateGDependentCrossBiases true if G-dependent cross biases
6528 * will be estimated, false
6529 * otherwise.
6530 * @param initialBias initial gyroscope bias to be
6531 * used to find a solution. This
6532 * must be 3x1 and is expressed in
6533 * radians per second (rad/s).
6534 * @param initialMg initial gyroscope scale factors
6535 * and cross coupling errors matrix.
6536 * Must be 3x3.
6537 * @param initialGg initial gyroscope G-dependent
6538 * cross biases introduced on the
6539 * gyroscope by the specific
6540 * forces sensed by the
6541 * accelerometer. Must be 3x3.
6542 * @param method robust estimator method.
6543 * @return a robust gyroscope calibrator.
6544 * @throws IllegalArgumentException if any of the provided values does
6545 * not have proper size or if either
6546 * turntable rotation rate or
6547 * time interval is zero or negative.
6548 */
6549 public static RobustTurntableGyroscopeCalibrator create(
6550 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6551 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
6552 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
6553 final Matrix initialGg, final RobustEstimatorMethod method) {
6554 return switch (method) {
6555 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6556 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
6557 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6558 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
6559 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6560 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
6561 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6562 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
6563 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6564 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
6565 };
6566 }
6567
6568 /**
6569 * Creates a robust gyroscope calibrator.
6570 *
6571 * @param position position where body kinematics
6572 * measures have been taken.
6573 * @param turntableRotationRate constant rotation rate at which
6574 * the turntable is spinning. Must
6575 * be expressed in radians per
6576 * second (rad/s).
6577 * @param timeInterval time interval between measurements
6578 * being captured expressed in
6579 * seconds (s).
6580 * @param measurements collection of body kinematics
6581 * measurements with standard
6582 * deviations taken at the same
6583 * position with zero velocity
6584 * and unknown different
6585 * orientations.
6586 * @param commonAxisUsed indicates whether z-axis is
6587 * assumed to be common for
6588 * accelerometer and gyroscope.
6589 * @param estimateGDependentCrossBiases true if G-dependent cross biases
6590 * will be estimated, false
6591 * otherwise.
6592 * @param initialBias initial gyroscope bias to be
6593 * used to find a solution. This
6594 * must be 3x1 and is expressed in
6595 * radians per second (rad/s).
6596 * @param initialMg initial gyroscope scale factors
6597 * and cross coupling errors matrix.
6598 * Must be 3x3.
6599 * @param initialGg initial gyroscope G-dependent
6600 * cross biases introduced on the
6601 * gyroscope by the specific
6602 * forces sensed by the
6603 * accelerometer. Must be 3x3.
6604 * @param listener listener to handle events raised
6605 * by this calibrator.
6606 * @param method robust estimator method.
6607 * @return a robust gyroscope calibrator.
6608 * @throws IllegalArgumentException if any of the provided values does
6609 * not have proper size or if either
6610 * turntable rotation rate or
6611 * time interval is zero or negative.
6612 */
6613 public static RobustTurntableGyroscopeCalibrator create(
6614 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6615 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
6616 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
6617 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
6618 final RobustEstimatorMethod method) {
6619 return switch (method) {
6620 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6621 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6622 listener);
6623 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6624 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6625 listener);
6626 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6627 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6628 listener);
6629 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6630 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6631 listener);
6632 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6633 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6634 listener);
6635 };
6636 }
6637
6638 /**
6639 * Creates a robust gyroscope calibrator.
6640 *
6641 * @param position position where body kinematics
6642 * measures have been taken.
6643 * @param turntableRotationRate constant rotation rate at which
6644 * the turntable is spinning. Must
6645 * be expressed in radians per
6646 * second (rad/s).
6647 * @param timeInterval time interval between measurements
6648 * being captured expressed in
6649 * seconds (s).
6650 * @param measurements collection of body kinematics
6651 * measurements with standard
6652 * deviations taken at the same
6653 * position with zero velocity
6654 * and unknown different
6655 * orientations.
6656 * @param commonAxisUsed indicates whether z-axis is
6657 * assumed to be common for
6658 * accelerometer and gyroscope.
6659 * @param estimateGDependentCrossBiases true if G-dependent cross biases
6660 * will be estimated, false
6661 * otherwise.
6662 * @param initialBias initial gyroscope bias to be
6663 * used to find a solution. This
6664 * must have length 3 and is
6665 * expressed in radians per second
6666 * (rad/s).
6667 * @param initialMg initial gyroscope scale factors
6668 * and cross coupling errors matrix.
6669 * Must be 3x3.
6670 * @param initialGg initial gyroscope G-dependent
6671 * cross biases introduced on the
6672 * gyroscope by the specific forces
6673 * sensed by the accelerometer.
6674 * Must be 3x3.
6675 * @param method robust estimator method.
6676 * @return a robust gyroscope calibrator.
6677 * @throws IllegalArgumentException if any of the provided values does
6678 * not have proper size or if either
6679 * turntable rotation rate or
6680 * time interval is zero or negative.
6681 */
6682 public static RobustTurntableGyroscopeCalibrator create(
6683 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6684 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
6685 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
6686 final Matrix initialGg, final RobustEstimatorMethod method) {
6687 return switch (method) {
6688 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6689 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
6690 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6691 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
6692 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6693 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
6694 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6695 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
6696 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6697 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
6698 };
6699 }
6700
6701 /**
6702 * Creates a robust gyroscope calibrator.
6703 *
6704 * @param position position where body kinematics
6705 * measures have been taken.
6706 * @param turntableRotationRate constant rotation rate at which
6707 * the turntable is spinning. Must
6708 * be expressed in radians per
6709 * second (rad/s).
6710 * @param timeInterval time interval between measurements
6711 * being captured expressed in
6712 * seconds (s).
6713 * @param measurements collection of body kinematics
6714 * measurements with standard
6715 * deviations taken at the same
6716 * position with zero velocity
6717 * and unknown different
6718 * orientations.
6719 * @param commonAxisUsed indicates whether z-axis is
6720 * assumed to be common for
6721 * accelerometer and gyroscope.
6722 * @param estimateGDependentCrossBiases true if G-dependent cross biases
6723 * will be estimated, false
6724 * otherwise.
6725 * @param initialBias initial gyroscope bias to be
6726 * used to find a solution. This
6727 * must have length 3 and is
6728 * expressed in radians per second
6729 * (rad/s).
6730 * @param initialMg initial gyroscope scale factors
6731 * and cross coupling errors matrix.
6732 * Must be 3x3.
6733 * @param initialGg initial gyroscope G-dependent
6734 * cross biases introduced on the
6735 * gyroscope by the specific forces
6736 * sensed by the accelerometer.
6737 * Must be 3x3.
6738 * @param listener listener to handle events raised
6739 * by this calibrator.
6740 * @param method robust estimator method.
6741 * @return a robust gyroscope calibrator.
6742 * @throws IllegalArgumentException if any of the provided values does
6743 * not have proper size or if either
6744 * turntable rotation rate or
6745 * time interval is zero or negative.
6746 */
6747 public static RobustTurntableGyroscopeCalibrator create(
6748 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6749 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
6750 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
6751 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
6752 final RobustEstimatorMethod method) {
6753 return switch (method) {
6754 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6755 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6756 listener);
6757 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6758 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6759 listener);
6760 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6761 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6762 listener);
6763 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6764 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6765 listener);
6766 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6767 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6768 listener);
6769 };
6770 }
6771
6772 /**
6773 * Creates a robust gyroscope calibrator.
6774 *
6775 * @param position position where body kinematics
6776 * measures have been taken.
6777 * @param turntableRotationRate constant rotation rate at which
6778 * the turntable is spinning. Must
6779 * be expressed in radians per
6780 * second (rad/s).
6781 * @param timeInterval time interval between measurements
6782 * being captured expressed in
6783 * seconds (s).
6784 * @param measurements collection of body kinematics
6785 * measurements with standard
6786 * deviations taken at the same
6787 * position with zero velocity
6788 * and unknown different
6789 * orientations.
6790 * @param commonAxisUsed indicates whether z-axis is
6791 * assumed to be common for
6792 * accelerometer and gyroscope.
6793 * @param estimateGDependentCrossBiases true if G-dependent cross
6794 * biases will be estimated,
6795 * false otherwise.
6796 * @param initialBias initial gyroscope bias to be
6797 * used to find a solution. This
6798 * must have length 3 and is
6799 * expressed in radians per second
6800 * (rad/s).
6801 * @param initialMg initial gyroscope scale factors
6802 * and cross coupling errors
6803 * matrix. Must be 3x3.
6804 * @param initialGg initial gyroscope G-dependent
6805 * cross biases introduced on the
6806 * gyroscope by the specific forces
6807 * sensed by the accelerometer.
6808 * Must be 3x3.
6809 * @param accelerometerBias known accelerometer bias. This
6810 * must have length 3 and is
6811 * expressed in meters per squared
6812 * second (m/s^2).
6813 * @param accelerometerMa known accelerometer scale factors
6814 * and cross coupling matrix. Must
6815 * be 3x3.
6816 * @param method robust estimator method.
6817 * @return a robust gyroscope calibrator.
6818 * @throws IllegalArgumentException if any of the provided values does
6819 * not have proper size or if either
6820 * turntable rotation rate or
6821 * time interval is zero or negative.
6822 */
6823 public static RobustTurntableGyroscopeCalibrator create(
6824 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6825 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
6826 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
6827 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
6828 final RobustEstimatorMethod method) {
6829 return switch (method) {
6830 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6831 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6832 accelerometerBias, accelerometerMa);
6833 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6834 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6835 accelerometerBias, accelerometerMa);
6836 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6837 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6838 accelerometerBias, accelerometerMa);
6839 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6840 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6841 accelerometerBias, accelerometerMa);
6842 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6843 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6844 accelerometerBias, accelerometerMa);
6845 };
6846 }
6847
6848 /**
6849 * Creates a robust gyroscope calibrator.
6850 *
6851 * @param position position where body kinematics
6852 * measures have been taken.
6853 * @param turntableRotationRate constant rotation rate at which
6854 * the turntable is spinning. Must
6855 * be expressed in radians per
6856 * second (rad/s).
6857 * @param timeInterval time interval between measurements
6858 * being captured expressed in
6859 * seconds (s).
6860 * @param measurements collection of body kinematics
6861 * measurements with standard
6862 * deviations taken at the same
6863 * position with zero velocity
6864 * and unknown different
6865 * orientations.
6866 * @param commonAxisUsed indicates whether z-axis is
6867 * assumed to be common for
6868 * accelerometer and gyroscope.
6869 * @param estimateGDependentCrossBiases true if G-dependent cross
6870 * biases will be estimated,
6871 * false otherwise.
6872 * @param initialBias initial gyroscope bias to be
6873 * used to find a solution. This
6874 * must have length 3 and is
6875 * expressed in radians per second
6876 * (rad/s).
6877 * @param initialMg initial gyroscope scale factors
6878 * and cross coupling errors
6879 * matrix. Must be 3x3.
6880 * @param initialGg initial gyroscope G-dependent
6881 * cross biases introduced on the
6882 * gyroscope by the specific forces
6883 * sensed by the accelerometer.
6884 * Must be 3x3.
6885 * @param accelerometerBias known accelerometer bias. This
6886 * must have length 3 and is
6887 * expressed in meters per squared
6888 * second (m/s^2).
6889 * @param accelerometerMa known accelerometer scale factors
6890 * and cross coupling matrix. Must
6891 * be 3x3.
6892 * @param listener listener to handle events raised
6893 * by this calibrator.
6894 * @param method robust estimator method.
6895 * @return a robust gyroscope calibrator.
6896 * @throws IllegalArgumentException if any of the provided values does
6897 * not have proper size or if either
6898 * turntable rotation rate or
6899 * time interval is zero or negative.
6900 */
6901 public static RobustTurntableGyroscopeCalibrator create(
6902 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6903 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
6904 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
6905 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
6906 final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
6907 return switch (method) {
6908 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6909 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6910 accelerometerBias, accelerometerMa, listener);
6911 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6912 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6913 accelerometerBias, accelerometerMa, listener);
6914 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6915 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6916 accelerometerBias, accelerometerMa, listener);
6917 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6918 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6919 accelerometerBias, accelerometerMa, listener);
6920 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6921 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6922 accelerometerBias, accelerometerMa, listener);
6923 };
6924 }
6925
6926 /**
6927 * Creates a robust gyroscope calibrator.
6928 *
6929 * @param position position where body kinematics
6930 * measures have been taken.
6931 * @param turntableRotationRate constant rotation rate at which
6932 * the turntable is spinning. Must
6933 * be expressed in radians per
6934 * second (rad/s).
6935 * @param timeInterval time interval between measurements
6936 * being captured expressed in
6937 * seconds (s).
6938 * @param measurements collection of body kinematics
6939 * measurements with standard
6940 * deviations taken at the same
6941 * position with zero velocity and
6942 * unknown different orientations.
6943 * @param commonAxisUsed indicates whether z-axis is
6944 * assumed to be common for
6945 * accelerometer and gyroscope.
6946 * @param estimateGDependentCrossBiases true if G-dependent cross biases
6947 * will be estimated, false
6948 * otherwise.
6949 * @param initialBias initial gyroscope bias to be
6950 * used to find a solution. This
6951 * must be 3x1 and is expressed in
6952 * radians per second (rad/s).
6953 * @param initialMg initial gyroscope scale factors
6954 * and cross coupling errors matrix.
6955 * Must be 3x3.
6956 * @param initialGg initial gyroscope G-dependent
6957 * cross biases introduced on the
6958 * gyroscope by the specific forces
6959 * sensed by the accelerometer. Must
6960 * be 3x3.
6961 * @param accelerometerBias known accelerometer bias. This
6962 * must have length 3 and is
6963 * expressed in meters per squared
6964 * second (m/s^2).
6965 * @param accelerometerMa known accelerometer scale factors
6966 * and cross coupling matrix. Must
6967 * be 3x3.
6968 * @param method robust estimator method.
6969 * @return a robust gyroscope calibrator.
6970 * @throws IllegalArgumentException if any of the provided values does
6971 * not have proper size or if either
6972 * turntable rotation rate or
6973 * time interval is zero or negative.
6974 */
6975 public static RobustTurntableGyroscopeCalibrator create(
6976 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
6977 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
6978 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
6979 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
6980 final RobustEstimatorMethod method) {
6981 return switch (method) {
6982 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6983 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6984 accelerometerBias, accelerometerMa);
6985 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6986 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6987 accelerometerBias, accelerometerMa);
6988 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6989 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6990 accelerometerBias, accelerometerMa);
6991 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6992 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6993 accelerometerBias, accelerometerMa);
6994 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
6995 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
6996 accelerometerBias, accelerometerMa);
6997 };
6998 }
6999
7000 /**
7001 * Creates a robust gyroscope calibrator.
7002 *
7003 * @param position position where body kinematics
7004 * measures have been taken.
7005 * @param turntableRotationRate constant rotation rate at which
7006 * the turntable is spinning. Must
7007 * be expressed in radians per
7008 * second (rad/s).
7009 * @param timeInterval time interval between measurements
7010 * being captured expressed in
7011 * seconds (s).
7012 * @param measurements collection of body kinematics
7013 * measurements with standard
7014 * deviations taken at the same
7015 * position with zero velocity and
7016 * unknown different orientations.
7017 * @param commonAxisUsed indicates whether z-axis is
7018 * assumed to be common for
7019 * accelerometer and gyroscope.
7020 * @param estimateGDependentCrossBiases true if G-dependent cross biases
7021 * will be estimated, false
7022 * otherwise.
7023 * @param initialBias initial gyroscope bias to be
7024 * used to find a solution. This
7025 * must be 3x1 and is expressed in
7026 * radians per second (rad/s).
7027 * @param initialMg initial gyroscope scale factors
7028 * and cross coupling errors matrix.
7029 * Must be 3x3.
7030 * @param initialGg initial gyroscope G-dependent
7031 * cross biases introduced on the
7032 * gyroscope by the specific forces
7033 * sensed by the accelerometer. Must
7034 * be 3x3.
7035 * @param accelerometerBias known accelerometer bias. This
7036 * must have length 3 and is
7037 * expressed in meters per squared
7038 * second (m/s^2).
7039 * @param accelerometerMa known accelerometer scale factors
7040 * and cross coupling matrix. Must
7041 * be 3x3.
7042 * @param listener listener to handle events raised
7043 * by this calibrator.
7044 * @param method robust estimator method.
7045 * @return a robust gyroscope calibrator.
7046 * @throws IllegalArgumentException if any of the provided values does
7047 * not have proper size or if either
7048 * turntable rotation rate or
7049 * time interval is zero or negative.
7050 */
7051 public static RobustTurntableGyroscopeCalibrator create(
7052 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
7053 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
7054 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
7055 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
7056 final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
7057 return switch (method) {
7058 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7059 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7060 accelerometerBias, accelerometerMa, listener);
7061 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7062 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7063 accelerometerBias, accelerometerMa, listener);
7064 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7065 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7066 accelerometerBias, accelerometerMa, listener);
7067 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7068 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7069 accelerometerBias, accelerometerMa, listener);
7070 default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7071 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7072 accelerometerBias, accelerometerMa, listener);
7073 };
7074 }
7075
7076 /**
7077 * Creates a robust gyroscope calibrator.
7078 *
7079 * @param qualityScores quality scores corresponding to each provided
7080 * measurement. The larger the score value the better
7081 * the quality of the sample.
7082 * @param method robust estimator method.
7083 * @return a robust gyroscope calibrator.
7084 * @throws IllegalArgumentException if provided quality scores length
7085 * is smaller than 10 samples.
7086 */
7087 public static RobustTurntableGyroscopeCalibrator create(
7088 final double[] qualityScores, final RobustEstimatorMethod method) {
7089 return switch (method) {
7090 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator();
7091 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator();
7092 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator();
7093 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores);
7094 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores);
7095 };
7096 }
7097
7098 /**
7099 * Creates a robust gyroscope calibrator.
7100 *
7101 * @param qualityScores quality scores corresponding to each provided
7102 * measurement. The larger the score value the better
7103 * the quality of the sample.*
7104 * @param position position where body kinematics measures
7105 * have been taken.
7106 * @param turntableRotationRate constant rotation rate at which the
7107 * turntable is spinning. Must be
7108 * expressed in radians per second (rad/s).
7109 * @param timeInterval time interval between measurements being
7110 * captured expressed in seconds (s).
7111 * @param measurements collection of body kinematics
7112 * measurements with standard deviations
7113 * taken at the same position with zero
7114 * velocity and unknown different
7115 * orientations.
7116 * @param initialBias initial gyroscope bias to be used to
7117 * find a solution. This must be 3x1 and
7118 * is expressed in radians per second
7119 * (rad/s).
7120 * @param initialMg initial gyroscope scale factors and
7121 * cross coupling errors matrix. Must
7122 * be 3x3.
7123 * @param initialGg initial gyroscope G-dependent cross
7124 * biases introduced on the gyroscope by
7125 * the specific forces sensed by the
7126 * accelerometer. Must be 3x3.
7127 * @param method robust estimator method.
7128 * @return a robust gyroscope calibrator.
7129 * @throws IllegalArgumentException if any of the provided values does
7130 * not have proper size, if either
7131 * turntable rotation rate or
7132 * time interval is zero or negative or
7133 * if provided quality scores length is
7134 * smaller than 10 samples.
7135 */
7136 public static RobustTurntableGyroscopeCalibrator create(
7137 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
7138 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
7139 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
7140 final RobustEstimatorMethod method) {
7141 return switch (method) {
7142 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7143 measurements, initialBias, initialMg, initialGg);
7144 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7145 measurements, initialBias, initialMg, initialGg);
7146 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7147 measurements, initialBias, initialMg, initialGg);
7148 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7149 timeInterval, measurements, initialBias, initialMg, initialGg);
7150 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7151 timeInterval, measurements, initialBias, initialMg, initialGg);
7152 };
7153 }
7154
7155 /**
7156 * Creates a robust gyroscope calibrator.
7157 *
7158 * @param qualityScores quality scores corresponding to each provided
7159 * measurement. The larger the score value the better
7160 * the quality of the sample.
7161 * @param position position where body kinematics measures
7162 * have been taken.
7163 * @param turntableRotationRate constant rotation rate at which the
7164 * turntable is spinning. Must be
7165 * expressed in radians per second (rad/s).
7166 * @param timeInterval time interval between measurements being
7167 * captured expressed in seconds (s).
7168 * @param measurements collection of body kinematics
7169 * measurements with standard deviations
7170 * taken at the same position with zero
7171 * velocity and unknown different
7172 * orientations.
7173 * @param initialBias initial gyroscope bias to be used to
7174 * find a solution. This must be 3x1 and
7175 * is expressed in radians per second
7176 * (rad/s).
7177 * @param initialMg initial gyroscope scale factors and
7178 * cross coupling errors matrix. Must
7179 * be 3x3.
7180 * @param initialGg initial gyroscope G-dependent cross
7181 * biases introduced on the gyroscope by
7182 * the specific forces sensed by the
7183 * accelerometer. Must be 3x3.
7184 * @param listener listener to handle events raised by this
7185 * calibrator.
7186 * @param method robust estimator method.
7187 * @return a robust gyroscope calibrator.
7188 * @throws IllegalArgumentException if any of the provided values does
7189 * not have proper size or if either
7190 * turntable rotation rate or
7191 * time interval is zero or negative.
7192 */
7193 public static RobustTurntableGyroscopeCalibrator create(
7194 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
7195 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
7196 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
7197 final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
7198 return switch (method) {
7199 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7200 measurements, initialBias, initialMg, initialGg, listener);
7201 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7202 measurements, initialBias, initialMg, initialGg, listener);
7203 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7204 measurements, initialBias, initialMg, initialGg, listener);
7205 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7206 timeInterval, measurements, initialBias, initialMg, initialGg, listener);
7207 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7208 timeInterval, measurements, initialBias, initialMg, initialGg, listener);
7209 };
7210 }
7211
7212 /**
7213 * Creates a robust gyroscope calibrator.
7214 *
7215 * @param qualityScores quality scores corresponding to each provided
7216 * measurement. The larger the score value the better
7217 * the quality of the sample.
7218 * @param position position where body kinematics measures
7219 * have been taken.
7220 * @param turntableRotationRate constant rotation rate at which the
7221 * turntable is spinning. Must be
7222 * expressed in radians per second (rad/s).
7223 * @param timeInterval time interval between measurements being
7224 * captured expressed in seconds (s).
7225 * @param measurements collection of body kinematics
7226 * measurements with standard deviations
7227 * taken at the same position with zero
7228 * velocity and unknown different
7229 * orientations.
7230 * @param initialBias initial gyroscope bias to be used to
7231 * find a solution. This must have
7232 * length 3 and is expressed in radians
7233 * per second (rad/s).
7234 * @param initialMg initial gyroscope scale factors and
7235 * cross coupling errors matrix. Must
7236 * be 3x3.
7237 * @param initialGg initial gyroscope G-dependent cross
7238 * biases introduced on the gyroscope by
7239 * the specific forces sensed by the
7240 * accelerometer. Must be 3x3.
7241 * @param method robust estimator method.
7242 * @return a robust gyroscope calibrator.
7243 * @throws IllegalArgumentException if any of the provided values does
7244 * not have proper size, if either
7245 * turntable rotation rate or
7246 * time interval is zero or negative or
7247 * if provided quality scores length is
7248 * smaller than 10 samples.
7249 */
7250 public static RobustTurntableGyroscopeCalibrator create(
7251 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
7252 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
7253 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
7254 final RobustEstimatorMethod method) {
7255 return switch (method) {
7256 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7257 measurements, initialBias, initialMg, initialGg);
7258 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7259 measurements, initialBias, initialMg, initialGg);
7260 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7261 measurements, initialBias, initialMg, initialGg);
7262 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7263 timeInterval, measurements, initialBias, initialMg, initialGg);
7264 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7265 timeInterval, measurements, initialBias, initialMg, initialGg);
7266 };
7267 }
7268
7269 /**
7270 * Creates a robust gyroscope calibrator.
7271 *
7272 * @param qualityScores quality scores corresponding to each provided
7273 * measurement. The larger the score value the better
7274 * the quality of the sample.
7275 * @param position position where body kinematics measures
7276 * have been taken.
7277 * @param turntableRotationRate constant rotation rate at which the
7278 * turntable is spinning. Must be
7279 * expressed in radians per second (rad/s).
7280 * @param timeInterval time interval between measurements being
7281 * captured expressed in seconds (s).
7282 * @param measurements collection of body kinematics
7283 * measurements with standard deviations
7284 * taken at the same position with zero
7285 * velocity and unknown different
7286 * orientations.
7287 * @param initialBias initial gyroscope bias to be used to
7288 * find a solution. This must have
7289 * length 3 and is expressed in radians
7290 * per second (rad/s).
7291 * @param initialMg initial gyroscope scale factors and
7292 * cross coupling errors matrix. Must
7293 * be 3x3.
7294 * @param initialGg initial gyroscope G-dependent cross
7295 * biases introduced on the gyroscope by
7296 * the specific forces sensed by the
7297 * accelerometer. Must be 3x3.
7298 * @param listener listener to handle events raised by
7299 * this calibrator.
7300 * @param method robust estimator method.
7301 * @return a robust gyroscope calibrator.
7302 * @throws IllegalArgumentException if any of the provided values does
7303 * not have proper size, if either
7304 * turntable rotation rate or
7305 * time interval is zero or negative or
7306 * if provided quality scores length is
7307 * smaller than 10 samples.
7308 */
7309 public static RobustTurntableGyroscopeCalibrator create(
7310 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
7311 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
7312 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
7313 final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
7314 return switch (method) {
7315 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7316 measurements, initialBias, initialMg, initialGg, listener);
7317 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7318 measurements, initialBias, initialMg, initialGg, listener);
7319 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7320 measurements, initialBias, initialMg, initialGg, listener);
7321 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7322 timeInterval, measurements, initialBias, initialMg, initialGg, listener);
7323 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7324 timeInterval, measurements, initialBias, initialMg, initialGg, listener);
7325 };
7326 }
7327
7328 /**
7329 * Creates a robust gyroscope calibrator.
7330 *
7331 * @param qualityScores quality scores corresponding to each provided
7332 * measurement. The larger the score value the better
7333 * the quality of the sample.
7334 * @param position position where body kinematics measures
7335 * have been taken.
7336 * @param turntableRotationRate constant rotation rate at which the
7337 * turntable is spinning. Must be
7338 * expressed in radians per second (rad/s).
7339 * @param timeInterval time interval between measurements being
7340 * captured expressed in seconds (s).
7341 * @param measurements collection of body kinematics
7342 * measurements with standard deviations
7343 * taken at the same position with zero
7344 * velocity and unknown different
7345 * orientations.
7346 * @param initialBias initial gyroscope bias to be used to
7347 * find a solution. This must have length
7348 * 3 and is expressed in radians per
7349 * second (rad/s).
7350 * @param initialMg initial gyroscope scale factors and
7351 * cross coupling errors matrix. Must
7352 * be 3x3.
7353 * @param initialGg initial gyroscope G-dependent cross
7354 * biases introduced on the gyroscope by
7355 * the specific forces sensed by the
7356 * accelerometer. Must be 3x3.
7357 * @param accelerometerBias known accelerometer bias. This must
7358 * have length 3 and is expressed in
7359 * meters per squared second
7360 * (m/s^2).
7361 * @param accelerometerMa known accelerometer scale factors and
7362 * cross coupling matrix. Must be 3x3.
7363 * @param method robust estimator method.
7364 * @return a robust gyroscope calibrator.
7365 * @throws IllegalArgumentException if any of the provided values does
7366 * not have proper size, if either
7367 * turntable rotation rate or
7368 * time interval is zero or negative or
7369 * if provided quality scores length is
7370 * smaller than 10 samples.
7371 */
7372 public static RobustTurntableGyroscopeCalibrator create(
7373 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
7374 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
7375 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
7376 final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
7377 return switch (method) {
7378 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7379 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
7380 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7381 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
7382 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7383 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
7384 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7385 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
7386 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7387 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
7388 };
7389 }
7390
7391 /**
7392 * Creates a robust gyroscope calibrator.
7393 *
7394 * @param qualityScores quality scores corresponding to each provided
7395 * measurement. The larger the score value the better
7396 * the quality of the sample.
7397 * @param position position where body kinematics measures
7398 * have been taken.
7399 * @param turntableRotationRate constant rotation rate at which the
7400 * turntable is spinning. Must be
7401 * expressed in radians per second (rad/s).
7402 * @param timeInterval time interval between measurements being
7403 * captured expressed in seconds (s).
7404 * @param measurements collection of body kinematics
7405 * measurements with standard deviations
7406 * taken at the same position with zero
7407 * velocity and unknown different
7408 * orientations.
7409 * @param initialBias initial gyroscope bias to be used to
7410 * find a solution. This must have length
7411 * 3 and is expressed in radians per
7412 * second (rad/s).
7413 * @param initialMg initial gyroscope scale factors and
7414 * cross coupling errors matrix. Must
7415 * be 3x3.
7416 * @param initialGg initial gyroscope G-dependent cross
7417 * biases introduced on the gyroscope by
7418 * the specific forces sensed by the
7419 * accelerometer. Must be 3x3.
7420 * @param accelerometerBias known accelerometer bias. This must
7421 * have length 3 and is expressed in
7422 * meters per squared second
7423 * (m/s^2).
7424 * @param accelerometerMa known accelerometer scale factors and
7425 * cross coupling matrix. Must be 3x3.
7426 * @param listener listener to handle events raised by
7427 * this calibrator.
7428 * @param method robust estimator method.
7429 * @return a robust gyroscope calibrator.
7430 * @throws IllegalArgumentException if any of the provided values does
7431 * not have proper size, if either
7432 * turntable rotation rate or
7433 * time interval is zero or negative or
7434 * if provided quality scores length is
7435 * smaller than 10 samples.
7436 */
7437 public static RobustTurntableGyroscopeCalibrator create(
7438 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
7439 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
7440 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
7441 final double[] accelerometerBias, final Matrix accelerometerMa,
7442 final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
7443 return switch (method) {
7444 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7445 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
7446 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7447 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
7448 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7449 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
7450 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7451 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
7452 listener);
7453 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7454 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
7455 listener);
7456 };
7457 }
7458
7459 /**
7460 * Creates a robust gyroscope calibrator.
7461 *
7462 * @param qualityScores quality scores corresponding to each provided
7463 * measurement. The larger the score value the better
7464 * the quality of the sample.
7465 * @param position position where body kinematics measures
7466 * have been taken.
7467 * @param turntableRotationRate constant rotation rate at which the
7468 * turntable is spinning. Must be
7469 * expressed in radians per second (rad/s).
7470 * @param timeInterval time interval between measurements being
7471 * captured expressed in seconds (s).
7472 * @param measurements collection of body kinematics
7473 * measurements with standard deviations
7474 * taken at the same position with zero
7475 * velocity and unknown different
7476 * orientations.
7477 * @param initialBias initial gyroscope bias to be used to
7478 * find a solution. This must be 3x1 and
7479 * is expressed in radians per second
7480 * (rad/s).
7481 * @param initialMg initial gyroscope scale factors and
7482 * cross coupling errors matrix. Must
7483 * be 3x3.
7484 * @param initialGg initial gyroscope G-dependent cross
7485 * biases introduced on the gyroscope by
7486 * the specific forces sensed by the
7487 * accelerometer. Must be 3x3.
7488 * @param accelerometerBias known accelerometer bias. This must
7489 * have length 3 and is expressed in
7490 * meters per squared second
7491 * (m/s^2).
7492 * @param accelerometerMa known accelerometer scale factors and
7493 * cross coupling matrix. Must be 3x3.
7494 * @param method robust estimator method.
7495 * @return a robust gyroscope calibrator.
7496 * @throws IllegalArgumentException if any of the provided values does
7497 * not have proper size, if either
7498 * turntable rotation rate or
7499 * time interval is zero or negative or
7500 * if provided quality scores length is
7501 * smaller than 10 samples.
7502 */
7503 public static RobustTurntableGyroscopeCalibrator create(
7504 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
7505 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
7506 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
7507 final Matrix accelerometerMa, final RobustEstimatorMethod method) {
7508 return switch (method) {
7509 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7510 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
7511 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7512 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
7513 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7514 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
7515 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7516 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
7517 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7518 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
7519 };
7520 }
7521
7522 /**
7523 * Creates a robust gyroscope calibrator.
7524 *
7525 * @param qualityScores quality scores corresponding to each provided
7526 * measurement. The larger the score value the better
7527 * the quality of the sample.
7528 * @param position position where body kinematics measures
7529 * have been taken.
7530 * @param turntableRotationRate constant rotation rate at which the
7531 * turntable is spinning. Must be
7532 * expressed in radians per second (rad/s).
7533 * @param timeInterval time interval between measurements being
7534 * captured expressed in seconds (s).
7535 * @param measurements collection of body kinematics
7536 * measurements with standard deviations
7537 * taken at the same position with zero
7538 * velocity and unknown different
7539 * orientations.
7540 * @param initialBias initial gyroscope bias to be used to
7541 * find a solution. This must be 3x1 and
7542 * is expressed in radians per second
7543 * (rad/s).
7544 * @param initialMg initial gyroscope scale factors and
7545 * cross coupling errors matrix. Must
7546 * be 3x3.
7547 * @param initialGg initial gyroscope G-dependent cross
7548 * biases introduced on the gyroscope by
7549 * the specific forces sensed by the
7550 * accelerometer. Must be 3x3.
7551 * @param accelerometerBias known accelerometer bias. This must
7552 * have length 3 and is expressed in
7553 * meters per squared second
7554 * (m/s^2).
7555 * @param accelerometerMa known accelerometer scale factors and
7556 * cross coupling matrix. Must be 3x3.
7557 * @param listener listener to handle events raised by
7558 * this calibrator.
7559 * @param method robust estimator method.
7560 * @return a robust gyroscope calibrator.
7561 * @throws IllegalArgumentException if any of the provided values does
7562 * not have proper size, if either
7563 * turntable rotation rate or
7564 * time interval is zero or negative or
7565 * if provided quality scores length is
7566 * smaller than 10 samples.
7567 */
7568 public static RobustTurntableGyroscopeCalibrator create(
7569 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
7570 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
7571 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
7572 final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
7573 final RobustEstimatorMethod method) {
7574 return switch (method) {
7575 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7576 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
7577 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7578 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
7579 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7580 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
7581 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7582 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
7583 listener);
7584 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7585 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
7586 listener);
7587 };
7588 }
7589
7590 /**
7591 * Creates a robust gyroscope calibrator.
7592 *
7593 * @param qualityScores quality scores corresponding to each provided
7594 * measurement. The larger the score value the better
7595 * the quality of the sample.
7596 * @param position position where body kinematics
7597 * measures have been taken.
7598 * @param turntableRotationRate constant rotation rate at which
7599 * the turntable is spinning. Must
7600 * be expressed in radians per
7601 * second (rad/s).
7602 * @param timeInterval time interval between measurements
7603 * being captured expressed in
7604 * seconds (s).
7605 * @param measurements collection of body kinematics
7606 * measurements with standard
7607 * deviations taken at the same
7608 * position with zero velocity
7609 * and unknown different
7610 * orientations.
7611 * @param commonAxisUsed indicates whether z-axis is
7612 * assumed to be common for
7613 * accelerometer and gyroscope.
7614 * @param estimateGDependentCrossBiases true if G-dependent cross biases
7615 * will be estimated, false
7616 * otherwise.
7617 * @param initialBias initial gyroscope bias to be
7618 * used to find a solution. This
7619 * must be 3x1 and is expressed in
7620 * radians per second (rad/s).
7621 * @param initialMg initial gyroscope scale factors
7622 * and cross coupling errors matrix.
7623 * Must be 3x3.
7624 * @param initialGg initial gyroscope G-dependent
7625 * cross biases introduced on the
7626 * gyroscope by the specific
7627 * forces sensed by the
7628 * accelerometer. Must be 3x3.
7629 * @param method robust estimator method.
7630 * @return a robust gyroscope calibrator.
7631 * @throws IllegalArgumentException if any of the provided values does
7632 * not have proper size, if either
7633 * turntable rotation rate or
7634 * time interval is zero or negative or
7635 * if provided quality scores length is
7636 * smaller than 10 samples.
7637 */
7638 public static RobustTurntableGyroscopeCalibrator create(
7639 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
7640 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
7641 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
7642 final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
7643 return switch (method) {
7644 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7645 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
7646 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7647 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
7648 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7649 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
7650 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7651 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
7652 initialGg);
7653 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7654 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
7655 initialGg);
7656 };
7657 }
7658
7659 /**
7660 * Creates a robust gyroscope calibrator.
7661 *
7662 * @param qualityScores quality scores corresponding to each provided
7663 * measurement. The larger the score value the better
7664 * the quality of the sample.
7665 * @param position position where body kinematics
7666 * measures have been taken.
7667 * @param turntableRotationRate constant rotation rate at which
7668 * the turntable is spinning. Must
7669 * be expressed in radians per
7670 * second (rad/s).
7671 * @param timeInterval time interval between measurements
7672 * being captured expressed in
7673 * seconds (s).
7674 * @param measurements collection of body kinematics
7675 * measurements with standard
7676 * deviations taken at the same
7677 * position with zero velocity
7678 * and unknown different
7679 * orientations.
7680 * @param commonAxisUsed indicates whether z-axis is
7681 * assumed to be common for
7682 * accelerometer and gyroscope.
7683 * @param estimateGDependentCrossBiases true if G-dependent cross biases
7684 * will be estimated, false
7685 * otherwise.
7686 * @param initialBias initial gyroscope bias to be
7687 * used to find a solution. This
7688 * must be 3x1 and is expressed in
7689 * radians per second (rad/s).
7690 * @param initialMg initial gyroscope scale factors
7691 * and cross coupling errors matrix.
7692 * Must be 3x3.
7693 * @param initialGg initial gyroscope G-dependent
7694 * cross biases introduced on the
7695 * gyroscope by the specific
7696 * forces sensed by the
7697 * accelerometer. Must be 3x3.
7698 * @param listener listener to handle events raised by
7699 * this calibrator.
7700 * @param method robust estimator method.
7701 * @return a robust gyroscope calibrator.
7702 * @throws IllegalArgumentException if any of the provided values does
7703 * not have proper size, if either
7704 * turntable rotation rate or
7705 * time interval is zero or negative or
7706 * if provided quality scores length is
7707 * smaller than 10 samples.
7708 */
7709 public static RobustTurntableGyroscopeCalibrator create(
7710 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
7711 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
7712 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
7713 final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
7714 final RobustEstimatorMethod method) {
7715 return switch (method) {
7716 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7717 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7718 listener);
7719 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7720 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7721 listener);
7722 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7723 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7724 listener);
7725 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7726 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
7727 initialGg, listener);
7728 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7729 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
7730 initialGg, listener);
7731 };
7732 }
7733
7734 /**
7735 * Creates a robust gyroscope calibrator.
7736 *
7737 * @param qualityScores quality scores corresponding to each provided
7738 * measurement. The larger the score value the better
7739 * the quality of the sample.
7740 * @param position position where body kinematics
7741 * measures have been taken.
7742 * @param turntableRotationRate constant rotation rate at which
7743 * the turntable is spinning. Must
7744 * be expressed in radians per
7745 * second (rad/s).
7746 * @param timeInterval time interval between measurements
7747 * being captured expressed in
7748 * seconds (s).
7749 * @param measurements collection of body kinematics
7750 * measurements with standard
7751 * deviations taken at the same
7752 * position with zero velocity
7753 * and unknown different
7754 * orientations.
7755 * @param commonAxisUsed indicates whether z-axis is
7756 * assumed to be common for
7757 * accelerometer and gyroscope.
7758 * @param estimateGDependentCrossBiases true if G-dependent cross biases
7759 * will be estimated, false
7760 * otherwise.
7761 * @param initialBias initial gyroscope bias to be
7762 * used to find a solution. This
7763 * must have length 3 and is
7764 * expressed in radians per second
7765 * (rad/s).
7766 * @param initialMg initial gyroscope scale factors
7767 * and cross coupling errors matrix.
7768 * Must be 3x3.
7769 * @param initialGg initial gyroscope G-dependent
7770 * cross biases introduced on the
7771 * gyroscope by the specific forces
7772 * sensed by the accelerometer.
7773 * Must be 3x3.
7774 * @param method robust estimator method.
7775 * @return a robust gyroscope calibrator.
7776 * @throws IllegalArgumentException if any of the provided values does
7777 * not have proper size or if either
7778 * turntable rotation rate or
7779 * time interval is zero or negative.
7780 */
7781 public static RobustTurntableGyroscopeCalibrator create(
7782 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
7783 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
7784 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
7785 final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
7786 return switch (method) {
7787 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7788 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
7789 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7790 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
7791 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7792 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
7793 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7794 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
7795 initialGg);
7796 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7797 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
7798 initialGg);
7799 };
7800 }
7801
7802 /**
7803 * Creates a robust gyroscope calibrator.
7804 *
7805 * @param qualityScores quality scores corresponding to each provided
7806 * measurement. The larger the score value the better
7807 * the quality of the sample.
7808 * @param position position where body kinematics
7809 * measures have been taken.
7810 * @param turntableRotationRate constant rotation rate at which
7811 * the turntable is spinning. Must
7812 * be expressed in radians per
7813 * second (rad/s).
7814 * @param timeInterval time interval between measurements
7815 * being captured expressed in
7816 * seconds (s).
7817 * @param measurements collection of body kinematics
7818 * measurements with standard
7819 * deviations taken at the same
7820 * position with zero velocity
7821 * and unknown different
7822 * orientations.
7823 * @param commonAxisUsed indicates whether z-axis is
7824 * assumed to be common for
7825 * accelerometer and gyroscope.
7826 * @param estimateGDependentCrossBiases true if G-dependent cross biases
7827 * will be estimated, false
7828 * otherwise.
7829 * @param initialBias initial gyroscope bias to be
7830 * used to find a solution. This
7831 * must have length 3 and is
7832 * expressed in radians per second
7833 * (rad/s).
7834 * @param initialMg initial gyroscope scale factors
7835 * and cross coupling errors matrix.
7836 * Must be 3x3.
7837 * @param initialGg initial gyroscope G-dependent
7838 * cross biases introduced on the
7839 * gyroscope by the specific forces
7840 * sensed by the accelerometer.
7841 * Must be 3x3.
7842 * @param listener listener to handle events raised
7843 * by this calibrator.
7844 * @param method robust estimator method.
7845 * @return a robust gyroscope calibrator.
7846 * @throws IllegalArgumentException if any of the provided values does
7847 * not have proper size, if either
7848 * turntable rotation rate or
7849 * time interval is zero or negative or
7850 * if provided quality scores length is
7851 * smaller than 10 samples.
7852 */
7853 public static RobustTurntableGyroscopeCalibrator create(
7854 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
7855 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
7856 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
7857 final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
7858 final RobustEstimatorMethod method) {
7859 return switch (method) {
7860 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7861 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7862 listener);
7863 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7864 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7865 listener);
7866 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7867 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7868 listener);
7869 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7870 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
7871 initialGg, listener);
7872 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7873 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
7874 initialGg, listener);
7875 };
7876 }
7877
7878 /**
7879 * Creates a robust gyroscope calibrator.
7880 *
7881 * @param qualityScores quality scores corresponding to each provided
7882 * measurement. The larger the score value the better
7883 * the quality of the sample.
7884 * @param position position where body kinematics
7885 * measures have been taken.
7886 * @param turntableRotationRate constant rotation rate at which
7887 * the turntable is spinning. Must
7888 * be expressed in radians per
7889 * second (rad/s).
7890 * @param timeInterval time interval between measurements
7891 * being captured expressed in
7892 * seconds (s).
7893 * @param measurements collection of body kinematics
7894 * measurements with standard
7895 * deviations taken at the same
7896 * position with zero velocity
7897 * and unknown different
7898 * orientations.
7899 * @param commonAxisUsed indicates whether z-axis is
7900 * assumed to be common for
7901 * accelerometer and gyroscope.
7902 * @param estimateGDependentCrossBiases true if G-dependent cross
7903 * biases will be estimated,
7904 * false otherwise.
7905 * @param initialBias initial gyroscope bias to be
7906 * used to find a solution. This
7907 * must have length 3 and is
7908 * expressed in radians per second
7909 * (rad/s).
7910 * @param initialMg initial gyroscope scale factors
7911 * and cross coupling errors
7912 * matrix. Must be 3x3.
7913 * @param initialGg initial gyroscope G-dependent
7914 * cross biases introduced on the
7915 * gyroscope by the specific forces
7916 * sensed by the accelerometer.
7917 * Must be 3x3.
7918 * @param accelerometerBias known accelerometer bias. This
7919 * must have length 3 and is
7920 * expressed in meters per squared
7921 * second (m/s^2).
7922 * @param accelerometerMa known accelerometer scale factors
7923 * and cross coupling matrix. Must
7924 * be 3x3.
7925 * @param method robust estimator method.
7926 * @return a robust gyroscope calibrator.
7927 * @throws IllegalArgumentException if any of the provided values does
7928 * not have proper size, if either
7929 * turntable rotation rate or
7930 * time interval is zero or negative or
7931 * if provided quality scores length is
7932 * smaller than 10 samples.
7933 */
7934 public static RobustTurntableGyroscopeCalibrator create(
7935 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
7936 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
7937 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
7938 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
7939 final Matrix accelerometerMa, final RobustEstimatorMethod method) {
7940 return switch (method) {
7941 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7942 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7943 accelerometerBias, accelerometerMa);
7944 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7945 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7946 accelerometerBias, accelerometerMa);
7947 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
7948 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
7949 accelerometerBias, accelerometerMa);
7950 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7951 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
7952 initialGg, accelerometerBias, accelerometerMa);
7953 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
7954 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
7955 initialGg, accelerometerBias, accelerometerMa);
7956 };
7957 }
7958
7959 /**
7960 * Creates a robust gyroscope calibrator.
7961 *
7962 * @param qualityScores quality scores corresponding to each provided
7963 * measurement. The larger the score value the better
7964 * the quality of the sample.
7965 * @param position position where body kinematics
7966 * measures have been taken.
7967 * @param turntableRotationRate constant rotation rate at which
7968 * the turntable is spinning. Must
7969 * be expressed in radians per
7970 * second (rad/s).
7971 * @param timeInterval time interval between measurements
7972 * being captured expressed in
7973 * seconds (s).
7974 * @param measurements collection of body kinematics
7975 * measurements with standard
7976 * deviations taken at the same
7977 * position with zero velocity
7978 * and unknown different
7979 * orientations.
7980 * @param commonAxisUsed indicates whether z-axis is
7981 * assumed to be common for
7982 * accelerometer and gyroscope.
7983 * @param estimateGDependentCrossBiases true if G-dependent cross
7984 * biases will be estimated,
7985 * false otherwise.
7986 * @param initialBias initial gyroscope bias to be
7987 * used to find a solution. This
7988 * must have length 3 and is
7989 * expressed in radians per second
7990 * (rad/s).
7991 * @param initialMg initial gyroscope scale factors
7992 * and cross coupling errors
7993 * matrix. Must be 3x3.
7994 * @param initialGg initial gyroscope G-dependent
7995 * cross biases introduced on the
7996 * gyroscope by the specific forces
7997 * sensed by the accelerometer.
7998 * Must be 3x3.
7999 * @param accelerometerBias known accelerometer bias. This
8000 * must have length 3 and is
8001 * expressed in meters per squared
8002 * second (m/s^2).
8003 * @param accelerometerMa known accelerometer scale factors
8004 * and cross coupling matrix. Must
8005 * be 3x3.
8006 * @param listener listener to handle events raised
8007 * by this calibrator.
8008 * @param method robust estimator method.
8009 * @return a robust gyroscope calibrator.
8010 * @throws IllegalArgumentException if any of the provided values does
8011 * not have proper size, if either
8012 * turntable rotation rate or
8013 * time interval is zero or negative or
8014 * if provided quality scores length is
8015 * smaller than 10 samples.
8016 */
8017 public static RobustTurntableGyroscopeCalibrator create(
8018 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
8019 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8020 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
8021 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
8022 final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
8023 final RobustEstimatorMethod method) {
8024 return switch (method) {
8025 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8026 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8027 accelerometerBias, accelerometerMa, listener);
8028 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8029 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8030 accelerometerBias, accelerometerMa, listener);
8031 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8032 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8033 accelerometerBias, accelerometerMa, listener);
8034 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8035 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
8036 initialGg, accelerometerBias, accelerometerMa, listener);
8037 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8038 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
8039 initialGg, accelerometerBias, accelerometerMa, listener);
8040 };
8041 }
8042
8043 /**
8044 * Creates a robust gyroscope calibrator.
8045 *
8046 * @param qualityScores quality scores corresponding to each provided
8047 * measurement. The larger the score value the better
8048 * the quality of the sample.
8049 * @param position position where body kinematics
8050 * measures have been taken.
8051 * @param turntableRotationRate constant rotation rate at which
8052 * the turntable is spinning. Must
8053 * be expressed in radians per
8054 * second (rad/s).
8055 * @param timeInterval time interval between measurements
8056 * being captured expressed in
8057 * seconds (s).
8058 * @param measurements collection of body kinematics
8059 * measurements with standard
8060 * deviations taken at the same
8061 * position with zero velocity and
8062 * unknown different orientations.
8063 * @param commonAxisUsed indicates whether z-axis is
8064 * assumed to be common for
8065 * accelerometer and gyroscope.
8066 * @param estimateGDependentCrossBiases true if G-dependent cross biases
8067 * will be estimated, false
8068 * otherwise.
8069 * @param initialBias initial gyroscope bias to be
8070 * used to find a solution. This
8071 * must be 3x1 and is expressed in
8072 * radians per second (rad/s).
8073 * @param initialMg initial gyroscope scale factors
8074 * and cross coupling errors matrix.
8075 * Must be 3x3.
8076 * @param initialGg initial gyroscope G-dependent
8077 * cross biases introduced on the
8078 * gyroscope by the specific forces
8079 * sensed by the accelerometer. Must
8080 * be 3x3.
8081 * @param accelerometerBias known accelerometer bias. This
8082 * must have length 3 and is
8083 * expressed in meters per squared
8084 * second (m/s^2).
8085 * @param accelerometerMa known accelerometer scale factors
8086 * and cross coupling matrix. Must
8087 * be 3x3.
8088 * @param method robust estimator method.
8089 * @return a robust gyroscope calibrator.
8090 * @throws IllegalArgumentException if any of the provided values does
8091 * not have proper size, if either
8092 * turntable rotation rate or
8093 * time interval is zero or negative or
8094 * if provided quality scores length is
8095 * smaller than 10 samples.
8096 */
8097 public static RobustTurntableGyroscopeCalibrator create(
8098 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
8099 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8100 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
8101 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
8102 final Matrix accelerometerMa, final RobustEstimatorMethod method) {
8103 return switch (method) {
8104 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8105 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8106 accelerometerBias, accelerometerMa);
8107 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8108 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8109 accelerometerBias, accelerometerMa);
8110 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8111 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8112 accelerometerBias, accelerometerMa);
8113 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8114 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
8115 initialGg, accelerometerBias, accelerometerMa);
8116 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8117 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias,
8118 initialMg, initialGg, accelerometerBias, accelerometerMa);
8119 };
8120 }
8121
8122 /**
8123 * Creates a robust gyroscope calibrator.
8124 *
8125 * @param qualityScores quality scores corresponding to each provided
8126 * measurement. The larger the score value the better
8127 * the quality of the sample.*
8128 * @param position position where body kinematics
8129 * measures have been taken.
8130 * @param turntableRotationRate constant rotation rate at which
8131 * the turntable is spinning. Must
8132 * be expressed in radians per
8133 * second (rad/s).
8134 * @param timeInterval time interval between measurements
8135 * being captured expressed in
8136 * seconds (s).
8137 * @param measurements collection of body kinematics
8138 * measurements with standard
8139 * deviations taken at the same
8140 * position with zero velocity and
8141 * unknown different orientations.
8142 * @param commonAxisUsed indicates whether z-axis is
8143 * assumed to be common for
8144 * accelerometer and gyroscope.
8145 * @param estimateGDependentCrossBiases true if G-dependent cross biases
8146 * will be estimated, false
8147 * otherwise.
8148 * @param initialBias initial gyroscope bias to be
8149 * used to find a solution. This
8150 * must be 3x1 and is expressed in
8151 * radians per second (rad/s).
8152 * @param initialMg initial gyroscope scale factors
8153 * and cross coupling errors matrix.
8154 * Must be 3x3.
8155 * @param initialGg initial gyroscope G-dependent
8156 * cross biases introduced on the
8157 * gyroscope by the specific forces
8158 * sensed by the accelerometer. Must
8159 * be 3x3.
8160 * @param accelerometerBias known accelerometer bias. This
8161 * must have length 3 and is
8162 * expressed in meters per squared
8163 * second (m/s^2).
8164 * @param accelerometerMa known accelerometer scale factors
8165 * and cross coupling matrix. Must
8166 * be 3x3.
8167 * @param listener listener to handle events raised
8168 * by this calibrator.
8169 * @param method robust estimator method.
8170 * @return a robust gyroscope calibrator.
8171 * @throws IllegalArgumentException if any of the provided values does
8172 * not have proper size, if either
8173 * turntable rotation rate or
8174 * time interval is zero or negative or
8175 * if provided quality scores length is
8176 * smaller than 10 samples.
8177 */
8178 public static RobustTurntableGyroscopeCalibrator create(
8179 final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
8180 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8181 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
8182 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
8183 final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
8184 final RobustEstimatorMethod method) {
8185 return switch (method) {
8186 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8187 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8188 accelerometerBias, accelerometerMa, listener);
8189 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8190 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8191 accelerometerBias, accelerometerMa, listener);
8192 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8193 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8194 accelerometerBias, accelerometerMa, listener);
8195 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8196 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
8197 initialGg, accelerometerBias, accelerometerMa, listener);
8198 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8199 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
8200 initialGg, accelerometerBias, accelerometerMa, listener);
8201 };
8202 }
8203
8204 /**
8205 * Creates a robust gyroscope calibrator.
8206 *
8207 * @param qualityScores quality scores corresponding to each provided
8208 * measurement. The larger the score value the better
8209 * the quality of the sample.
8210 * @param position position where body kinematics measures
8211 * have been taken.
8212 * @param turntableRotationRate constant rotation rate at which the
8213 * turntable is spinning. Must be
8214 * expressed in radians per second (rad/s).
8215 * @param timeInterval time interval between measurements being
8216 * captured expressed in seconds (s).
8217 * @param measurements collection of body kinematics
8218 * measurements with standard deviations
8219 * taken at the same position with zero
8220 * velocity and unknown different
8221 * orientations.
8222 * @param initialBias initial gyroscope bias to be used to
8223 * find a solution. This must be 3x1 and
8224 * is expressed in radians per second
8225 * (rad/s).
8226 * @param initialMg initial gyroscope scale factors and
8227 * cross coupling errors matrix. Must
8228 * be 3x3.
8229 * @param initialGg initial gyroscope G-dependent cross
8230 * biases introduced on the gyroscope by
8231 * the specific forces sensed by the
8232 * accelerometer. Must be 3x3.
8233 * @param method robust estimator method.
8234 * @return a robust gyroscope calibrator.
8235 * @throws IllegalArgumentException if any of the provided values does
8236 * not have proper size, if either
8237 * turntable rotation rate or
8238 * time interval is zero or negative or
8239 * if provided quality scores length is
8240 * smaller than 10 samples.
8241 */
8242 public static RobustTurntableGyroscopeCalibrator create(
8243 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
8244 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8245 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
8246 final RobustEstimatorMethod method) {
8247 return switch (method) {
8248 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8249 measurements, initialBias, initialMg, initialGg);
8250 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8251 measurements, initialBias, initialMg, initialGg);
8252 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8253 measurements, initialBias, initialMg, initialGg);
8254 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8255 timeInterval, measurements, initialBias, initialMg, initialGg);
8256 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8257 timeInterval, measurements, initialBias, initialMg, initialGg);
8258 };
8259 }
8260
8261 /**
8262 * Creates a robust gyroscope calibrator.
8263 *
8264 * @param qualityScores quality scores corresponding to each provided
8265 * measurement. The larger the score value the better
8266 * the quality of the sample.
8267 * @param position position where body kinematics measures
8268 * have been taken.
8269 * @param turntableRotationRate constant rotation rate at which the
8270 * turntable is spinning. Must be
8271 * expressed in radians per second (rad/s).
8272 * @param timeInterval time interval between measurements being
8273 * captured expressed in seconds (s).
8274 * @param measurements collection of body kinematics
8275 * measurements with standard deviations
8276 * taken at the same position with zero
8277 * velocity and unknown different
8278 * orientations.
8279 * @param initialBias initial gyroscope bias to be used to
8280 * find a solution. This must be 3x1 and
8281 * is expressed in radians per second
8282 * (rad/s).
8283 * @param initialMg initial gyroscope scale factors and
8284 * cross coupling errors matrix. Must
8285 * be 3x3.
8286 * @param initialGg initial gyroscope G-dependent cross
8287 * biases introduced on the gyroscope by
8288 * the specific forces sensed by the
8289 * accelerometer. Must be 3x3.
8290 * @param listener listener to handle events raised
8291 * by this calibrator.
8292 * @param method robust estimator method.
8293 * @return a robust gyroscope calibrator.
8294 * @throws IllegalArgumentException if any of the provided values does
8295 * not have proper size, if either
8296 * turntable rotation rate or
8297 * time interval is zero or negative or
8298 * if provided quality scores length is
8299 * smaller than 10 samples.
8300 */
8301 public static RobustTurntableGyroscopeCalibrator create(
8302 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
8303 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8304 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
8305 final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
8306 return switch (method) {
8307 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8308 measurements, initialBias, initialMg, initialGg, listener);
8309 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8310 measurements, initialBias, initialMg, initialGg, listener);
8311 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8312 measurements, initialBias, initialMg, initialGg, listener);
8313 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8314 timeInterval, measurements, initialBias, initialMg, initialGg, listener);
8315 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8316 timeInterval, measurements, initialBias, initialMg, initialGg, listener);
8317 };
8318 }
8319
8320 /**
8321 * Creates a robust gyroscope calibrator.
8322 *
8323 * @param qualityScores quality scores corresponding to each provided
8324 * measurement. The larger the score value the better
8325 * the quality of the sample.
8326 * @param position position where body kinematics measures
8327 * have been taken.
8328 * @param turntableRotationRate constant rotation rate at which the
8329 * turntable is spinning. Must be
8330 * expressed in radians per second (rad/s).
8331 * @param timeInterval time interval between measurements being
8332 * captured expressed in seconds (s).
8333 * @param measurements collection of body kinematics
8334 * measurements with standard deviations
8335 * taken at the same position with zero
8336 * velocity and unknown different
8337 * orientations.
8338 * @param initialBias initial gyroscope bias to be used to
8339 * find a solution. This must have
8340 * length 3 and is expressed in radians
8341 * per second (rad/s).
8342 * @param initialMg initial gyroscope scale factors and
8343 * cross coupling errors matrix. Must
8344 * be 3x3.
8345 * @param initialGg initial gyroscope G-dependent cross
8346 * biases introduced on the gyroscope by
8347 * the specific forces sensed by the
8348 * accelerometer. Must be 3x3.
8349 * @param method robust estimator method.
8350 * @return a robust gyroscope calibrator.
8351 * @throws IllegalArgumentException if any of the provided values does
8352 * not have proper size, if either
8353 * turntable rotation rate or
8354 * time interval is zero or negative or
8355 * if provided quality scores length is
8356 * smaller than 10 samples.
8357 */
8358 public static RobustTurntableGyroscopeCalibrator create(
8359 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
8360 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8361 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
8362 final RobustEstimatorMethod method) {
8363 return switch (method) {
8364 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8365 measurements, initialBias, initialMg, initialGg);
8366 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8367 measurements, initialBias, initialMg, initialGg);
8368 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8369 measurements, initialBias, initialMg, initialGg);
8370 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8371 timeInterval, measurements, initialBias, initialMg, initialGg);
8372 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8373 timeInterval, measurements, initialBias, initialMg, initialGg);
8374 };
8375 }
8376
8377 /**
8378 * Creates a robust gyroscope calibrator.
8379 *
8380 * @param qualityScores quality scores corresponding to each provided
8381 * measurement. The larger the score value the better
8382 * the quality of the sample.
8383 * @param position position where body kinematics measures
8384 * have been taken.
8385 * @param turntableRotationRate constant rotation rate at which the
8386 * turntable is spinning. Must be
8387 * expressed in radians per second (rad/s).
8388 * @param timeInterval time interval between measurements being
8389 * captured expressed in seconds (s).
8390 * @param measurements collection of body kinematics
8391 * measurements with standard deviations
8392 * taken at the same position with zero
8393 * velocity and unknown different
8394 * orientations.
8395 * @param initialBias initial gyroscope bias to be used to
8396 * find a solution. This must have
8397 * length 3 and is expressed in radians
8398 * per second (rad/s).
8399 * @param initialMg initial gyroscope scale factors and
8400 * cross coupling errors matrix. Must
8401 * be 3x3.
8402 * @param initialGg initial gyroscope G-dependent cross
8403 * biases introduced on the gyroscope by
8404 * the specific forces sensed by the
8405 * accelerometer. Must be 3x3.
8406 * @param listener listener to handle events raised
8407 * by this calibrator.
8408 * @param method robust estimator method.
8409 * @return a robust gyroscope calibrator.
8410 * @throws IllegalArgumentException if any of the provided values does
8411 * not have proper size, if either
8412 * turntable rotation rate or
8413 * time interval is zero or negative or
8414 * if provided quality scores length is
8415 * smaller than 10 samples.
8416 */
8417 public static RobustTurntableGyroscopeCalibrator create(
8418 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
8419 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8420 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
8421 final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
8422 return switch (method) {
8423 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8424 measurements, initialBias, initialMg, initialGg, listener);
8425 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8426 measurements, initialBias, initialMg, initialGg, listener);
8427 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8428 measurements, initialBias, initialMg, initialGg, listener);
8429 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8430 timeInterval, measurements, initialBias, initialMg, initialGg, listener);
8431 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8432 timeInterval, measurements, initialBias, initialMg, initialGg, listener);
8433 };
8434 }
8435
8436 /**
8437 * Creates a robust gyroscope calibrator.
8438 *
8439 * @param qualityScores quality scores corresponding to each provided
8440 * measurement. The larger the score value the better
8441 * the quality of the sample.
8442 * @param position position where body kinematics measures
8443 * have been taken.
8444 * @param turntableRotationRate constant rotation rate at which the
8445 * turntable is spinning. Must be
8446 * expressed in radians per second (rad/s).
8447 * @param timeInterval time interval between measurements being
8448 * captured expressed in seconds (s).
8449 * @param measurements collection of body kinematics
8450 * measurements with standard deviations
8451 * taken at the same position with zero
8452 * velocity and unknown different
8453 * orientations.
8454 * @param initialBias initial gyroscope bias to be used to
8455 * find a solution. This must have length
8456 * 3 and is expressed in radians per
8457 * second (rad/s).
8458 * @param initialMg initial gyroscope scale factors and
8459 * cross coupling errors matrix. Must
8460 * be 3x3.
8461 * @param initialGg initial gyroscope G-dependent cross
8462 * biases introduced on the gyroscope by
8463 * the specific forces sensed by the
8464 * accelerometer. Must be 3x3.
8465 * @param accelerometerBias known accelerometer bias. This must
8466 * have length 3 and is expressed in
8467 * meters per squared second
8468 * (m/s^2).
8469 * @param accelerometerMa known accelerometer scale factors and
8470 * cross coupling matrix. Must be 3x3.
8471 * @param method robust estimator method.
8472 * @return a robust gyroscope calibrator.
8473 * @throws IllegalArgumentException if any of the provided values does
8474 * not have proper size, if either
8475 * turntable rotation rate or
8476 * time interval is zero or negative or
8477 * if provided quality scores length is
8478 * smaller than 10 samples.
8479 */
8480 public static RobustTurntableGyroscopeCalibrator create(
8481 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
8482 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8483 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
8484 final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
8485 return switch (method) {
8486 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8487 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
8488 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8489 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
8490 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8491 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
8492 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8493 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
8494 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8495 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
8496 };
8497 }
8498
8499 /**
8500 * Creates a robust gyroscope calibrator.
8501 *
8502 * @param qualityScores quality scores corresponding to each provided
8503 * measurement. The larger the score value the better
8504 * the quality of the sample.
8505 * @param position position where body kinematics measures
8506 * have been taken.
8507 * @param turntableRotationRate constant rotation rate at which the
8508 * turntable is spinning. Must be
8509 * expressed in radians per second (rad/s).
8510 * @param timeInterval time interval between measurements being
8511 * captured expressed in seconds (s).
8512 * @param measurements collection of body kinematics
8513 * measurements with standard deviations
8514 * taken at the same position with zero
8515 * velocity and unknown different
8516 * orientations.
8517 * @param initialBias initial gyroscope bias to be used to
8518 * find a solution. This must have length
8519 * 3 and is expressed in radians per
8520 * second (rad/s).
8521 * @param initialMg initial gyroscope scale factors and
8522 * cross coupling errors matrix. Must
8523 * be 3x3.
8524 * @param initialGg initial gyroscope G-dependent cross
8525 * biases introduced on the gyroscope by
8526 * the specific forces sensed by the
8527 * accelerometer. Must be 3x3.
8528 * @param accelerometerBias known accelerometer bias. This must
8529 * have length 3 and is expressed in
8530 * meters per squared second
8531 * (m/s^2).
8532 * @param accelerometerMa known accelerometer scale factors and
8533 * cross coupling matrix. Must be 3x3.
8534 * @param listener listener to handle events raised
8535 * by this calibrator.
8536 * @param method robust estimator method.
8537 * @return a robust gyroscope calibrator.
8538 * @throws IllegalArgumentException if any of the provided values does
8539 * not have proper size, if either
8540 * turntable rotation rate or
8541 * time interval is zero or negative or
8542 * if provided quality scores length is
8543 * smaller than 10 samples.
8544 */
8545 public static RobustTurntableGyroscopeCalibrator create(
8546 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
8547 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8548 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
8549 final double[] accelerometerBias, final Matrix accelerometerMa,
8550 final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
8551 return switch (method) {
8552 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8553 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
8554 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8555 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
8556 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8557 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
8558 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8559 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
8560 listener);
8561 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8562 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
8563 listener);
8564 };
8565 }
8566
8567 /**
8568 * Creates a robust gyroscope calibrator.
8569 *
8570 * @param qualityScores quality scores corresponding to each provided
8571 * measurement. The larger the score value the better
8572 * the quality of the sample.
8573 * @param position position where body kinematics measures
8574 * have been taken.
8575 * @param turntableRotationRate constant rotation rate at which the
8576 * turntable is spinning. Must be
8577 * expressed in radians per second (rad/s).
8578 * @param timeInterval time interval between measurements being
8579 * captured expressed in seconds (s).
8580 * @param measurements collection of body kinematics
8581 * measurements with standard deviations
8582 * taken at the same position with zero
8583 * velocity and unknown different
8584 * orientations.
8585 * @param initialBias initial gyroscope bias to be used to
8586 * find a solution. This must be 3x1 and
8587 * is expressed in radians per second
8588 * (rad/s).
8589 * @param initialMg initial gyroscope scale factors and
8590 * cross coupling errors matrix. Must
8591 * be 3x3.
8592 * @param initialGg initial gyroscope G-dependent cross
8593 * biases introduced on the gyroscope by
8594 * the specific forces sensed by the
8595 * accelerometer. Must be 3x3.
8596 * @param accelerometerBias known accelerometer bias. This must
8597 * have length 3 and is expressed in
8598 * meters per squared second
8599 * (m/s^2).
8600 * @param accelerometerMa known accelerometer scale factors and
8601 * cross coupling matrix. Must be 3x3.
8602 * @param method robust estimator method.
8603 * @return a robust gyroscope calibrator.
8604 * @throws IllegalArgumentException if any of the provided values does
8605 * not have proper size, if either
8606 * turntable rotation rate or
8607 * time interval is zero or negative or
8608 * if provided quality scores length is
8609 * smaller than 10 samples.
8610 */
8611 public static RobustTurntableGyroscopeCalibrator create(
8612 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
8613 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8614 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
8615 final Matrix accelerometerMa, final RobustEstimatorMethod method) {
8616 return switch (method) {
8617 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8618 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
8619 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8620 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
8621 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8622 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
8623 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8624 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
8625 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8626 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
8627 };
8628 }
8629
8630 /**
8631 * Creates a robust gyroscope calibrator.
8632 *
8633 * @param qualityScores quality scores corresponding to each provided
8634 * measurement. The larger the score value the better
8635 * the quality of the sample.
8636 * @param position position where body kinematics measures
8637 * have been taken.
8638 * @param turntableRotationRate constant rotation rate at which the
8639 * turntable is spinning. Must be
8640 * expressed in radians per second (rad/s).
8641 * @param timeInterval time interval between measurements being
8642 * captured expressed in seconds (s).
8643 * @param measurements collection of body kinematics
8644 * measurements with standard deviations
8645 * taken at the same position with zero
8646 * velocity and unknown different
8647 * orientations.
8648 * @param initialBias initial gyroscope bias to be used to
8649 * find a solution. This must be 3x1 and
8650 * is expressed in radians per second
8651 * (rad/s).
8652 * @param initialMg initial gyroscope scale factors and
8653 * cross coupling errors matrix. Must
8654 * be 3x3.
8655 * @param initialGg initial gyroscope G-dependent cross
8656 * biases introduced on the gyroscope by
8657 * the specific forces sensed by the
8658 * accelerometer. Must be 3x3.
8659 * @param accelerometerBias known accelerometer bias. This must
8660 * have length 3 and is expressed in
8661 * meters per squared second
8662 * (m/s^2).
8663 * @param accelerometerMa known accelerometer scale factors and
8664 * cross coupling matrix. Must be 3x3.
8665 * @param listener listener to handle events raised
8666 * by this calibrator.
8667 * @param method robust estimator method.
8668 * @return a robust gyroscope calibrator.
8669 * @throws IllegalArgumentException if any of the provided values does
8670 * not have proper size, if either
8671 * turntable rotation rate or
8672 * time interval is zero or negative or
8673 * if provided quality scores length is
8674 * smaller than 10 samples.
8675 */
8676 public static RobustTurntableGyroscopeCalibrator create(
8677 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
8678 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8679 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
8680 final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
8681 final RobustEstimatorMethod method) {
8682 return switch (method) {
8683 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8684 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
8685 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8686 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
8687 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8688 measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
8689 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8690 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
8691 listener);
8692 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8693 timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
8694 listener);
8695 };
8696 }
8697
8698 /**
8699 * Creates a robust gyroscope calibrator.
8700 *
8701 * @param qualityScores quality scores corresponding to each provided
8702 * measurement. The larger the score value the better
8703 * the quality of the sample.
8704 * @param position position where body kinematics
8705 * measures have been taken.
8706 * @param turntableRotationRate constant rotation rate at which
8707 * the turntable is spinning. Must
8708 * be expressed in radians per
8709 * second (rad/s).
8710 * @param timeInterval time interval between measurements
8711 * being captured expressed in
8712 * seconds (s).
8713 * @param measurements collection of body kinematics
8714 * measurements with standard
8715 * deviations taken at the same
8716 * position with zero velocity
8717 * and unknown different
8718 * orientations.
8719 * @param commonAxisUsed indicates whether z-axis is
8720 * assumed to be common for
8721 * accelerometer and gyroscope.
8722 * @param estimateGDependentCrossBiases true if G-dependent cross biases
8723 * will be estimated, false
8724 * otherwise.
8725 * @param initialBias initial gyroscope bias to be
8726 * used to find a solution. This
8727 * must be 3x1 and is expressed in
8728 * radians per second (rad/s).
8729 * @param initialMg initial gyroscope scale factors
8730 * and cross coupling errors matrix.
8731 * Must be 3x3.
8732 * @param initialGg initial gyroscope G-dependent
8733 * cross biases introduced on the
8734 * gyroscope by the specific
8735 * forces sensed by the
8736 * accelerometer. Must be 3x3.
8737 * @param method robust estimator method.
8738 * @return a robust gyroscope calibrator.
8739 * @throws IllegalArgumentException if any of the provided values does
8740 * not have proper size, if either
8741 * turntable rotation rate or
8742 * time interval is zero or negative or
8743 * if provided quality scores length is
8744 * smaller than 10 samples.
8745 */
8746 public static RobustTurntableGyroscopeCalibrator create(
8747 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
8748 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8749 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
8750 final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
8751 return switch (method) {
8752 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8753 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
8754 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8755 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
8756 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8757 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
8758 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8759 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
8760 initialGg);
8761 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8762 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
8763 initialGg);
8764 };
8765 }
8766
8767 /**
8768 * Creates a robust gyroscope calibrator.
8769 *
8770 * @param qualityScores quality scores corresponding to each provided
8771 * measurement. The larger the score value the better
8772 * the quality of the sample.
8773 * @param position position where body kinematics
8774 * measures have been taken.
8775 * @param turntableRotationRate constant rotation rate at which
8776 * the turntable is spinning. Must
8777 * be expressed in radians per
8778 * second (rad/s).
8779 * @param timeInterval time interval between measurements
8780 * being captured expressed in
8781 * seconds (s).
8782 * @param measurements collection of body kinematics
8783 * measurements with standard
8784 * deviations taken at the same
8785 * position with zero velocity
8786 * and unknown different
8787 * orientations.
8788 * @param commonAxisUsed indicates whether z-axis is
8789 * assumed to be common for
8790 * accelerometer and gyroscope.
8791 * @param estimateGDependentCrossBiases true if G-dependent cross biases
8792 * will be estimated, false
8793 * otherwise.
8794 * @param initialBias initial gyroscope bias to be
8795 * used to find a solution. This
8796 * must be 3x1 and is expressed in
8797 * radians per second (rad/s).
8798 * @param initialMg initial gyroscope scale factors
8799 * and cross coupling errors matrix.
8800 * Must be 3x3.
8801 * @param initialGg initial gyroscope G-dependent
8802 * cross biases introduced on the
8803 * gyroscope by the specific
8804 * forces sensed by the
8805 * accelerometer. Must be 3x3.
8806 * @param listener listener to handle events raised
8807 * by this calibrator.
8808 * @param method robust estimator method.
8809 * @return a robust gyroscope calibrator.
8810 * @throws IllegalArgumentException if any of the provided values does
8811 * not have proper size, if either
8812 * turntable rotation rate or
8813 * time interval is zero or negative or
8814 * if provided quality scores length is
8815 * smaller than 10 samples.
8816 */
8817 public static RobustTurntableGyroscopeCalibrator create(
8818 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
8819 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8820 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
8821 final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
8822 final RobustEstimatorMethod method) {
8823 return switch (method) {
8824 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8825 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8826 listener);
8827 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8828 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8829 listener);
8830 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8831 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8832 listener);
8833 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8834 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
8835 initialGg, listener);
8836 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8837 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
8838 initialGg, listener);
8839 };
8840 }
8841
8842 /**
8843 * Creates a robust gyroscope calibrator.
8844 *
8845 * @param qualityScores quality scores corresponding to each provided
8846 * measurement. The larger the score value the better
8847 * the quality of the sample.
8848 * @param position position where body kinematics
8849 * measures have been taken.
8850 * @param turntableRotationRate constant rotation rate at which
8851 * the turntable is spinning. Must
8852 * be expressed in radians per
8853 * second (rad/s).
8854 * @param timeInterval time interval between measurements
8855 * being captured expressed in
8856 * seconds (s).
8857 * @param measurements collection of body kinematics
8858 * measurements with standard
8859 * deviations taken at the same
8860 * position with zero velocity
8861 * and unknown different
8862 * orientations.
8863 * @param commonAxisUsed indicates whether z-axis is
8864 * assumed to be common for
8865 * accelerometer and gyroscope.
8866 * @param estimateGDependentCrossBiases true if G-dependent cross biases
8867 * will be estimated, false
8868 * otherwise.
8869 * @param initialBias initial gyroscope bias to be
8870 * used to find a solution. This
8871 * must have length 3 and is
8872 * expressed in radians per second
8873 * (rad/s).
8874 * @param initialMg initial gyroscope scale factors
8875 * and cross coupling errors matrix.
8876 * Must be 3x3.
8877 * @param initialGg initial gyroscope G-dependent
8878 * cross biases introduced on the
8879 * gyroscope by the specific forces
8880 * sensed by the accelerometer.
8881 * Must be 3x3.
8882 * @param method robust estimator method.
8883 * @return a robust gyroscope calibrator.
8884 * @throws IllegalArgumentException if any of the provided values does
8885 * not have proper size, if either
8886 * turntable rotation rate or
8887 * time interval is zero or negative or
8888 * if provided quality scores length is
8889 * smaller than 10 samples.
8890 */
8891 public static RobustTurntableGyroscopeCalibrator create(
8892 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
8893 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8894 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
8895 final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
8896 return switch (method) {
8897 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8898 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
8899 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8900 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
8901 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8902 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
8903 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8904 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
8905 initialGg);
8906 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8907 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
8908 initialGg);
8909 };
8910 }
8911
8912 /**
8913 * Creates a robust gyroscope calibrator.
8914 *
8915 * @param qualityScores quality scores corresponding to each provided
8916 * measurement. The larger the score value the better
8917 * the quality of the sample.
8918 * @param position position where body kinematics
8919 * measures have been taken.
8920 * @param turntableRotationRate constant rotation rate at which
8921 * the turntable is spinning. Must
8922 * be expressed in radians per
8923 * second (rad/s).
8924 * @param timeInterval time interval between measurements
8925 * being captured expressed in
8926 * seconds (s).
8927 * @param measurements collection of body kinematics
8928 * measurements with standard
8929 * deviations taken at the same
8930 * position with zero velocity
8931 * and unknown different
8932 * orientations.
8933 * @param commonAxisUsed indicates whether z-axis is
8934 * assumed to be common for
8935 * accelerometer and gyroscope.
8936 * @param estimateGDependentCrossBiases true if G-dependent cross biases
8937 * will be estimated, false
8938 * otherwise.
8939 * @param initialBias initial gyroscope bias to be
8940 * used to find a solution. This
8941 * must have length 3 and is
8942 * expressed in radians per second
8943 * (rad/s).
8944 * @param initialMg initial gyroscope scale factors
8945 * and cross coupling errors matrix.
8946 * Must be 3x3.
8947 * @param initialGg initial gyroscope G-dependent
8948 * cross biases introduced on the
8949 * gyroscope by the specific forces
8950 * sensed by the accelerometer.
8951 * Must be 3x3.
8952 * @param listener listener to handle events raised
8953 * by this calibrator.
8954 * @param method robust estimator method.
8955 * @return a robust gyroscope calibrator.
8956 * @throws IllegalArgumentException if any of the provided values does
8957 * not have proper size, if either
8958 * turntable rotation rate or
8959 * time interval is zero or negative or
8960 * if provided quality scores length is
8961 * smaller than 10 samples.
8962 */
8963 public static RobustTurntableGyroscopeCalibrator create(
8964 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
8965 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
8966 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
8967 final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
8968 final RobustEstimatorMethod method) {
8969 return switch (method) {
8970 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8971 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8972 listener);
8973 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8974 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8975 listener);
8976 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
8977 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
8978 listener);
8979 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8980 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
8981 initialGg, listener);
8982 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
8983 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
8984 initialGg, listener);
8985 };
8986 }
8987
8988 /**
8989 * Creates a robust gyroscope calibrator.
8990 *
8991 * @param qualityScores quality scores corresponding to each provided
8992 * measurement. The larger the score value the better
8993 * the quality of the sample.
8994 * @param position position where body kinematics
8995 * measures have been taken.
8996 * @param turntableRotationRate constant rotation rate at which
8997 * the turntable is spinning. Must
8998 * be expressed in radians per
8999 * second (rad/s).
9000 * @param timeInterval time interval between measurements
9001 * being captured expressed in
9002 * seconds (s).
9003 * @param measurements collection of body kinematics
9004 * measurements with standard
9005 * deviations taken at the same
9006 * position with zero velocity
9007 * and unknown different
9008 * orientations.
9009 * @param commonAxisUsed indicates whether z-axis is
9010 * assumed to be common for
9011 * accelerometer and gyroscope.
9012 * @param estimateGDependentCrossBiases true if G-dependent cross
9013 * biases will be estimated,
9014 * false otherwise.
9015 * @param initialBias initial gyroscope bias to be
9016 * used to find a solution. This
9017 * must have length 3 and is
9018 * expressed in radians per second
9019 * (rad/s).
9020 * @param initialMg initial gyroscope scale factors
9021 * and cross coupling errors
9022 * matrix. Must be 3x3.
9023 * @param initialGg initial gyroscope G-dependent
9024 * cross biases introduced on the
9025 * gyroscope by the specific forces
9026 * sensed by the accelerometer.
9027 * Must be 3x3.
9028 * @param accelerometerBias known accelerometer bias. This
9029 * must have length 3 and is
9030 * expressed in meters per squared
9031 * second (m/s^2).
9032 * @param accelerometerMa known accelerometer scale factors
9033 * and cross coupling matrix. Must
9034 * be 3x3.
9035 * @param method robust estimator method.
9036 * @return a robust gyroscope calibrator.
9037 * @throws IllegalArgumentException if any of the provided values does
9038 * not have proper size, if either
9039 * turntable rotation rate or
9040 * time interval is zero or negative or
9041 * if provided quality scores length is
9042 * smaller than 10 samples.
9043 */
9044 public static RobustTurntableGyroscopeCalibrator create(
9045 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
9046 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
9047 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
9048 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
9049 final Matrix accelerometerMa, final RobustEstimatorMethod method) {
9050 return switch (method) {
9051 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
9052 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
9053 accelerometerBias, accelerometerMa);
9054 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
9055 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
9056 accelerometerBias, accelerometerMa);
9057 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
9058 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
9059 accelerometerBias, accelerometerMa);
9060 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
9061 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
9062 initialGg, accelerometerBias, accelerometerMa);
9063 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
9064 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
9065 initialGg, accelerometerBias, accelerometerMa);
9066 };
9067 }
9068
9069 /**
9070 * Creates a robust gyroscope calibrator.
9071 *
9072 * @param qualityScores quality scores corresponding to each provided
9073 * measurement. The larger the score value the better
9074 * the quality of the sample.
9075 * @param position position where body kinematics
9076 * measures have been taken.
9077 * @param turntableRotationRate constant rotation rate at which
9078 * the turntable is spinning. Must
9079 * be expressed in radians per
9080 * second (rad/s).
9081 * @param timeInterval time interval between measurements
9082 * being captured expressed in
9083 * seconds (s).
9084 * @param measurements collection of body kinematics
9085 * measurements with standard
9086 * deviations taken at the same
9087 * position with zero velocity
9088 * and unknown different
9089 * orientations.
9090 * @param commonAxisUsed indicates whether z-axis is
9091 * assumed to be common for
9092 * accelerometer and gyroscope.
9093 * @param estimateGDependentCrossBiases true if G-dependent cross
9094 * biases will be estimated,
9095 * false otherwise.
9096 * @param initialBias initial gyroscope bias to be
9097 * used to find a solution. This
9098 * must have length 3 and is
9099 * expressed in radians per second
9100 * (rad/s).
9101 * @param initialMg initial gyroscope scale factors
9102 * and cross coupling errors
9103 * matrix. Must be 3x3.
9104 * @param initialGg initial gyroscope G-dependent
9105 * cross biases introduced on the
9106 * gyroscope by the specific forces
9107 * sensed by the accelerometer.
9108 * Must be 3x3.
9109 * @param accelerometerBias known accelerometer bias. This
9110 * must have length 3 and is
9111 * expressed in meters per squared
9112 * second (m/s^2).
9113 * @param accelerometerMa known accelerometer scale factors
9114 * and cross coupling matrix. Must
9115 * be 3x3.
9116 * @param listener listener to handle events raised
9117 * by this calibrator.
9118 * @param method robust estimator method.
9119 * @return a robust gyroscope calibrator.
9120 * @throws IllegalArgumentException if any of the provided values does
9121 * not have proper size, if either
9122 * turntable rotation rate or
9123 * time interval is zero or negative or
9124 * if provided quality scores length is
9125 * smaller than 10 samples.
9126 */
9127 public static RobustTurntableGyroscopeCalibrator create(
9128 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
9129 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
9130 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
9131 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
9132 final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
9133 final RobustEstimatorMethod method) {
9134 return switch (method) {
9135 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
9136 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
9137 accelerometerBias, accelerometerMa, listener);
9138 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
9139 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
9140 accelerometerBias, accelerometerMa, listener);
9141 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
9142 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
9143 accelerometerBias, accelerometerMa, listener);
9144 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
9145 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
9146 initialGg, accelerometerBias, accelerometerMa, listener);
9147 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
9148 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
9149 initialGg, accelerometerBias, accelerometerMa, listener);
9150 };
9151 }
9152
9153 /**
9154 * Creates a robust gyroscope calibrator.
9155 *
9156 * @param qualityScores quality scores corresponding to each provided
9157 * measurement. The larger the score value the better
9158 * the quality of the sample.
9159 * @param position position where body kinematics
9160 * measures have been taken.
9161 * @param turntableRotationRate constant rotation rate at which
9162 * the turntable is spinning. Must
9163 * be expressed in radians per
9164 * second (rad/s).
9165 * @param timeInterval time interval between measurements
9166 * being captured expressed in
9167 * seconds (s).
9168 * @param measurements collection of body kinematics
9169 * measurements with standard
9170 * deviations taken at the same
9171 * position with zero velocity and
9172 * unknown different orientations.
9173 * @param commonAxisUsed indicates whether z-axis is
9174 * assumed to be common for
9175 * accelerometer and gyroscope.
9176 * @param estimateGDependentCrossBiases true if G-dependent cross biases
9177 * will be estimated, false
9178 * otherwise.
9179 * @param initialBias initial gyroscope bias to be
9180 * used to find a solution. This
9181 * must be 3x1 and is expressed in
9182 * radians per second (rad/s).
9183 * @param initialMg initial gyroscope scale factors
9184 * and cross coupling errors matrix.
9185 * Must be 3x3.
9186 * @param initialGg initial gyroscope G-dependent
9187 * cross biases introduced on the
9188 * gyroscope by the specific forces
9189 * sensed by the accelerometer. Must
9190 * be 3x3.
9191 * @param accelerometerBias known accelerometer bias. This
9192 * must have length 3 and is
9193 * expressed in meters per squared
9194 * second (m/s^2).
9195 * @param accelerometerMa known accelerometer scale factors
9196 * and cross coupling matrix. Must
9197 * be 3x3.
9198 * @param method robust estimator method.
9199 * @return a robust gyroscope calibrator.
9200 * @throws IllegalArgumentException if any of the provided values does
9201 * not have proper size, if either
9202 * turntable rotation rate or
9203 * time interval is zero or negative or
9204 * if provided quality scores length is
9205 * smaller than 10 samples.
9206 */
9207 public static RobustTurntableGyroscopeCalibrator create(
9208 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
9209 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
9210 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
9211 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
9212 final Matrix accelerometerMa, final RobustEstimatorMethod method) {
9213 return switch (method) {
9214 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
9215 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
9216 accelerometerBias, accelerometerMa);
9217 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
9218 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
9219 accelerometerBias, accelerometerMa);
9220 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
9221 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
9222 accelerometerBias, accelerometerMa);
9223 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
9224 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
9225 initialGg, accelerometerBias, accelerometerMa);
9226 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
9227 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
9228 initialGg, accelerometerBias, accelerometerMa);
9229 };
9230 }
9231
9232 /**
9233 * Creates a robust gyroscope calibrator.
9234 *
9235 * @param qualityScores quality scores corresponding to each provided
9236 * measurement. The larger the score value the better
9237 * the quality of the sample.
9238 * @param position position where body kinematics
9239 * measures have been taken.
9240 * @param turntableRotationRate constant rotation rate at which
9241 * the turntable is spinning. Must
9242 * be expressed in radians per
9243 * second (rad/s).
9244 * @param timeInterval time interval between measurements
9245 * being captured expressed in
9246 * seconds (s).
9247 * @param measurements collection of body kinematics
9248 * measurements with standard
9249 * deviations taken at the same
9250 * position with zero velocity and
9251 * unknown different orientations.
9252 * @param commonAxisUsed indicates whether z-axis is
9253 * assumed to be common for
9254 * accelerometer and gyroscope.
9255 * @param estimateGDependentCrossBiases true if G-dependent cross biases
9256 * will be estimated, false
9257 * otherwise.
9258 * @param initialBias initial gyroscope bias to be
9259 * used to find a solution. This
9260 * must be 3x1 and is expressed in
9261 * radians per second (rad/s).
9262 * @param initialMg initial gyroscope scale factors
9263 * and cross coupling errors matrix.
9264 * Must be 3x3.
9265 * @param initialGg initial gyroscope G-dependent
9266 * cross biases introduced on the
9267 * gyroscope by the specific forces
9268 * sensed by the accelerometer. Must
9269 * be 3x3.
9270 * @param accelerometerBias known accelerometer bias. This
9271 * must have length 3 and is
9272 * expressed in meters per squared
9273 * second (m/s^2).
9274 * @param accelerometerMa known accelerometer scale factors
9275 * and cross coupling matrix. Must
9276 * be 3x3.
9277 * @param listener listener to handle events raised
9278 * by this calibrator.
9279 * @param method robust estimator method.
9280 * @return a robust gyroscope calibrator.
9281 * @throws IllegalArgumentException if any of the provided values does
9282 * not have proper size, if either
9283 * turntable rotation rate or
9284 * time interval is zero or negative or
9285 * if provided quality scores length is
9286 * smaller than 10 samples.
9287 */
9288 public static RobustTurntableGyroscopeCalibrator create(
9289 final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
9290 final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
9291 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
9292 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
9293 final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
9294 final RobustEstimatorMethod method) {
9295 return switch (method) {
9296 case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
9297 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
9298 accelerometerBias, accelerometerMa, listener);
9299 case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
9300 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
9301 accelerometerBias, accelerometerMa, listener);
9302 case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
9303 measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
9304 accelerometerBias, accelerometerMa, listener);
9305 case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
9306 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
9307 initialGg, accelerometerBias, accelerometerMa, listener);
9308 default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
9309 timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
9310 initialGg, accelerometerBias, accelerometerMa, listener);
9311 };
9312 }
9313
9314 /**
9315 * Creates a robust gyroscope calibrator using default robust method.
9316 *
9317 * @return a robust gyroscope calibrator.
9318 */
9319 public static RobustTurntableGyroscopeCalibrator create() {
9320 return create(DEFAULT_ROBUST_METHOD);
9321 }
9322
9323 /**
9324 * Creates a robust gyroscope calibrator using default robust method.
9325 *
9326 * @param position position where body kinematics measures
9327 * have been taken.
9328 * @param turntableRotationRate constant rotation rate at which the
9329 * turntable is spinning. Must be
9330 * expressed in radians per second (rad/s).
9331 * @param timeInterval time interval between measurements being
9332 * captured expressed in seconds (s).
9333 * @param measurements collection of body kinematics
9334 * measurements with standard deviations
9335 * taken at the same position with zero
9336 * velocity and unknown different
9337 * orientations.
9338 * @param initialBias initial gyroscope bias to be used to
9339 * find a solution. This must be 3x1 and
9340 * is expressed in radians per second
9341 * (rad/s).
9342 * @param initialMg initial gyroscope scale factors and
9343 * cross coupling errors matrix. Must
9344 * be 3x3.
9345 * @param initialGg initial gyroscope G-dependent cross
9346 * biases introduced on the gyroscope by
9347 * the specific forces sensed by the
9348 * accelerometer. Must be 3x3.
9349 * @return a robust gyroscope calibrator.
9350 * @throws IllegalArgumentException if any of the provided values does
9351 * not have proper size or if either
9352 * turntable rotation rate or
9353 * time interval is zero or negative.
9354 */
9355 public static RobustTurntableGyroscopeCalibrator create(
9356 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
9357 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
9358 final Matrix initialGg) {
9359 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
9360 DEFAULT_ROBUST_METHOD);
9361 }
9362
9363 /**
9364 * Creates a robust gyroscope calibrator using default robust method.
9365 *
9366 * @param position position where body kinematics measures
9367 * have been taken.
9368 * @param turntableRotationRate constant rotation rate at which the
9369 * turntable is spinning. Must be
9370 * expressed in radians per second (rad/s).
9371 * @param timeInterval time interval between measurements being
9372 * captured expressed in seconds (s).
9373 * @param measurements collection of body kinematics
9374 * measurements with standard deviations
9375 * taken at the same position with zero
9376 * velocity and unknown different
9377 * orientations.
9378 * @param initialBias initial gyroscope bias to be used to
9379 * find a solution. This must be 3x1 and
9380 * is expressed in radians per second
9381 * (rad/s).
9382 * @param initialMg initial gyroscope scale factors and
9383 * cross coupling errors matrix. Must
9384 * be 3x3.
9385 * @param initialGg initial gyroscope G-dependent cross
9386 * biases introduced on the gyroscope by
9387 * the specific forces sensed by the
9388 * accelerometer. Must be 3x3.
9389 * @param listener listener to handle events raised by this
9390 * calibrator.
9391 * @return a robust gyroscope calibrator.
9392 * @throws IllegalArgumentException if any of the provided values does
9393 * not have proper size or if either
9394 * turntable rotation rate or
9395 * time interval is zero or negative.
9396 */
9397 public static RobustTurntableGyroscopeCalibrator create(
9398 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
9399 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
9400 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
9401 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
9402 listener, DEFAULT_ROBUST_METHOD);
9403 }
9404
9405 /**
9406 * Creates a robust gyroscope calibrator using default robust method.
9407 *
9408 * @param position position where body kinematics measures
9409 * have been taken.
9410 * @param turntableRotationRate constant rotation rate at which the
9411 * turntable is spinning. Must be
9412 * expressed in radians per second (rad/s).
9413 * @param timeInterval time interval between measurements being
9414 * captured expressed in seconds (s).
9415 * @param measurements collection of body kinematics
9416 * measurements with standard deviations
9417 * taken at the same position with zero
9418 * velocity and unknown different
9419 * orientations.
9420 * @param initialBias initial gyroscope bias to be used to
9421 * find a solution. This must have
9422 * length 3 and is expressed in radians
9423 * per second (rad/s).
9424 * @param initialMg initial gyroscope scale factors and
9425 * cross coupling errors matrix. Must
9426 * be 3x3.
9427 * @param initialGg initial gyroscope G-dependent cross
9428 * biases introduced on the gyroscope by
9429 * the specific forces sensed by the
9430 * accelerometer. Must be 3x3.
9431 * @return a robust gyroscope calibrator.
9432 * @throws IllegalArgumentException if any of the provided values does
9433 * not have proper size or if either
9434 * turntable rotation rate or
9435 * time interval is zero or negative.
9436 */
9437 public static RobustTurntableGyroscopeCalibrator create(
9438 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
9439 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
9440 final Matrix initialMg, final Matrix initialGg) {
9441 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
9442 DEFAULT_ROBUST_METHOD);
9443 }
9444
9445 /**
9446 * Creates a robust gyroscope calibrator using default robust method.
9447 *
9448 * @param position position where body kinematics measures
9449 * have been taken.
9450 * @param turntableRotationRate constant rotation rate at which the
9451 * turntable is spinning. Must be
9452 * expressed in radians per second (rad/s).
9453 * @param timeInterval time interval between measurements being
9454 * captured expressed in seconds (s).
9455 * @param measurements collection of body kinematics
9456 * measurements with standard deviations
9457 * taken at the same position with zero
9458 * velocity and unknown different
9459 * orientations.
9460 * @param initialBias initial gyroscope bias to be used to
9461 * find a solution. This must have
9462 * length 3 and is expressed in radians
9463 * per second (rad/s).
9464 * @param initialMg initial gyroscope scale factors and
9465 * cross coupling errors matrix. Must
9466 * be 3x3.
9467 * @param initialGg initial gyroscope G-dependent cross
9468 * biases introduced on the gyroscope by
9469 * the specific forces sensed by the
9470 * accelerometer. Must be 3x3.
9471 * @param listener listener to handle events raised by
9472 * this calibrator.
9473 * @return a robust gyroscope calibrator.
9474 * @throws IllegalArgumentException if any of the provided values does
9475 * not have proper size or if either
9476 * turntable rotation rate or
9477 * time interval is zero or negative.
9478 */
9479 public static RobustTurntableGyroscopeCalibrator create(
9480 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
9481 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
9482 final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
9483 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
9484 listener, DEFAULT_ROBUST_METHOD);
9485 }
9486
9487 /**
9488 * Creates a robust gyroscope calibrator using default robust method.
9489 *
9490 * @param position position where body kinematics measures
9491 * have been taken.
9492 * @param turntableRotationRate constant rotation rate at which the
9493 * turntable is spinning. Must be
9494 * expressed in radians per second (rad/s).
9495 * @param timeInterval time interval between measurements being
9496 * captured expressed in seconds (s).
9497 * @param measurements collection of body kinematics
9498 * measurements with standard deviations
9499 * taken at the same position with zero
9500 * velocity and unknown different
9501 * orientations.
9502 * @param initialBias initial gyroscope bias to be used to
9503 * find a solution. This must have length
9504 * 3 and is expressed in radians per
9505 * second (rad/s).
9506 * @param initialMg initial gyroscope scale factors and
9507 * cross coupling errors matrix. Must
9508 * be 3x3.
9509 * @param initialGg initial gyroscope G-dependent cross
9510 * biases introduced on the gyroscope by
9511 * the specific forces sensed by the
9512 * accelerometer. Must be 3x3.
9513 * @param accelerometerBias known accelerometer bias. This must
9514 * have length 3 and is expressed in
9515 * meters per squared second
9516 * (m/s^2).
9517 * @param accelerometerMa known accelerometer scale factors and
9518 * cross coupling matrix. Must be 3x3.
9519 * @return a robust gyroscope calibrator.
9520 * @throws IllegalArgumentException if any of the provided values does
9521 * not have proper size or if either
9522 * turntable rotation rate or
9523 * time interval is zero or negative.
9524 */
9525 public static RobustTurntableGyroscopeCalibrator create(
9526 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
9527 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
9528 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
9529 final Matrix accelerometerMa) {
9530 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
9531 accelerometerBias, accelerometerMa, DEFAULT_ROBUST_METHOD);
9532 }
9533
9534 /**
9535 * Creates a robust gyroscope calibrator using default robust method.
9536 *
9537 * @param position position where body kinematics measures
9538 * have been taken.
9539 * @param turntableRotationRate constant rotation rate at which the
9540 * turntable is spinning. Must be
9541 * expressed in radians per second (rad/s).
9542 * @param timeInterval time interval between measurements being
9543 * captured expressed in seconds (s).
9544 * @param measurements collection of body kinematics
9545 * measurements with standard deviations
9546 * taken at the same position with zero
9547 * velocity and unknown different
9548 * orientations.
9549 * @param initialBias initial gyroscope bias to be used to
9550 * find a solution. This must have length
9551 * 3 and is expressed in radians per
9552 * second (rad/s).
9553 * @param initialMg initial gyroscope scale factors and
9554 * cross coupling errors matrix. Must
9555 * be 3x3.
9556 * @param initialGg initial gyroscope G-dependent cross
9557 * biases introduced on the gyroscope by
9558 * the specific forces sensed by the
9559 * accelerometer. Must be 3x3.
9560 * @param accelerometerBias known accelerometer bias. This must
9561 * have length 3 and is expressed in
9562 * meters per squared second
9563 * (m/s^2).
9564 * @param accelerometerMa known accelerometer scale factors and
9565 * cross coupling matrix. Must be 3x3.
9566 * @param listener listener to handle events raised by
9567 * this calibrator.
9568 * @return a robust gyroscope calibrator.
9569 * @throws IllegalArgumentException if any of the provided values does
9570 * not have proper size or if either
9571 * turntable rotation rate or
9572 * time interval is zero or negative.
9573 */
9574 public static RobustTurntableGyroscopeCalibrator create(
9575 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
9576 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
9577 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
9578 final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener) {
9579 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
9580 accelerometerBias, accelerometerMa, listener, DEFAULT_ROBUST_METHOD);
9581 }
9582
9583 /**
9584 * Creates a robust gyroscope calibrator using default robust method.
9585 *
9586 * @param position position where body kinematics measures
9587 * have been taken.
9588 * @param turntableRotationRate constant rotation rate at which the
9589 * turntable is spinning. Must be
9590 * expressed in radians per second (rad/s).
9591 * @param timeInterval time interval between measurements being
9592 * captured expressed in seconds (s).
9593 * @param measurements collection of body kinematics
9594 * measurements with standard deviations
9595 * taken at the same position with zero
9596 * velocity and unknown different
9597 * orientations.
9598 * @param initialBias initial gyroscope bias to be used to
9599 * find a solution. This must be 3x1 and
9600 * is expressed in radians per second
9601 * (rad/s).
9602 * @param initialMg initial gyroscope scale factors and
9603 * cross coupling errors matrix. Must
9604 * be 3x3.
9605 * @param initialGg initial gyroscope G-dependent cross
9606 * biases introduced on the gyroscope by
9607 * the specific forces sensed by the
9608 * accelerometer. Must be 3x3.
9609 * @param accelerometerBias known accelerometer bias. This must
9610 * have length 3 and is expressed in
9611 * meters per squared second
9612 * (m/s^2).
9613 * @param accelerometerMa known accelerometer scale factors and
9614 * cross coupling matrix. Must be 3x3.
9615 * @return a robust gyroscope calibrator.
9616 * @throws IllegalArgumentException if any of the provided values does
9617 * not have proper size or if either
9618 * turntable rotation rate or
9619 * time interval is zero or negative.
9620 */
9621 public static RobustTurntableGyroscopeCalibrator create(
9622 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
9623 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
9624 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
9625 final Matrix accelerometerMa) {
9626 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
9627 accelerometerBias, accelerometerMa, DEFAULT_ROBUST_METHOD);
9628 }
9629
9630 /**
9631 * Creates a robust gyroscope calibrator using default robust method.
9632 *
9633 * @param position position where body kinematics measures
9634 * have been taken.
9635 * @param turntableRotationRate constant rotation rate at which the
9636 * turntable is spinning. Must be
9637 * expressed in radians per second (rad/s).
9638 * @param timeInterval time interval between measurements being
9639 * captured expressed in seconds (s).
9640 * @param measurements collection of body kinematics
9641 * measurements with standard deviations
9642 * taken at the same position with zero
9643 * velocity and unknown different
9644 * orientations.
9645 * @param initialBias initial gyroscope bias to be used to
9646 * find a solution. This must be 3x1 and
9647 * is expressed in radians per second
9648 * (rad/s).
9649 * @param initialMg initial gyroscope scale factors and
9650 * cross coupling errors matrix. Must
9651 * be 3x3.
9652 * @param initialGg initial gyroscope G-dependent cross
9653 * biases introduced on the gyroscope by
9654 * the specific forces sensed by the
9655 * accelerometer. Must be 3x3.
9656 * @param accelerometerBias known accelerometer bias. This must
9657 * have length 3 and is expressed in
9658 * meters per squared second
9659 * (m/s^2).
9660 * @param accelerometerMa known accelerometer scale factors and
9661 * cross coupling matrix. Must be 3x3.
9662 * @param listener listener to handle events raised by
9663 * this calibrator.
9664 * @return a robust gyroscope calibrator.
9665 * @throws IllegalArgumentException if any of the provided values does
9666 * not have proper size or if either
9667 * turntable rotation rate or
9668 * time interval is zero or negative.
9669 */
9670 public static RobustTurntableGyroscopeCalibrator create(
9671 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
9672 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
9673 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
9674 final RobustTurntableGyroscopeCalibratorListener listener) {
9675 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
9676 accelerometerBias, accelerometerMa, listener, DEFAULT_ROBUST_METHOD);
9677 }
9678
9679 /**
9680 * Creates a robust gyroscope calibrator using default robust method.
9681 *
9682 * @param position position where body kinematics
9683 * measures have been taken.
9684 * @param turntableRotationRate constant rotation rate at which
9685 * the turntable is spinning. Must
9686 * be expressed in radians per
9687 * second (rad/s).
9688 * @param timeInterval time interval between measurements
9689 * being captured expressed in
9690 * seconds (s).
9691 * @param measurements collection of body kinematics
9692 * measurements with standard
9693 * deviations taken at the same
9694 * position with zero velocity
9695 * and unknown different
9696 * orientations.
9697 * @param commonAxisUsed indicates whether z-axis is
9698 * assumed to be common for
9699 * accelerometer and gyroscope.
9700 * @param estimateGDependentCrossBiases true if G-dependent cross biases
9701 * will be estimated, false
9702 * otherwise.
9703 * @param initialBias initial gyroscope bias to be
9704 * used to find a solution. This
9705 * must be 3x1 and is expressed in
9706 * radians per second (rad/s).
9707 * @param initialMg initial gyroscope scale factors
9708 * and cross coupling errors matrix.
9709 * Must be 3x3.
9710 * @param initialGg initial gyroscope G-dependent
9711 * cross biases introduced on the
9712 * gyroscope by the specific
9713 * forces sensed by the
9714 * accelerometer. Must be 3x3.
9715 * @return a robust gyroscope calibrator.
9716 * @throws IllegalArgumentException if any of the provided values does
9717 * not have proper size or if either
9718 * turntable rotation rate or
9719 * time interval is zero or negative.
9720 */
9721 public static RobustTurntableGyroscopeCalibrator create(
9722 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
9723 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
9724 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
9725 final Matrix initialGg) {
9726 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
9727 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, DEFAULT_ROBUST_METHOD);
9728 }
9729
9730 /**
9731 * Creates a robust gyroscope calibrator using default robust method.
9732 *
9733 * @param position position where body kinematics
9734 * measures have been taken.
9735 * @param turntableRotationRate constant rotation rate at which
9736 * the turntable is spinning. Must
9737 * be expressed in radians per
9738 * second (rad/s).
9739 * @param timeInterval time interval between measurements
9740 * being captured expressed in
9741 * seconds (s).
9742 * @param measurements collection of body kinematics
9743 * measurements with standard
9744 * deviations taken at the same
9745 * position with zero velocity
9746 * and unknown different
9747 * orientations.
9748 * @param commonAxisUsed indicates whether z-axis is
9749 * assumed to be common for
9750 * accelerometer and gyroscope.
9751 * @param estimateGDependentCrossBiases true if G-dependent cross biases
9752 * will be estimated, false
9753 * otherwise.
9754 * @param initialBias initial gyroscope bias to be
9755 * used to find a solution. This
9756 * must be 3x1 and is expressed in
9757 * radians per second (rad/s).
9758 * @param initialMg initial gyroscope scale factors
9759 * and cross coupling errors matrix.
9760 * Must be 3x3.
9761 * @param initialGg initial gyroscope G-dependent
9762 * cross biases introduced on the
9763 * gyroscope by the specific
9764 * forces sensed by the
9765 * accelerometer. Must be 3x3.
9766 * @param listener listener to handle events raised by
9767 * this calibrator.
9768 * @return a robust gyroscope calibrator.
9769 * @throws IllegalArgumentException if any of the provided values does
9770 * not have proper size or if either
9771 * turntable rotation rate or
9772 * time interval is zero or negative.
9773 */
9774 public static RobustTurntableGyroscopeCalibrator create(
9775 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
9776 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
9777 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
9778 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
9779 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
9780 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener, DEFAULT_ROBUST_METHOD);
9781 }
9782
9783 /**
9784 * Creates a robust gyroscope calibrator using default robust method.
9785 *
9786 * @param position position where body kinematics
9787 * measures have been taken.
9788 * @param turntableRotationRate constant rotation rate at which
9789 * the turntable is spinning. Must
9790 * be expressed in radians per
9791 * second (rad/s).
9792 * @param timeInterval time interval between measurements
9793 * being captured expressed in
9794 * seconds (s).
9795 * @param measurements collection of body kinematics
9796 * measurements with standard
9797 * deviations taken at the same
9798 * position with zero velocity
9799 * and unknown different
9800 * orientations.
9801 * @param commonAxisUsed indicates whether z-axis is
9802 * assumed to be common for
9803 * accelerometer and gyroscope.
9804 * @param estimateGDependentCrossBiases true if G-dependent cross biases
9805 * will be estimated, false
9806 * otherwise.
9807 * @param initialBias initial gyroscope bias to be
9808 * used to find a solution. This
9809 * must have length 3 and is
9810 * expressed in radians per second
9811 * (rad/s).
9812 * @param initialMg initial gyroscope scale factors
9813 * and cross coupling errors matrix.
9814 * Must be 3x3.
9815 * @param initialGg initial gyroscope G-dependent
9816 * cross biases introduced on the
9817 * gyroscope by the specific forces
9818 * sensed by the accelerometer.
9819 * Must be 3x3.
9820 * @return a robust gyroscope calibrator.
9821 * @throws IllegalArgumentException if any of the provided values does
9822 * not have proper size or if either
9823 * turntable rotation rate or
9824 * time interval is zero or negative.
9825 */
9826 public static RobustTurntableGyroscopeCalibrator create(
9827 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
9828 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
9829 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
9830 final Matrix initialGg) {
9831 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
9832 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, DEFAULT_ROBUST_METHOD);
9833 }
9834
9835 /**
9836 * Creates a robust gyroscope calibrator using default robust method.
9837 *
9838 * @param position position where body kinematics
9839 * measures have been taken.
9840 * @param turntableRotationRate constant rotation rate at which
9841 * the turntable is spinning. Must
9842 * be expressed in radians per
9843 * second (rad/s).
9844 * @param timeInterval time interval between measurements
9845 * being captured expressed in
9846 * seconds (s).
9847 * @param measurements collection of body kinematics
9848 * measurements with standard
9849 * deviations taken at the same
9850 * position with zero velocity
9851 * and unknown different
9852 * orientations.
9853 * @param commonAxisUsed indicates whether z-axis is
9854 * assumed to be common for
9855 * accelerometer and gyroscope.
9856 * @param estimateGDependentCrossBiases true if G-dependent cross biases
9857 * will be estimated, false
9858 * otherwise.
9859 * @param initialBias initial gyroscope bias to be
9860 * used to find a solution. This
9861 * must have length 3 and is
9862 * expressed in radians per second
9863 * (rad/s).
9864 * @param initialMg initial gyroscope scale factors
9865 * and cross coupling errors matrix.
9866 * Must be 3x3.
9867 * @param initialGg initial gyroscope G-dependent
9868 * cross biases introduced on the
9869 * gyroscope by the specific forces
9870 * sensed by the accelerometer.
9871 * Must be 3x3.
9872 * @param listener listener to handle events raised
9873 * by this calibrator.
9874 * @return a robust gyroscope calibrator.
9875 * @throws IllegalArgumentException if any of the provided values does
9876 * not have proper size or if either
9877 * turntable rotation rate or
9878 * time interval is zero or negative.
9879 */
9880 public static RobustTurntableGyroscopeCalibrator create(
9881 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
9882 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
9883 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
9884 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
9885 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
9886 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener, DEFAULT_ROBUST_METHOD);
9887 }
9888
9889 /**
9890 * Creates a robust gyroscope calibrator using default robust method.
9891 *
9892 * @param position position where body kinematics
9893 * measures have been taken.
9894 * @param turntableRotationRate constant rotation rate at which
9895 * the turntable is spinning. Must
9896 * be expressed in radians per
9897 * second (rad/s).
9898 * @param timeInterval time interval between measurements
9899 * being captured expressed in
9900 * seconds (s).
9901 * @param measurements collection of body kinematics
9902 * measurements with standard
9903 * deviations taken at the same
9904 * position with zero velocity
9905 * and unknown different
9906 * orientations.
9907 * @param commonAxisUsed indicates whether z-axis is
9908 * assumed to be common for
9909 * accelerometer and gyroscope.
9910 * @param estimateGDependentCrossBiases true if G-dependent cross
9911 * biases will be estimated,
9912 * false otherwise.
9913 * @param initialBias initial gyroscope bias to be
9914 * used to find a solution. This
9915 * must have length 3 and is
9916 * expressed in radians per second
9917 * (rad/s).
9918 * @param initialMg initial gyroscope scale factors
9919 * and cross coupling errors
9920 * matrix. Must be 3x3.
9921 * @param initialGg initial gyroscope G-dependent
9922 * cross biases introduced on the
9923 * gyroscope by the specific forces
9924 * sensed by the accelerometer.
9925 * Must be 3x3.
9926 * @param accelerometerBias known accelerometer bias. This
9927 * must have length 3 and is
9928 * expressed in meters per squared
9929 * second (m/s^2).
9930 * @param accelerometerMa known accelerometer scale factors
9931 * and cross coupling matrix. Must
9932 * be 3x3.
9933 * @return a robust gyroscope calibrator.
9934 * @throws IllegalArgumentException if any of the provided values does
9935 * not have proper size or if either
9936 * turntable rotation rate or
9937 * time interval is zero or negative.
9938 */
9939 public static RobustTurntableGyroscopeCalibrator create(
9940 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
9941 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
9942 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
9943 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa) {
9944 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
9945 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
9946 DEFAULT_ROBUST_METHOD);
9947 }
9948
9949 /**
9950 * Creates a robust gyroscope calibrator using default robust method.
9951 *
9952 * @param position position where body kinematics
9953 * measures have been taken.
9954 * @param turntableRotationRate constant rotation rate at which
9955 * the turntable is spinning. Must
9956 * be expressed in radians per
9957 * second (rad/s).
9958 * @param timeInterval time interval between measurements
9959 * being captured expressed in
9960 * seconds (s).
9961 * @param measurements collection of body kinematics
9962 * measurements with standard
9963 * deviations taken at the same
9964 * position with zero velocity
9965 * and unknown different
9966 * orientations.
9967 * @param commonAxisUsed indicates whether z-axis is
9968 * assumed to be common for
9969 * accelerometer and gyroscope.
9970 * @param estimateGDependentCrossBiases true if G-dependent cross
9971 * biases will be estimated,
9972 * false otherwise.
9973 * @param initialBias initial gyroscope bias to be
9974 * used to find a solution. This
9975 * must have length 3 and is
9976 * expressed in radians per second
9977 * (rad/s).
9978 * @param initialMg initial gyroscope scale factors
9979 * and cross coupling errors
9980 * matrix. Must be 3x3.
9981 * @param initialGg initial gyroscope G-dependent
9982 * cross biases introduced on the
9983 * gyroscope by the specific forces
9984 * sensed by the accelerometer.
9985 * Must be 3x3.
9986 * @param accelerometerBias known accelerometer bias. This
9987 * must have length 3 and is
9988 * expressed in meters per squared
9989 * second (m/s^2).
9990 * @param accelerometerMa known accelerometer scale factors
9991 * and cross coupling matrix. Must
9992 * be 3x3.
9993 * @param listener listener to handle events raised
9994 * by this calibrator.
9995 * @return a robust gyroscope calibrator.
9996 * @throws IllegalArgumentException if any of the provided values does
9997 * not have proper size or if either
9998 * turntable rotation rate or
9999 * time interval is zero or negative.
10000 */
10001 public static RobustTurntableGyroscopeCalibrator create(
10002 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
10003 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
10004 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
10005 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
10006 final RobustTurntableGyroscopeCalibratorListener listener) {
10007 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
10008 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
10009 listener, DEFAULT_ROBUST_METHOD);
10010 }
10011
10012 /**
10013 * Creates a robust gyroscope calibrator using default robust method.
10014 *
10015 * @param position position where body kinematics
10016 * measures have been taken.
10017 * @param turntableRotationRate constant rotation rate at which
10018 * the turntable is spinning. Must
10019 * be expressed in radians per
10020 * second (rad/s).
10021 * @param timeInterval time interval between measurements
10022 * being captured expressed in
10023 * seconds (s).
10024 * @param measurements collection of body kinematics
10025 * measurements with standard
10026 * deviations taken at the same
10027 * position with zero velocity and
10028 * unknown different orientations.
10029 * @param commonAxisUsed indicates whether z-axis is
10030 * assumed to be common for
10031 * accelerometer and gyroscope.
10032 * @param estimateGDependentCrossBiases true if G-dependent cross biases
10033 * will be estimated, false
10034 * otherwise.
10035 * @param initialBias initial gyroscope bias to be
10036 * used to find a solution. This
10037 * must be 3x1 and is expressed in
10038 * radians per second (rad/s).
10039 * @param initialMg initial gyroscope scale factors
10040 * and cross coupling errors matrix.
10041 * Must be 3x3.
10042 * @param initialGg initial gyroscope G-dependent
10043 * cross biases introduced on the
10044 * gyroscope by the specific forces
10045 * sensed by the accelerometer. Must
10046 * be 3x3.
10047 * @param accelerometerBias known accelerometer bias. This
10048 * must have length 3 and is
10049 * expressed in meters per squared
10050 * second (m/s^2).
10051 * @param accelerometerMa known accelerometer scale factors
10052 * and cross coupling matrix. Must
10053 * be 3x3.
10054 * @return a robust gyroscope calibrator.
10055 * @throws IllegalArgumentException if any of the provided values does
10056 * not have proper size or if either
10057 * turntable rotation rate or
10058 * time interval is zero or negative.
10059 */
10060 public static RobustTurntableGyroscopeCalibrator create(
10061 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
10062 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
10063 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
10064 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa) {
10065 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
10066 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
10067 DEFAULT_ROBUST_METHOD);
10068 }
10069
10070 /**
10071 * Creates a robust gyroscope calibrator using default robust method.
10072 *
10073 * @param position position where body kinematics
10074 * measures have been taken.
10075 * @param turntableRotationRate constant rotation rate at which
10076 * the turntable is spinning. Must
10077 * be expressed in radians per
10078 * second (rad/s).
10079 * @param timeInterval time interval between measurements
10080 * being captured expressed in
10081 * seconds (s).
10082 * @param measurements collection of body kinematics
10083 * measurements with standard
10084 * deviations taken at the same
10085 * position with zero velocity and
10086 * unknown different orientations.
10087 * @param commonAxisUsed indicates whether z-axis is
10088 * assumed to be common for
10089 * accelerometer and gyroscope.
10090 * @param estimateGDependentCrossBiases true if G-dependent cross biases
10091 * will be estimated, false
10092 * otherwise.
10093 * @param initialBias initial gyroscope bias to be
10094 * used to find a solution. This
10095 * must be 3x1 and is expressed in
10096 * radians per second (rad/s).
10097 * @param initialMg initial gyroscope scale factors
10098 * and cross coupling errors matrix.
10099 * Must be 3x3.
10100 * @param initialGg initial gyroscope G-dependent
10101 * cross biases introduced on the
10102 * gyroscope by the specific forces
10103 * sensed by the accelerometer. Must
10104 * be 3x3.
10105 * @param accelerometerBias known accelerometer bias. This
10106 * must have length 3 and is
10107 * expressed in meters per squared
10108 * second (m/s^2).
10109 * @param accelerometerMa known accelerometer scale factors
10110 * and cross coupling matrix. Must
10111 * be 3x3.
10112 * @param listener listener to handle events raised
10113 * by this calibrator.
10114 * @return a robust gyroscope calibrator.
10115 * @throws IllegalArgumentException if any of the provided values does
10116 * not have proper size or if either
10117 * turntable rotation rate or
10118 * time interval is zero or negative.
10119 */
10120 public static RobustTurntableGyroscopeCalibrator create(
10121 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
10122 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
10123 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
10124 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
10125 final RobustTurntableGyroscopeCalibratorListener listener) {
10126 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
10127 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
10128 listener, DEFAULT_ROBUST_METHOD);
10129 }
10130
10131 /**
10132 * Creates a robust gyroscope calibrator using default robust method.
10133 *
10134 * @param position position where body kinematics measures
10135 * have been taken.
10136 * @param turntableRotationRate constant rotation rate at which the
10137 * turntable is spinning. Must be
10138 * expressed in radians per second (rad/s).
10139 * @param timeInterval time interval between measurements being
10140 * captured expressed in seconds (s).
10141 * @param measurements collection of body kinematics
10142 * measurements with standard deviations
10143 * taken at the same position with zero
10144 * velocity and unknown different
10145 * orientations.
10146 * @param initialBias initial gyroscope bias to be used to
10147 * find a solution. This must be 3x1 and
10148 * is expressed in radians per second
10149 * (rad/s).
10150 * @param initialMg initial gyroscope scale factors and
10151 * cross coupling errors matrix. Must
10152 * be 3x3.
10153 * @param initialGg initial gyroscope G-dependent cross
10154 * biases introduced on the gyroscope by
10155 * the specific forces sensed by the
10156 * accelerometer. Must be 3x3.
10157 * @return a robust gyroscope calibrator.
10158 * @throws IllegalArgumentException if any of the provided values does
10159 * not have proper size or if either
10160 * turntable rotation rate or
10161 * time interval is zero or negative.
10162 */
10163 public static RobustTurntableGyroscopeCalibrator create(
10164 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10165 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
10166 final Matrix initialGg) {
10167 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
10168 DEFAULT_ROBUST_METHOD);
10169 }
10170
10171 /**
10172 * Creates a robust gyroscope calibrator using default robust method.
10173 *
10174 * @param position position where body kinematics measures
10175 * have been taken.
10176 * @param turntableRotationRate constant rotation rate at which the
10177 * turntable is spinning. Must be
10178 * expressed in radians per second (rad/s).
10179 * @param timeInterval time interval between measurements being
10180 * captured expressed in seconds (s).
10181 * @param measurements collection of body kinematics
10182 * measurements with standard deviations
10183 * taken at the same position with zero
10184 * velocity and unknown different
10185 * orientations.
10186 * @param initialBias initial gyroscope bias to be used to
10187 * find a solution. This must be 3x1 and
10188 * is expressed in radians per second
10189 * (rad/s).
10190 * @param initialMg initial gyroscope scale factors and
10191 * cross coupling errors matrix. Must
10192 * be 3x3.
10193 * @param initialGg initial gyroscope G-dependent cross
10194 * biases introduced on the gyroscope by
10195 * the specific forces sensed by the
10196 * accelerometer. Must be 3x3.
10197 * @param listener listener to handle events raised
10198 * by this calibrator.
10199 * @return a robust gyroscope calibrator.
10200 * @throws IllegalArgumentException if any of the provided values does
10201 * not have proper size or if either
10202 * turntable rotation rate or
10203 * time interval is zero or negative.
10204 */
10205 public static RobustTurntableGyroscopeCalibrator create(
10206 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10207 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
10208 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
10209 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
10210 listener, DEFAULT_ROBUST_METHOD);
10211 }
10212
10213 /**
10214 * Creates a robust gyroscope calibrator using default robust method.
10215 *
10216 * @param position position where body kinematics measures
10217 * have been taken.
10218 * @param turntableRotationRate constant rotation rate at which the
10219 * turntable is spinning. Must be
10220 * expressed in radians per second (rad/s).
10221 * @param timeInterval time interval between measurements being
10222 * captured expressed in seconds (s).
10223 * @param measurements collection of body kinematics
10224 * measurements with standard deviations
10225 * taken at the same position with zero
10226 * velocity and unknown different
10227 * orientations.
10228 * @param initialBias initial gyroscope bias to be used to
10229 * find a solution. This must have
10230 * length 3 and is expressed in radians
10231 * per second (rad/s).
10232 * @param initialMg initial gyroscope scale factors and
10233 * cross coupling errors matrix. Must
10234 * be 3x3.
10235 * @param initialGg initial gyroscope G-dependent cross
10236 * biases introduced on the gyroscope by
10237 * the specific forces sensed by the
10238 * accelerometer. Must be 3x3.
10239 * @return a robust gyroscope calibrator.
10240 * @throws IllegalArgumentException if any of the provided values does
10241 * not have proper size or if either
10242 * turntable rotation rate or
10243 * time interval is zero or negative.
10244 */
10245 public static RobustTurntableGyroscopeCalibrator create(
10246 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10247 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
10248 final Matrix initialMg, final Matrix initialGg) {
10249 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
10250 DEFAULT_ROBUST_METHOD);
10251 }
10252
10253 /**
10254 * Creates a robust gyroscope calibrator using default robust method.
10255 *
10256 * @param position position where body kinematics measures
10257 * have been taken.
10258 * @param turntableRotationRate constant rotation rate at which the
10259 * turntable is spinning. Must be
10260 * expressed in radians per second (rad/s).
10261 * @param timeInterval time interval between measurements being
10262 * captured expressed in seconds (s).
10263 * @param measurements collection of body kinematics
10264 * measurements with standard deviations
10265 * taken at the same position with zero
10266 * velocity and unknown different
10267 * orientations.
10268 * @param initialBias initial gyroscope bias to be used to
10269 * find a solution. This must have
10270 * length 3 and is expressed in radians
10271 * per second (rad/s).
10272 * @param initialMg initial gyroscope scale factors and
10273 * cross coupling errors matrix. Must
10274 * be 3x3.
10275 * @param initialGg initial gyroscope G-dependent cross
10276 * biases introduced on the gyroscope by
10277 * the specific forces sensed by the
10278 * accelerometer. Must be 3x3.
10279 * @param listener listener to handle events raised
10280 * by this calibrator.
10281 * @return a robust gyroscope calibrator.
10282 * @throws IllegalArgumentException if any of the provided values does
10283 * not have proper size or if either
10284 * turntable rotation rate or
10285 * time interval is zero or negative.
10286 */
10287 public static RobustTurntableGyroscopeCalibrator create(
10288 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10289 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
10290 final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
10291 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
10292 listener, DEFAULT_ROBUST_METHOD);
10293 }
10294
10295 /**
10296 * Creates a robust gyroscope calibrator using default robust method.
10297 *
10298 * @param position position where body kinematics measures
10299 * have been taken.
10300 * @param turntableRotationRate constant rotation rate at which the
10301 * turntable is spinning. Must be
10302 * expressed in radians per second (rad/s).
10303 * @param timeInterval time interval between measurements being
10304 * captured expressed in seconds (s).
10305 * @param measurements collection of body kinematics
10306 * measurements with standard deviations
10307 * taken at the same position with zero
10308 * velocity and unknown different
10309 * orientations.
10310 * @param initialBias initial gyroscope bias to be used to
10311 * find a solution. This must have length
10312 * 3 and is expressed in radians per
10313 * second (rad/s).
10314 * @param initialMg initial gyroscope scale factors and
10315 * cross coupling errors matrix. Must
10316 * be 3x3.
10317 * @param initialGg initial gyroscope G-dependent cross
10318 * biases introduced on the gyroscope by
10319 * the specific forces sensed by the
10320 * accelerometer. Must be 3x3.
10321 * @param accelerometerBias known accelerometer bias. This must
10322 * have length 3 and is expressed in
10323 * meters per squared second
10324 * (m/s^2).
10325 * @param accelerometerMa known accelerometer scale factors and
10326 * cross coupling matrix. Must be 3x3.
10327 * @return a robust gyroscope calibrator.
10328 * @throws IllegalArgumentException if any of the provided values does
10329 * not have proper size or if either
10330 * turntable rotation rate or
10331 * time interval is zero or negative.
10332 */
10333 public static RobustTurntableGyroscopeCalibrator create(
10334 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10335 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
10336 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
10337 final Matrix accelerometerMa) {
10338 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
10339 accelerometerBias, accelerometerMa, DEFAULT_ROBUST_METHOD);
10340 }
10341
10342 /**
10343 * Creates a robust gyroscope calibrator using default robust method.
10344 *
10345 * @param position position where body kinematics measures
10346 * have been taken.
10347 * @param turntableRotationRate constant rotation rate at which the
10348 * turntable is spinning. Must be
10349 * expressed in radians per second (rad/s).
10350 * @param timeInterval time interval between measurements being
10351 * captured expressed in seconds (s).
10352 * @param measurements collection of body kinematics
10353 * measurements with standard deviations
10354 * taken at the same position with zero
10355 * velocity and unknown different
10356 * orientations.
10357 * @param initialBias initial gyroscope bias to be used to
10358 * find a solution. This must have length
10359 * 3 and is expressed in radians per
10360 * second (rad/s).
10361 * @param initialMg initial gyroscope scale factors and
10362 * cross coupling errors matrix. Must
10363 * be 3x3.
10364 * @param initialGg initial gyroscope G-dependent cross
10365 * biases introduced on the gyroscope by
10366 * the specific forces sensed by the
10367 * accelerometer. Must be 3x3.
10368 * @param accelerometerBias known accelerometer bias. This must
10369 * have length 3 and is expressed in
10370 * meters per squared second
10371 * (m/s^2).
10372 * @param accelerometerMa known accelerometer scale factors and
10373 * cross coupling matrix. Must be 3x3.
10374 * @param listener listener to handle events raised
10375 * by this calibrator.
10376 * @return a robust gyroscope calibrator.
10377 * @throws IllegalArgumentException if any of the provided values does
10378 * not have proper size or if either
10379 * turntable rotation rate or
10380 * time interval is zero or negative.
10381 */
10382 public static RobustTurntableGyroscopeCalibrator create(
10383 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10384 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
10385 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
10386 final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener) {
10387 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
10388 accelerometerBias, accelerometerMa, listener, DEFAULT_ROBUST_METHOD);
10389 }
10390
10391 /**
10392 * Creates a robust gyroscope calibrator using default robust method.
10393 *
10394 * @param position position where body kinematics measures
10395 * have been taken.
10396 * @param turntableRotationRate constant rotation rate at which the
10397 * turntable is spinning. Must be
10398 * expressed in radians per second (rad/s).
10399 * @param timeInterval time interval between measurements being
10400 * captured expressed in seconds (s).
10401 * @param measurements collection of body kinematics
10402 * measurements with standard deviations
10403 * taken at the same position with zero
10404 * velocity and unknown different
10405 * orientations.
10406 * @param initialBias initial gyroscope bias to be used to
10407 * find a solution. This must be 3x1 and
10408 * is expressed in radians per second
10409 * (rad/s).
10410 * @param initialMg initial gyroscope scale factors and
10411 * cross coupling errors matrix. Must
10412 * be 3x3.
10413 * @param initialGg initial gyroscope G-dependent cross
10414 * biases introduced on the gyroscope by
10415 * the specific forces sensed by the
10416 * accelerometer. Must be 3x3.
10417 * @param accelerometerBias known accelerometer bias. This must
10418 * have length 3 and is expressed in
10419 * meters per squared second
10420 * (m/s^2).
10421 * @param accelerometerMa known accelerometer scale factors and
10422 * cross coupling matrix. Must be 3x3.
10423 * @return a robust gyroscope calibrator.
10424 * @throws IllegalArgumentException if any of the provided values does
10425 * not have proper size or if either
10426 * turntable rotation rate or
10427 * time interval is zero or negative.
10428 */
10429 public static RobustTurntableGyroscopeCalibrator create(
10430 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10431 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
10432 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa) {
10433 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
10434 accelerometerBias, accelerometerMa, DEFAULT_ROBUST_METHOD);
10435 }
10436
10437 /**
10438 * Creates a robust gyroscope calibrator using default robust method.
10439 *
10440 * @param position position where body kinematics measures
10441 * have been taken.
10442 * @param turntableRotationRate constant rotation rate at which the
10443 * turntable is spinning. Must be
10444 * expressed in radians per second (rad/s).
10445 * @param timeInterval time interval between measurements being
10446 * captured expressed in seconds (s).
10447 * @param measurements collection of body kinematics
10448 * measurements with standard deviations
10449 * taken at the same position with zero
10450 * velocity and unknown different
10451 * orientations.
10452 * @param initialBias initial gyroscope bias to be used to
10453 * find a solution. This must be 3x1 and
10454 * is expressed in radians per second
10455 * (rad/s).
10456 * @param initialMg initial gyroscope scale factors and
10457 * cross coupling errors matrix. Must
10458 * be 3x3.
10459 * @param initialGg initial gyroscope G-dependent cross
10460 * biases introduced on the gyroscope by
10461 * the specific forces sensed by the
10462 * accelerometer. Must be 3x3.
10463 * @param accelerometerBias known accelerometer bias. This must
10464 * have length 3 and is expressed in
10465 * meters per squared second
10466 * (m/s^2).
10467 * @param accelerometerMa known accelerometer scale factors and
10468 * cross coupling matrix. Must be 3x3.
10469 * @param listener listener to handle events raised
10470 * by this calibrator.
10471 * @return a robust gyroscope calibrator.
10472 * @throws IllegalArgumentException if any of the provided values does
10473 * not have proper size or if either
10474 * turntable rotation rate or
10475 * time interval is zero or negative.
10476 */
10477 public static RobustTurntableGyroscopeCalibrator create(
10478 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10479 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
10480 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
10481 final RobustTurntableGyroscopeCalibratorListener listener) {
10482 return create(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
10483 accelerometerBias, accelerometerMa, listener, DEFAULT_ROBUST_METHOD);
10484 }
10485
10486 /**
10487 * Creates a robust gyroscope calibrator using default robust method.
10488 *
10489 * @param position position where body kinematics
10490 * measures have been taken.
10491 * @param turntableRotationRate constant rotation rate at which
10492 * the turntable is spinning. Must
10493 * be expressed in radians per
10494 * second (rad/s).
10495 * @param timeInterval time interval between measurements
10496 * being captured expressed in
10497 * seconds (s).
10498 * @param measurements collection of body kinematics
10499 * measurements with standard
10500 * deviations taken at the same
10501 * position with zero velocity
10502 * and unknown different
10503 * orientations.
10504 * @param commonAxisUsed indicates whether z-axis is
10505 * assumed to be common for
10506 * accelerometer and gyroscope.
10507 * @param estimateGDependentCrossBiases true if G-dependent cross biases
10508 * will be estimated, false
10509 * otherwise.
10510 * @param initialBias initial gyroscope bias to be
10511 * used to find a solution. This
10512 * must be 3x1 and is expressed in
10513 * radians per second (rad/s).
10514 * @param initialMg initial gyroscope scale factors
10515 * and cross coupling errors matrix.
10516 * Must be 3x3.
10517 * @param initialGg initial gyroscope G-dependent
10518 * cross biases introduced on the
10519 * gyroscope by the specific
10520 * forces sensed by the
10521 * accelerometer. Must be 3x3.
10522 * @return a robust gyroscope calibrator.
10523 * @throws IllegalArgumentException if any of the provided values does
10524 * not have proper size or if either
10525 * turntable rotation rate or
10526 * time interval is zero or negative.
10527 */
10528 public static RobustTurntableGyroscopeCalibrator create(
10529 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10530 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
10531 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
10532 final Matrix initialGg) {
10533 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
10534 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, DEFAULT_ROBUST_METHOD);
10535 }
10536
10537 /**
10538 * Creates a robust gyroscope calibrator using default robust method.
10539 *
10540 * @param position position where body kinematics
10541 * measures have been taken.
10542 * @param turntableRotationRate constant rotation rate at which
10543 * the turntable is spinning. Must
10544 * be expressed in radians per
10545 * second (rad/s).
10546 * @param timeInterval time interval between measurements
10547 * being captured expressed in
10548 * seconds (s).
10549 * @param measurements collection of body kinematics
10550 * measurements with standard
10551 * deviations taken at the same
10552 * position with zero velocity
10553 * and unknown different
10554 * orientations.
10555 * @param commonAxisUsed indicates whether z-axis is
10556 * assumed to be common for
10557 * accelerometer and gyroscope.
10558 * @param estimateGDependentCrossBiases true if G-dependent cross biases
10559 * will be estimated, false
10560 * otherwise.
10561 * @param initialBias initial gyroscope bias to be
10562 * used to find a solution. This
10563 * must be 3x1 and is expressed in
10564 * radians per second (rad/s).
10565 * @param initialMg initial gyroscope scale factors
10566 * and cross coupling errors matrix.
10567 * Must be 3x3.
10568 * @param initialGg initial gyroscope G-dependent
10569 * cross biases introduced on the
10570 * gyroscope by the specific
10571 * forces sensed by the
10572 * accelerometer. Must be 3x3.
10573 * @param listener listener to handle events raised
10574 * by this calibrator.
10575 * @return a robust gyroscope calibrator.
10576 * @throws IllegalArgumentException if any of the provided values does
10577 * not have proper size or if either
10578 * turntable rotation rate or
10579 * time interval is zero or negative.
10580 */
10581 public static RobustTurntableGyroscopeCalibrator create(
10582 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10583 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
10584 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
10585 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
10586 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
10587 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener, DEFAULT_ROBUST_METHOD);
10588 }
10589
10590 /**
10591 * Creates a robust gyroscope calibrator using default robust method.
10592 *
10593 * @param position position where body kinematics
10594 * measures have been taken.
10595 * @param turntableRotationRate constant rotation rate at which
10596 * the turntable is spinning. Must
10597 * be expressed in radians per
10598 * second (rad/s).
10599 * @param timeInterval time interval between measurements
10600 * being captured expressed in
10601 * seconds (s).
10602 * @param measurements collection of body kinematics
10603 * measurements with standard
10604 * deviations taken at the same
10605 * position with zero velocity
10606 * and unknown different
10607 * orientations.
10608 * @param commonAxisUsed indicates whether z-axis is
10609 * assumed to be common for
10610 * accelerometer and gyroscope.
10611 * @param estimateGDependentCrossBiases true if G-dependent cross biases
10612 * will be estimated, false
10613 * otherwise.
10614 * @param initialBias initial gyroscope bias to be
10615 * used to find a solution. This
10616 * must have length 3 and is
10617 * expressed in radians per second
10618 * (rad/s).
10619 * @param initialMg initial gyroscope scale factors
10620 * and cross coupling errors matrix.
10621 * Must be 3x3.
10622 * @param initialGg initial gyroscope G-dependent
10623 * cross biases introduced on the
10624 * gyroscope by the specific forces
10625 * sensed by the accelerometer.
10626 * Must be 3x3.
10627 * @return a robust gyroscope calibrator.
10628 * @throws IllegalArgumentException if any of the provided values does
10629 * not have proper size or if either
10630 * turntable rotation rate or
10631 * time interval is zero or negative.
10632 */
10633 public static RobustTurntableGyroscopeCalibrator create(
10634 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10635 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
10636 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
10637 final Matrix initialGg) {
10638 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
10639 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, DEFAULT_ROBUST_METHOD);
10640 }
10641
10642 /**
10643 * Creates a robust gyroscope calibrator using default robust method.
10644 *
10645 * @param position position where body kinematics
10646 * measures have been taken.
10647 * @param turntableRotationRate constant rotation rate at which
10648 * the turntable is spinning. Must
10649 * be expressed in radians per
10650 * second (rad/s).
10651 * @param timeInterval time interval between measurements
10652 * being captured expressed in
10653 * seconds (s).
10654 * @param measurements collection of body kinematics
10655 * measurements with standard
10656 * deviations taken at the same
10657 * position with zero velocity
10658 * and unknown different
10659 * orientations.
10660 * @param commonAxisUsed indicates whether z-axis is
10661 * assumed to be common for
10662 * accelerometer and gyroscope.
10663 * @param estimateGDependentCrossBiases true if G-dependent cross biases
10664 * will be estimated, false
10665 * otherwise.
10666 * @param initialBias initial gyroscope bias to be
10667 * used to find a solution. This
10668 * must have length 3 and is
10669 * expressed in radians per second
10670 * (rad/s).
10671 * @param initialMg initial gyroscope scale factors
10672 * and cross coupling errors matrix.
10673 * Must be 3x3.
10674 * @param initialGg initial gyroscope G-dependent
10675 * cross biases introduced on the
10676 * gyroscope by the specific forces
10677 * sensed by the accelerometer.
10678 * Must be 3x3.
10679 * @param listener listener to handle events raised
10680 * by this calibrator.
10681 * @return a robust gyroscope calibrator.
10682 * @throws IllegalArgumentException if any of the provided values does
10683 * not have proper size or if either
10684 * turntable rotation rate or
10685 * time interval is zero or negative.
10686 */
10687 public static RobustTurntableGyroscopeCalibrator create(
10688 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10689 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
10690 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
10691 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
10692 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
10693 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener, DEFAULT_ROBUST_METHOD);
10694 }
10695
10696 /**
10697 * Creates a robust gyroscope calibrator using default robust method.
10698 *
10699 * @param position position where body kinematics
10700 * measures have been taken.
10701 * @param turntableRotationRate constant rotation rate at which
10702 * the turntable is spinning. Must
10703 * be expressed in radians per
10704 * second (rad/s).
10705 * @param timeInterval time interval between measurements
10706 * being captured expressed in
10707 * seconds (s).
10708 * @param measurements collection of body kinematics
10709 * measurements with standard
10710 * deviations taken at the same
10711 * position with zero velocity
10712 * and unknown different
10713 * orientations.
10714 * @param commonAxisUsed indicates whether z-axis is
10715 * assumed to be common for
10716 * accelerometer and gyroscope.
10717 * @param estimateGDependentCrossBiases true if G-dependent cross
10718 * biases will be estimated,
10719 * false otherwise.
10720 * @param initialBias initial gyroscope bias to be
10721 * used to find a solution. This
10722 * must have length 3 and is
10723 * expressed in radians per second
10724 * (rad/s).
10725 * @param initialMg initial gyroscope scale factors
10726 * and cross coupling errors
10727 * matrix. Must be 3x3.
10728 * @param initialGg initial gyroscope G-dependent
10729 * cross biases introduced on the
10730 * gyroscope by the specific forces
10731 * sensed by the accelerometer.
10732 * Must be 3x3.
10733 * @param accelerometerBias known accelerometer bias. This
10734 * must have length 3 and is
10735 * expressed in meters per squared
10736 * second (m/s^2).
10737 * @param accelerometerMa known accelerometer scale factors
10738 * and cross coupling matrix. Must
10739 * be 3x3.
10740 * @return a robust gyroscope calibrator.
10741 * @throws IllegalArgumentException if any of the provided values does
10742 * not have proper size or if either
10743 * turntable rotation rate or
10744 * time interval is zero or negative.
10745 */
10746 public static RobustTurntableGyroscopeCalibrator create(
10747 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10748 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
10749 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
10750 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa) {
10751 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
10752 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
10753 DEFAULT_ROBUST_METHOD);
10754 }
10755
10756 /**
10757 * Creates a robust gyroscope calibrator using default robust method.
10758 *
10759 * @param position position where body kinematics
10760 * measures have been taken.
10761 * @param turntableRotationRate constant rotation rate at which
10762 * the turntable is spinning. Must
10763 * be expressed in radians per
10764 * second (rad/s).
10765 * @param timeInterval time interval between measurements
10766 * being captured expressed in
10767 * seconds (s).
10768 * @param measurements collection of body kinematics
10769 * measurements with standard
10770 * deviations taken at the same
10771 * position with zero velocity
10772 * and unknown different
10773 * orientations.
10774 * @param commonAxisUsed indicates whether z-axis is
10775 * assumed to be common for
10776 * accelerometer and gyroscope.
10777 * @param estimateGDependentCrossBiases true if G-dependent cross
10778 * biases will be estimated,
10779 * false otherwise.
10780 * @param initialBias initial gyroscope bias to be
10781 * used to find a solution. This
10782 * must have length 3 and is
10783 * expressed in radians per second
10784 * (rad/s).
10785 * @param initialMg initial gyroscope scale factors
10786 * and cross coupling errors
10787 * matrix. Must be 3x3.
10788 * @param initialGg initial gyroscope G-dependent
10789 * cross biases introduced on the
10790 * gyroscope by the specific forces
10791 * sensed by the accelerometer.
10792 * Must be 3x3.
10793 * @param accelerometerBias known accelerometer bias. This
10794 * must have length 3 and is
10795 * expressed in meters per squared
10796 * second (m/s^2).
10797 * @param accelerometerMa known accelerometer scale factors
10798 * and cross coupling matrix. Must
10799 * be 3x3.
10800 * @param listener listener to handle events raised
10801 * by this calibrator.
10802 * @return a robust gyroscope calibrator.
10803 * @throws IllegalArgumentException if any of the provided values does
10804 * not have proper size or if either
10805 * turntable rotation rate or
10806 * time interval is zero or negative.
10807 */
10808 public static RobustTurntableGyroscopeCalibrator create(
10809 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10810 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
10811 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
10812 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
10813 final RobustTurntableGyroscopeCalibratorListener listener) {
10814 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
10815 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
10816 listener, DEFAULT_ROBUST_METHOD);
10817 }
10818
10819 /**
10820 * Creates a robust gyroscope calibrator using default robust method.
10821 *
10822 * @param position position where body kinematics
10823 * measures have been taken.
10824 * @param turntableRotationRate constant rotation rate at which
10825 * the turntable is spinning. Must
10826 * be expressed in radians per
10827 * second (rad/s).
10828 * @param timeInterval time interval between measurements
10829 * being captured expressed in
10830 * seconds (s).
10831 * @param measurements collection of body kinematics
10832 * measurements with standard
10833 * deviations taken at the same
10834 * position with zero velocity and
10835 * unknown different orientations.
10836 * @param commonAxisUsed indicates whether z-axis is
10837 * assumed to be common for
10838 * accelerometer and gyroscope.
10839 * @param estimateGDependentCrossBiases true if G-dependent cross biases
10840 * will be estimated, false
10841 * otherwise.
10842 * @param initialBias initial gyroscope bias to be
10843 * used to find a solution. This
10844 * must be 3x1 and is expressed in
10845 * radians per second (rad/s).
10846 * @param initialMg initial gyroscope scale factors
10847 * and cross coupling errors matrix.
10848 * Must be 3x3.
10849 * @param initialGg initial gyroscope G-dependent
10850 * cross biases introduced on the
10851 * gyroscope by the specific forces
10852 * sensed by the accelerometer. Must
10853 * be 3x3.
10854 * @param accelerometerBias known accelerometer bias. This
10855 * must have length 3 and is
10856 * expressed in meters per squared
10857 * second (m/s^2).
10858 * @param accelerometerMa known accelerometer scale factors
10859 * and cross coupling matrix. Must
10860 * be 3x3.
10861 * @return a robust gyroscope calibrator.
10862 * @throws IllegalArgumentException if any of the provided values does
10863 * not have proper size or if either
10864 * turntable rotation rate or
10865 * time interval is zero or negative.
10866 */
10867 public static RobustTurntableGyroscopeCalibrator create(
10868 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10869 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
10870 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
10871 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa) {
10872 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
10873 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
10874 DEFAULT_ROBUST_METHOD);
10875 }
10876
10877 /**
10878 * Creates a robust gyroscope calibrator using default robust method.
10879 *
10880 * @param position position where body kinematics
10881 * measures have been taken.
10882 * @param turntableRotationRate constant rotation rate at which
10883 * the turntable is spinning. Must
10884 * be expressed in radians per
10885 * second (rad/s).
10886 * @param timeInterval time interval between measurements
10887 * being captured expressed in
10888 * seconds (s).
10889 * @param measurements collection of body kinematics
10890 * measurements with standard
10891 * deviations taken at the same
10892 * position with zero velocity and
10893 * unknown different orientations.
10894 * @param commonAxisUsed indicates whether z-axis is
10895 * assumed to be common for
10896 * accelerometer and gyroscope.
10897 * @param estimateGDependentCrossBiases true if G-dependent cross biases
10898 * will be estimated, false
10899 * otherwise.
10900 * @param initialBias initial gyroscope bias to be
10901 * used to find a solution. This
10902 * must be 3x1 and is expressed in
10903 * radians per second (rad/s).
10904 * @param initialMg initial gyroscope scale factors
10905 * and cross coupling errors matrix.
10906 * Must be 3x3.
10907 * @param initialGg initial gyroscope G-dependent
10908 * cross biases introduced on the
10909 * gyroscope by the specific forces
10910 * sensed by the accelerometer. Must
10911 * be 3x3.
10912 * @param accelerometerBias known accelerometer bias. This
10913 * must have length 3 and is
10914 * expressed in meters per squared
10915 * second (m/s^2).
10916 * @param accelerometerMa known accelerometer scale factors
10917 * and cross coupling matrix. Must
10918 * be 3x3.
10919 * @param listener listener to handle events raised
10920 * by this calibrator.
10921 * @return a robust gyroscope calibrator.
10922 * @throws IllegalArgumentException if any of the provided values does
10923 * not have proper size or if either
10924 * turntable rotation rate or
10925 * time interval is zero or negative.
10926 */
10927 public static RobustTurntableGyroscopeCalibrator create(
10928 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
10929 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
10930 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
10931 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
10932 final RobustTurntableGyroscopeCalibratorListener listener) {
10933 return create(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
10934 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
10935 listener, DEFAULT_ROBUST_METHOD);
10936 }
10937
10938 /**
10939 * Computes error of a preliminary result respect a given measurement.
10940 *
10941 * @param measurement a measurement.
10942 * @param preliminaryResult a preliminary result.
10943 * @return computed error.
10944 */
10945 protected double computeError(
10946 final StandardDeviationBodyKinematics measurement, final PreliminaryResult preliminaryResult) {
10947 // We know that measured angular rate is:
10948 // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
10949
10950 // Hence:
10951 // [Ωmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [Ωtruex] + [g11 g12 g13][ftruex]
10952 // [Ωmeasy] [by] [0 1 0] [myx sy myz] [Ωtruey] [g21 g22 g23][ftruey]
10953 // [Ωmeasz] [bz] [0 0 1] [mzx mzy sz ] [Ωtruez] [g31 g32 g33][ftruez]
10954
10955 final var measuredKinematics = measurement.getKinematics();
10956
10957 final var specificForce = new double[]{
10958 measuredKinematics.getFx(),
10959 measuredKinematics.getFy(),
10960 measuredKinematics.getFz()
10961 };
10962
10963 try {
10964 final var axis1 = ArrayUtils.normalizeAndReturnNew(specificForce);
10965 final var rot1 = new Quaternion(axis1, 0.0);
10966
10967 final var nedC1 = new CoordinateTransformation(
10968 rot1.asInhomogeneousMatrix(), FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
10969
10970 final var nedPosition = getNedPosition();
10971 final var nedFrame1 = new NEDFrame(nedPosition, nedC1);
10972 final var ecefFrame1 = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame1);
10973 var ti = this.timeInterval;
10974 var angleIncrement = turntableRotationRate * ti;
10975 if (Math.abs(angleIncrement) > Math.PI / 2.0) {
10976 // angle = rot_rate * interval
10977 // rot_rate * interval / x = angle / x
10978
10979 // if we want angle / x = pi / 2, then:
10980 final var x = Math.abs(angleIncrement) / (Math.PI / 2.0);
10981 ti /= x;
10982 angleIncrement = turntableRotationRate * ti;
10983 }
10984 final var rot = new AxisRotation3D(axis1, angleIncrement);
10985 final var rot2 = rot1.combineAndReturnNew(rot);
10986 final var nedC2 = new CoordinateTransformation(rot2.asInhomogeneousMatrix(), FrameType.BODY_FRAME,
10987 FrameType.LOCAL_NAVIGATION_FRAME);
10988
10989 final var nedFrame2 = new NEDFrame(nedPosition, nedC2);
10990 final var ecefFrame2 = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame2);
10991
10992 final var expectedKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(
10993 ti, ecefFrame2, ecefFrame1);
10994
10995 final var angularRateMeasX1 = measuredKinematics.getAngularRateX();
10996 final var angularRateMeasY1 = measuredKinematics.getAngularRateY();
10997 final var angularRateMeasZ1 = measuredKinematics.getAngularRateZ();
10998
10999 final var angularRateTrueX = expectedKinematics.getAngularRateX();
11000 final var angularRateTrueY = expectedKinematics.getAngularRateY();
11001 final var angularRateTrueZ = expectedKinematics.getAngularRateZ();
11002
11003 final var fTrueX = expectedKinematics.getFx();
11004 final var fTrueY = expectedKinematics.getFy();
11005 final var fTrueZ = expectedKinematics.getFz();
11006
11007 final var b = preliminaryResult.estimatedBiases;
11008 final var bx = b[0];
11009 final var by = b[1];
11010 final var bz = b[2];
11011
11012 final var mg = preliminaryResult.estimatedMg;
11013
11014 final var gg = preliminaryResult.estimatedGg;
11015
11016 final var m1 = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
11017 m1.add(mg);
11018
11019 final var angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
11020 angularRateTrue.setElementAtIndex(0, angularRateTrueX);
11021 angularRateTrue.setElementAtIndex(1, angularRateTrueY);
11022 angularRateTrue.setElementAtIndex(2, angularRateTrueZ);
11023
11024 m1.multiply(angularRateTrue);
11025
11026 final var fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
11027 fTrue.setElementAtIndex(0, fTrueX);
11028 fTrue.setElementAtIndex(1, fTrueY);
11029 fTrue.setElementAtIndex(2, fTrueZ);
11030 final var m2 = gg.multiplyAndReturnNew(fTrue);
11031
11032 m1.add(m2);
11033
11034 final var angularRateMeasX2 = bx + m1.getElementAtIndex(0);
11035 final var angularRateMeasY2 = by + m1.getElementAtIndex(1);
11036 final var angularRateMeasZ2 = bz + m1.getElementAtIndex(2);
11037
11038 final var sqrNormMeas1 = angularRateMeasX1 * angularRateMeasX1 + angularRateMeasY1 * angularRateMeasY1
11039 + angularRateMeasZ1 * angularRateMeasZ1;
11040 final var sqrNormMeas2 = angularRateMeasX2 * angularRateMeasX2 + angularRateMeasY2 * angularRateMeasY2
11041 + angularRateMeasZ2 * angularRateMeasZ2;
11042
11043 final var normMeas1 = Math.sqrt(sqrNormMeas1);
11044 final var normMeas2 = Math.sqrt(sqrNormMeas2);
11045
11046 return Math.abs(normMeas1 - normMeas2);
11047
11048 } catch (final WrongSizeException | InvalidRotationMatrixException
11049 | InvalidSourceAndDestinationFrameTypeException e) {
11050 return Double.MAX_VALUE;
11051 }
11052 }
11053
11054 /**
11055 * Computes a preliminary solution for a subset of samples picked by a robust estimator.
11056 *
11057 * @param samplesIndices indices of samples picked by the robust estimator.
11058 * @param solutions list where estimated preliminary solution will be stored.
11059 */
11060 protected void computePreliminarySolutions(final int[] samplesIndices, final List<PreliminaryResult> solutions) {
11061
11062 final var meas = new ArrayList<StandardDeviationBodyKinematics>();
11063
11064 for (final var samplesIndex : samplesIndices) {
11065 meas.add(this.measurements.get(samplesIndex));
11066 }
11067
11068 try {
11069 final var result = new PreliminaryResult();
11070 result.estimatedBiases = getInitialBias();
11071 result.estimatedMg = getInitialMg();
11072 result.estimatedGg = getInitialGg();
11073
11074 innerCalibrator.setTurntableRotationRate(turntableRotationRate);
11075 innerCalibrator.setTimeInterval(timeInterval);
11076 innerCalibrator.setGDependentCrossBiasesEstimated(estimateGDependentCrossBiases);
11077 innerCalibrator.setInitialBias(result.estimatedBiases);
11078 innerCalibrator.setInitialMg(result.estimatedMg);
11079 innerCalibrator.setInitialGg(result.estimatedGg);
11080 innerCalibrator.setAccelerometerBias(accelerometerBiasX, accelerometerBiasY, accelerometerBiasZ);
11081 innerCalibrator.setAccelerometerScalingFactorsAndCrossCouplingErrors(
11082 accelerometerSx, accelerometerSy, accelerometerSz,
11083 accelerometerMxy, accelerometerMxz, accelerometerMyx,
11084 accelerometerMyz, accelerometerMzx, accelerometerMzy);
11085 innerCalibrator.setCommonAxisUsed(commonAxisUsed);
11086 innerCalibrator.setMeasurements(meas);
11087 innerCalibrator.setPosition(position);
11088 innerCalibrator.calibrate();
11089
11090 innerCalibrator.getEstimatedBiases(result.estimatedBiases);
11091 result.estimatedMg = innerCalibrator.getEstimatedMg();
11092 result.estimatedGg = innerCalibrator.getEstimatedGg();
11093
11094 if (keepCovariance) {
11095 result.covariance = innerCalibrator.getEstimatedCovariance();
11096 } else {
11097 result.covariance = null;
11098 }
11099
11100 result.estimatedMse = innerCalibrator.getEstimatedMse();
11101 result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
11102 result.estimatedChiSqDegreesOfFreedom = innerCalibrator.getEstimatedChiSqDegreesOfFreedom();
11103 result.estimatedReducedChiSq = innerCalibrator.getEstimatedReducedChiSq();
11104 result.estimatedP = innerCalibrator.getEstimatedP();
11105 result.estimatedQ = innerCalibrator.getEstimatedQ();
11106
11107 solutions.add(result);
11108 } catch (final LockedException | CalibrationException | NotReadyException e) {
11109 solutions.clear();
11110 }
11111 }
11112
11113 /**
11114 * Attempts to refine calibration parameters if refinement is requested.
11115 * This method returns a refined solution or provided input if refinement is not
11116 * requested or has failed.
11117 * If refinement is enabled and it is requested to keep covariance, this method
11118 * will also keep covariance of refined position.
11119 *
11120 * @param preliminaryResult a preliminary result.
11121 */
11122 protected void attemptRefine(final PreliminaryResult preliminaryResult) {
11123 if (refineResult && inliersData != null) {
11124 final var inliers = inliersData.getInliers();
11125 final var nSamples = measurements.size();
11126
11127 final var inlierMeasurements = new ArrayList<StandardDeviationBodyKinematics>();
11128 for (var i = 0; i < nSamples; i++) {
11129 if (inliers.get(i)) {
11130 // sample is inlier
11131 inlierMeasurements.add(measurements.get(i));
11132 }
11133 }
11134
11135 try {
11136 innerCalibrator.setTurntableRotationRate(turntableRotationRate);
11137 innerCalibrator.setTimeInterval(timeInterval);
11138 innerCalibrator.setGDependentCrossBiasesEstimated(estimateGDependentCrossBiases);
11139 innerCalibrator.setInitialBias(preliminaryResult.estimatedBiases);
11140 innerCalibrator.setInitialMg(preliminaryResult.estimatedMg);
11141 innerCalibrator.setInitialGg(preliminaryResult.estimatedGg);
11142 innerCalibrator.setAccelerometerBias(accelerometerBiasX, accelerometerBiasY, accelerometerBiasZ);
11143 innerCalibrator.setAccelerometerScalingFactorsAndCrossCouplingErrors(
11144 accelerometerSx, accelerometerSy, accelerometerSz,
11145 accelerometerMxy, accelerometerMxz, accelerometerMyx,
11146 accelerometerMyz, accelerometerMzx, accelerometerMzy);
11147 innerCalibrator.setCommonAxisUsed(commonAxisUsed);
11148 innerCalibrator.setMeasurements(inlierMeasurements);
11149 innerCalibrator.setPosition(position);
11150 innerCalibrator.calibrate();
11151
11152 estimatedBiases = innerCalibrator.getEstimatedBiases();
11153 estimatedMg = innerCalibrator.getEstimatedMg();
11154 estimatedGg = innerCalibrator.getEstimatedGg();
11155
11156 if (keepCovariance) {
11157 estimatedCovariance = innerCalibrator.getEstimatedCovariance();
11158 } else {
11159 estimatedCovariance = null;
11160 }
11161
11162 estimatedMse = innerCalibrator.getEstimatedMse();
11163 estimatedChiSq = innerCalibrator.getEstimatedChiSq();
11164 estimatedChiSqDegreesOfFreedom = innerCalibrator.getEstimatedChiSqDegreesOfFreedom();
11165 estimatedReducedChiSq = innerCalibrator.getEstimatedReducedChiSq();
11166 estimatedP = innerCalibrator.getEstimatedP();
11167 estimatedQ = innerCalibrator.getEstimatedQ();
11168
11169 } catch (final LockedException | CalibrationException | NotReadyException e) {
11170 estimatedCovariance = preliminaryResult.covariance;
11171 estimatedBiases = preliminaryResult.estimatedBiases;
11172 estimatedMg = preliminaryResult.estimatedMg;
11173 estimatedGg = preliminaryResult.estimatedGg;
11174 estimatedMse = preliminaryResult.estimatedMse;
11175 estimatedChiSq = preliminaryResult.estimatedChiSq;
11176 estimatedChiSqDegreesOfFreedom = preliminaryResult.estimatedChiSqDegreesOfFreedom;
11177 estimatedReducedChiSq = preliminaryResult.estimatedReducedChiSq;
11178 estimatedP = preliminaryResult.estimatedP;
11179 estimatedQ = preliminaryResult.estimatedQ;
11180 }
11181 } else {
11182 estimatedCovariance = preliminaryResult.covariance;
11183 estimatedBiases = preliminaryResult.estimatedBiases;
11184 estimatedMg = preliminaryResult.estimatedMg;
11185 estimatedGg = preliminaryResult.estimatedGg;
11186 estimatedMse = preliminaryResult.estimatedMse;
11187 estimatedChiSq = preliminaryResult.estimatedChiSq;
11188 estimatedChiSqDegreesOfFreedom = preliminaryResult.estimatedChiSqDegreesOfFreedom;
11189 estimatedReducedChiSq = preliminaryResult.estimatedReducedChiSq;
11190 estimatedP = preliminaryResult.estimatedP;
11191 estimatedQ = preliminaryResult.estimatedQ;
11192 }
11193 }
11194
11195 /**
11196 * Converts provided NED position expressed in terms of latitude, longitude and height respect
11197 * mean Earth surface, to position expressed in ECEF coordinates.
11198 *
11199 * @param position NED position to be converted.
11200 * @return converted position expressed in ECEF coordinates.
11201 */
11202 private static ECEFPosition convertPosition(final NEDPosition position) {
11203 final var velocity = new ECEFVelocity();
11204 final var result = new ECEFPosition();
11205 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
11206 position.getLatitude(), position.getLongitude(), position.getHeight(),
11207 0.0, 0.0, 0.0, result, velocity);
11208 return result;
11209 }
11210
11211 /**
11212 * Converts acceleration instance to meters per squared second.
11213 *
11214 * @param acceleration acceleration instance to be converted.
11215 * @return converted value.
11216 */
11217 private static double convertAcceleration(final Acceleration acceleration) {
11218 return AccelerationConverter.convert(acceleration.getValue().doubleValue(),
11219 acceleration.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11220 }
11221
11222 /**
11223 * Converts angular speed value and unit to radians per second.
11224 *
11225 * @param value angular speed value.
11226 * @param unit unit of angular speed value.
11227 * @return converted value.
11228 */
11229 private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
11230 return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
11231 }
11232
11233 /**
11234 * Converts angular speed instance to radians per second.
11235 *
11236 * @param angularSpeed angular speed instance to be converted.
11237 * @return converted value.
11238 */
11239 private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
11240 return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
11241 }
11242
11243 /**
11244 * Converts time instance to seconds.
11245 *
11246 * @param time time instance to be converted.
11247 * @return converted value.
11248 */
11249 private static double convertTime(final Time time) {
11250 return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
11251 }
11252
11253 /**
11254 * Internal class containing estimated preliminary result.
11255 */
11256 protected static class PreliminaryResult {
11257 /**
11258 * Estimated gyroscope biases for each IMU axis expressed in radians per second
11259 * (rad/s).
11260 */
11261 private double[] estimatedBiases;
11262
11263 /**
11264 * Estimated gyroscope scale factors and cross coupling errors.
11265 * This is the product of matrix Tg containing cross coupling errors and Kg
11266 * containing scaling factors.
11267 * So that:
11268 * <pre>
11269 * Mg = [sx mxy mxz] = Tg*Kg
11270 * [myx sy myz]
11271 * [mzx mzy sz ]
11272 * </pre>
11273 * Where:
11274 * <pre>
11275 * Kg = [sx 0 0 ]
11276 * [0 sy 0 ]
11277 * [0 0 sz]
11278 * </pre>
11279 * and
11280 * <pre>
11281 * Tg = [1 -alphaXy alphaXz ]
11282 * [alphaYx 1 -alphaYz]
11283 * [-alphaZx alphaZy 1 ]
11284 * </pre>
11285 * Hence:
11286 * <pre>
11287 * Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
11288 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
11289 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
11290 * </pre>
11291 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
11292 * are considered to be zero if the gyroscope z-axis is assumed to be the same
11293 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
11294 * becomes upper diagonal:
11295 * <pre>
11296 * Mg = [sx mxy mxz]
11297 * [0 sy myz]
11298 * [0 0 sz ]
11299 * </pre>
11300 * Values of this matrix are unit-less.
11301 */
11302 private Matrix estimatedMg;
11303
11304 /**
11305 * Estimated G-dependent cross biases introduced on the gyroscope by the
11306 * specific forces sensed by the accelerometer.
11307 * This instance allows any 3x3 matrix.
11308 */
11309 private Matrix estimatedGg;
11310
11311 /**
11312 * Covariance matrix for estimated result.
11313 */
11314 private Matrix covariance;
11315
11316 /**
11317 * Estimated Mean Square Error.
11318 */
11319 private double estimatedMse;
11320
11321 /**
11322 * Estimated chi square value.
11323 */
11324 private double estimatedChiSq;
11325
11326 /**
11327 * Estimated degrees of freedom of chi square value. Degrees of freedom is equal to the number of sampled data
11328 * minus the number of estimated parameters.
11329 */
11330 private int estimatedChiSqDegreesOfFreedom;
11331
11332 /**
11333 * Estimated reduced chi square value. This is equal to estimated chi square value divided by its degrees of
11334 * freedom. Ideally this value should be close to 1.0.
11335 */
11336 private double estimatedReducedChiSq;
11337
11338 /**
11339 * Estimated probability of finding a smaller chi square value expressed as a value between 0.0 and 1.0. The smaller
11340 * the found chi square value is, the better the fit of the estimated parameters to the actual parameter. Thus, the
11341 * smaller the chance of finding a smaller chi square value, then the better the estimated fit is.
11342 */
11343 private double estimatedP;
11344
11345 /**
11346 * Estimated measure of quality of estimated fit as a value between 0.0 and 1.0. The larger the quality value is,
11347 * the better the fit that has been estimated.
11348 */
11349 private double estimatedQ;
11350 }
11351 }