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