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.Matrix;
19 import com.irurueta.navigation.LockedException;
20 import com.irurueta.navigation.NotReadyException;
21 import com.irurueta.navigation.frames.ECEFPosition;
22 import com.irurueta.navigation.frames.NEDPosition;
23 import com.irurueta.navigation.inertial.calibration.CalibrationException;
24 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
25 import com.irurueta.numerical.robust.MSACRobustEstimator;
26 import com.irurueta.numerical.robust.MSACRobustEstimatorListener;
27 import com.irurueta.numerical.robust.RobustEstimator;
28 import com.irurueta.numerical.robust.RobustEstimatorException;
29 import com.irurueta.numerical.robust.RobustEstimatorMethod;
30
31 import java.util.List;
32
33 /**
34 * Robustly estimates gyroscope biases, cross couplings and scaling factors
35 * along with G-dependent cross biases introduced on the gyroscope by the
36 * specific forces sensed by the accelerometer using MSAC robust estimator.
37 * <p>
38 * This calibrator assumes that the IMU is placed flat on a turntable spinning
39 * at constant speed, but absolute orientation or position of IMU is unknown.
40 * Turntable must rotate fast enough so that Earth rotation effects can be
41 * neglected, bus slow enough so that gyroscope readings can be properly made.
42 * <p>
43 * To use this calibrator at least 10 measurements are needed when common
44 * z-axis is assumed and G-dependent cross biases are ignored, otherwise
45 * at least 13 measurements are required when common z-axis is not assumed.
46 * If G-dependent cross biases are being estimated, then at least 19
47 * measurements are needed when common z-axis is assumed, otherwise at
48 * least 22 measurements are required when common z-axis is not assumed.
49 * <p>
50 * Measured gyroscope angular rates is assumed to follow the model shown below:
51 * <pre>
52 * Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
53 * </pre>
54 * Where:
55 * - Ωmeas is the measured gyroscope angular rates. This is a 3x1 vector.
56 * - bg is the gyroscope bias. Ideally, on a perfect gyroscope, this should be a
57 * 3x1 zero vector.
58 * - I is the 3x3 identity matrix.
59 * - Mg is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
60 * a perfect gyroscope, this should be a 3x3 zero matrix.
61 * - Ωtrue is ground-truth gyroscope angular rates.
62 * - Gg is the G-dependent cross biases introduced by the specific forces sensed
63 * by the accelerometer. Ideally, on a perfect gyroscope, this should be a 3x3
64 * zero matrix.
65 * - ftrue is ground-truth specific force. This is a 3x1 vector.
66 * - w is measurement noise. This is a 3x1 vector.
67 */
68 public class MSACRobustTurntableGyroscopeCalibrator extends RobustTurntableGyroscopeCalibrator {
69
70 /**
71 * Constant defining default threshold to determine whether samples are
72 * inliers or not.
73 */
74 public static final double DEFAULT_THRESHOLD = 5e-1;
75
76 /**
77 * Minimum value that can be set as threshold.
78 * Threshold must be strictly greater than 0.0.
79 */
80 public static final double MIN_THRESHOLD = 0.0;
81
82 /**
83 * Threshold to determine whether samples are inliers or not when
84 * testing possible estimation solutions.
85 */
86 private double threshold = DEFAULT_THRESHOLD;
87
88 /**
89 * Constructor.
90 */
91 public MSACRobustTurntableGyroscopeCalibrator() {
92 super();
93 }
94
95 /**
96 * Constructor.
97 *
98 * @param position position where body kinematics measures
99 * have been taken.
100 * @param turntableRotationRate constant rotation rate at which the
101 * turntable is spinning. Must be
102 * expressed in radians per second (rad/s).
103 * @param timeInterval time interval between measurements being
104 * captured expressed in seconds (s).
105 * @param measurements collection of body kinematics
106 * measurements with standard deviations
107 * taken at the same position with zero
108 * velocity and unknown different
109 * orientations.
110 * @param initialBias initial gyroscope bias to be used to
111 * find a solution. This must be 3x1 and
112 * is expressed in radians per second
113 * (rad/s).
114 * @param initialMg initial gyroscope scale factors and
115 * cross coupling errors matrix. Must
116 * be 3x3.
117 * @param initialGg initial gyroscope G-dependent cross
118 * biases introduced on the gyroscope by
119 * the specific forces sensed by the
120 * accelerometer. Must be 3x3.
121 * @throws IllegalArgumentException if any of the provided values does
122 * not have proper size or if either
123 * turntable rotation rate or
124 * time interval is zero or negative.
125 */
126 public MSACRobustTurntableGyroscopeCalibrator(
127 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
128 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
129 final Matrix initialGg) {
130 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg);
131 }
132
133 /**
134 * Constructor.
135 *
136 * @param position position where body kinematics measures
137 * have been taken.
138 * @param turntableRotationRate constant rotation rate at which the
139 * turntable is spinning. Must be
140 * expressed in radians per second (rad/s).
141 * @param timeInterval time interval between measurements being
142 * captured expressed in seconds (s).
143 * @param measurements collection of body kinematics
144 * measurements with standard deviations
145 * taken at the same position with zero
146 * velocity and unknown different
147 * orientations.
148 * @param initialBias initial gyroscope bias to be used to
149 * find a solution. This must be 3x1 and
150 * is expressed in radians per second
151 * (rad/s).
152 * @param initialMg initial gyroscope scale factors and
153 * cross coupling errors matrix. Must
154 * be 3x3.
155 * @param initialGg initial gyroscope G-dependent cross
156 * biases introduced on the gyroscope by
157 * the specific forces sensed by the
158 * accelerometer. Must be 3x3.
159 * @param listener listener to handle events raised by this
160 * calibrator.
161 * @throws IllegalArgumentException if any of the provided values does
162 * not have proper size or if either
163 * turntable rotation rate or
164 * time interval is zero or negative.
165 */
166 public MSACRobustTurntableGyroscopeCalibrator(
167 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
168 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
169 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
170 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg, listener);
171 }
172
173 /**
174 * Constructor.
175 *
176 * @param position position where body kinematics measures
177 * have been taken.
178 * @param turntableRotationRate constant rotation rate at which the
179 * turntable is spinning. Must be
180 * expressed in radians per second (rad/s).
181 * @param timeInterval time interval between measurements being
182 * captured expressed in seconds (s).
183 * @param measurements collection of body kinematics
184 * measurements with standard deviations
185 * taken at the same position with zero
186 * velocity and unknown different
187 * orientations.
188 * @param initialBias initial gyroscope bias to be used to
189 * find a solution. This must have
190 * length 3 and is expressed in radians
191 * per second (rad/s).
192 * @param initialMg initial gyroscope scale factors and
193 * cross coupling errors matrix. Must
194 * be 3x3.
195 * @param initialGg initial gyroscope G-dependent cross
196 * biases introduced on the gyroscope by
197 * the specific forces sensed by the
198 * accelerometer. Must be 3x3.
199 * @throws IllegalArgumentException if any of the provided values does
200 * not have proper size or if either
201 * turntable rotation rate or
202 * time interval is zero or negative.
203 */
204 public MSACRobustTurntableGyroscopeCalibrator(
205 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
206 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
207 final Matrix initialMg, final Matrix initialGg) {
208 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg);
209 }
210
211 /**
212 * Constructor.
213 *
214 * @param position position where body kinematics measures
215 * have been taken.
216 * @param turntableRotationRate constant rotation rate at which the
217 * turntable is spinning. Must be
218 * expressed in radians per second (rad/s).
219 * @param timeInterval time interval between measurements being
220 * captured expressed in seconds (s).
221 * @param measurements collection of body kinematics
222 * measurements with standard deviations
223 * taken at the same position with zero
224 * velocity and unknown different
225 * orientations.
226 * @param initialBias initial gyroscope bias to be used to
227 * find a solution. This must have length
228 * 3 and is expressed in radians
229 * per second (rad/s).
230 * @param initialMg initial gyroscope scale factors and
231 * cross coupling errors matrix. Must
232 * be 3x3.
233 * @param initialGg initial gyroscope G-dependent cross
234 * biases introduced on the gyroscope by
235 * the specific forces sensed by the
236 * accelerometer. Must be 3x3.
237 * @param listener listener to handle events raised by
238 * this calibrator.
239 * @throws IllegalArgumentException if any of the provided values does
240 * not have proper size or if either
241 * turntable rotation rate or
242 * time interval is zero or negative.
243 */
244 public MSACRobustTurntableGyroscopeCalibrator(
245 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
246 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
247 final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
248 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg, listener);
249 }
250
251 /**
252 * Constructor.
253 *
254 * @param position position where body kinematics measures
255 * have been taken.
256 * @param turntableRotationRate constant rotation rate at which the
257 * turntable is spinning. Must be
258 * expressed in radians per second (rad/s).
259 * @param timeInterval time interval between measurements being
260 * captured expressed in seconds (s).
261 * @param measurements collection of body kinematics
262 * measurements with standard deviations
263 * taken at the same position with zero
264 * velocity and unknown different
265 * orientations.
266 * @param initialBias initial gyroscope bias to be used to
267 * find a solution. This must have length
268 * 3 and is expressed in radians per
269 * second (rad/s).
270 * @param initialMg initial gyroscope scale factors and
271 * cross coupling errors matrix. Must
272 * be 3x3.
273 * @param initialGg initial gyroscope G-dependent cross
274 * biases introduced on the gyroscope by
275 * the specific forces sensed by the
276 * accelerometer. Must be 3x3.
277 * @param accelerometerBias known accelerometer bias. This must
278 * have length 3 and is expressed in
279 * meters per squared second
280 * (m/s^2).
281 * @param accelerometerMa known accelerometer scale factors and
282 * cross coupling matrix. Must be 3x3.
283 * @throws IllegalArgumentException if any of the provided values does
284 * not have proper size or if either
285 * turntable rotation rate or
286 * time interval is zero or negative.
287 */
288 public MSACRobustTurntableGyroscopeCalibrator(
289 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
290 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
291 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
292 final Matrix accelerometerMa) {
293 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
294 accelerometerBias, accelerometerMa);
295 }
296
297 /**
298 * Constructor.
299 *
300 * @param position position where body kinematics measures
301 * have been taken.
302 * @param turntableRotationRate constant rotation rate at which the
303 * turntable is spinning. Must be
304 * expressed in radians per second (rad/s).
305 * @param timeInterval time interval between measurements being
306 * captured expressed in seconds (s).
307 * @param measurements collection of body kinematics
308 * measurements with standard deviations
309 * taken at the same position with zero
310 * velocity and unknown different
311 * orientations.
312 * @param initialBias initial gyroscope bias to be used to
313 * find a solution. This must have length
314 * 3 and is expressed in radians per
315 * second (rad/s).
316 * @param initialMg initial gyroscope scale factors and
317 * cross coupling errors matrix. Must
318 * be 3x3.
319 * @param initialGg initial gyroscope G-dependent cross
320 * biases introduced on the gyroscope by
321 * the specific forces sensed by the
322 * accelerometer. Must be 3x3.
323 * @param accelerometerBias known accelerometer bias. This must
324 * have length 3 and is expressed in
325 * meters per squared second (m/s^2).
326 * @param accelerometerMa known accelerometer scale factors and
327 * cross coupling matrix. Must be 3x3.
328 * @param listener listener to handle events raised by
329 * this calibrator.
330 * @throws IllegalArgumentException if any of the provided values does
331 * not have proper size or if either
332 * turntable rotation rate or
333 * time interval is zero or negative.
334 */
335 public MSACRobustTurntableGyroscopeCalibrator(
336 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
337 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
338 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
339 final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener) {
340 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
341 accelerometerBias, accelerometerMa, listener);
342 }
343
344 /**
345 * Constructor.
346 *
347 * @param position position where body kinematics measures
348 * have been taken.
349 * @param turntableRotationRate constant rotation rate at which the
350 * turntable is spinning. Must be
351 * expressed in radians per second (rad/s).
352 * @param timeInterval time interval between measurements being
353 * captured expressed in seconds (s).
354 * @param measurements collection of body kinematics
355 * measurements with standard deviations
356 * taken at the same position with zero
357 * velocity and unknown different
358 * orientations.
359 * @param initialBias initial gyroscope bias to be used to
360 * find a solution. This must be 3x1 and
361 * is expressed in radians per second
362 * (rad/s).
363 * @param initialMg initial gyroscope scale factors and
364 * cross coupling errors matrix. Must
365 * be 3x3.
366 * @param initialGg initial gyroscope G-dependent cross
367 * biases introduced on the gyroscope by
368 * the specific forces sensed by the
369 * accelerometer. Must be 3x3.
370 * @param accelerometerBias known accelerometer bias. This must
371 * have length 3 and is expressed in
372 * meters per squared second
373 * (m/s^2).
374 * @param accelerometerMa known accelerometer scale factors and
375 * cross coupling matrix. Must be 3x3.
376 * @throws IllegalArgumentException if any of the provided values does
377 * not have proper size or if either
378 * turntable rotation rate or
379 * time interval is zero or negative.
380 */
381 public MSACRobustTurntableGyroscopeCalibrator(
382 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
383 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
384 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa) {
385 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
386 accelerometerBias, accelerometerMa);
387 }
388
389 /**
390 * Constructor.
391 *
392 * @param position position where body kinematics measures
393 * have been taken.
394 * @param turntableRotationRate constant rotation rate at which the
395 * turntable is spinning. Must be
396 * expressed in radians per second (rad/s).
397 * @param timeInterval time interval between measurements being
398 * captured expressed in seconds (s).
399 * @param measurements collection of body kinematics
400 * measurements with standard deviations
401 * taken at the same position with zero
402 * velocity and unknown different
403 * orientations.
404 * @param initialBias initial gyroscope bias to be used to
405 * find a solution. This must be 3x1 and
406 * is expressed in radians per second
407 * (rad/s).
408 * @param initialMg initial gyroscope scale factors and
409 * cross coupling errors matrix. Must
410 * be 3x3.
411 * @param initialGg initial gyroscope G-dependent cross
412 * biases introduced on the gyroscope by
413 * the specific forces sensed by the
414 * accelerometer. Must be 3x3.
415 * @param accelerometerBias known accelerometer bias. This must
416 * have length 3 and is expressed in
417 * meters per squared second (m/s^2).
418 * @param accelerometerMa known accelerometer scale factors and
419 * cross coupling matrix. Must be 3x3.
420 * @param listener listener to handle events raised by
421 * this calibrator.
422 * @throws IllegalArgumentException if any of the provided values does
423 * not have proper size or if either
424 * turntable rotation rate or
425 * time interval is zero or negative.
426 */
427 public MSACRobustTurntableGyroscopeCalibrator(
428 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
429 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
430 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
431 final RobustTurntableGyroscopeCalibratorListener listener) {
432 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
433 accelerometerBias, accelerometerMa, listener);
434 }
435
436 /**
437 * Constructor.
438 *
439 * @param position position where body kinematics
440 * measures have been taken.
441 * @param turntableRotationRate constant rotation rate at which
442 * the turntable is spinning. Must
443 * be expressed in radians per
444 * second (rad/s).
445 * @param timeInterval time interval between measurements
446 * being captured expressed in
447 * seconds (s).
448 * @param measurements collection of body kinematics
449 * measurements with standard
450 * deviations taken at the same
451 * position with zero velocity
452 * and unknown different
453 * orientations.
454 * @param commonAxisUsed indicates whether z-axis is
455 * assumed to be common for
456 * accelerometer and gyroscope.
457 * @param estimateGDependentCrossBiases true if G-dependent cross biases
458 * will be estimated, false
459 * otherwise.
460 * @param initialBias initial gyroscope bias to be
461 * used to find a solution. This
462 * must be 3x1 and is expressed in
463 * radians per second (rad/s).
464 * @param initialMg initial gyroscope scale factors
465 * and cross coupling errors matrix.
466 * Must be 3x3.
467 * @param initialGg initial gyroscope G-dependent
468 * cross biases introduced on the
469 * gyroscope by the specific
470 * forces sensed by the
471 * accelerometer. Must be 3x3.
472 * @throws IllegalArgumentException if any of the provided values does
473 * not have proper size or if either
474 * turntable rotation rate or
475 * time interval is zero or negative.
476 */
477 public MSACRobustTurntableGyroscopeCalibrator(
478 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
479 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
480 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
481 final Matrix initialGg) {
482 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
483 estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
484 }
485
486 /**
487 * Constructor.
488 *
489 * @param position position where body kinematics
490 * measures have been taken.
491 * @param turntableRotationRate constant rotation rate at which
492 * the turntable is spinning. Must
493 * be expressed in radians per
494 * second (rad/s).
495 * @param timeInterval time interval between measurements
496 * being captured expressed in
497 * seconds (s).
498 * @param measurements collection of body kinematics
499 * measurements with standard
500 * deviations taken at the same
501 * position with zero velocity and
502 * unknown different orientations.
503 * @param commonAxisUsed indicates whether z-axis is
504 * assumed to be common for
505 * accelerometer and gyroscope.
506 * @param estimateGDependentCrossBiases true if G-dependent cross
507 * biases will be estimated, false
508 * otherwise.
509 * @param initialBias initial gyroscope bias to be
510 * used to find a solution. This
511 * must be 3x1 and is expressed in
512 * radians per second (rad/s).
513 * @param initialMg initial gyroscope scale factors
514 * and cross coupling errors
515 * matrix. Must be 3x3.
516 * @param initialGg initial gyroscope G-dependent
517 * cross biases introduced on the
518 * gyroscope by the specific
519 * forces sensed by the
520 * accelerometer. Must be 3x3.
521 * @param listener listener to handle events
522 * raised by this calibrator.
523 * @throws IllegalArgumentException if any of the provided values does
524 * not have proper size or if either
525 * turntable rotation rate or
526 * time interval is zero or negative.
527 */
528 public MSACRobustTurntableGyroscopeCalibrator(
529 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
530 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
531 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
532 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
533 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
534 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
535 }
536
537 /**
538 * Constructor.
539 *
540 * @param position position where body kinematics
541 * measures have been taken.
542 * @param turntableRotationRate constant rotation rate at which
543 * the turntable is spinning. Must
544 * be expressed in radians per
545 * second (rad/s).
546 * @param timeInterval time interval between measurements
547 * being captured expressed in
548 * seconds (s).
549 * @param measurements collection of body kinematics
550 * measurements with standard
551 * deviations taken at the same
552 * position with zero velocity
553 * and unknown different
554 * orientations.
555 * @param commonAxisUsed indicates whether z-axis is
556 * assumed to be common for
557 * accelerometer and gyroscope.
558 * @param estimateGDependentCrossBiases true if G-dependent cross biases
559 * will be estimated, false
560 * otherwise.
561 * @param initialBias initial gyroscope bias to be
562 * used to find a solution. This
563 * must have length 3 and is
564 * expressed in radians per second
565 * (rad/s).
566 * @param initialMg initial gyroscope scale factors
567 * and cross coupling errors matrix.
568 * Must be 3x3.
569 * @param initialGg initial gyroscope G-dependent
570 * cross biases introduced on the
571 * gyroscope by the specific forces
572 * sensed by the accelerometer.
573 * Must be 3x3.
574 * @throws IllegalArgumentException if any of the provided values does
575 * not have proper size or if either
576 * turntable rotation rate or
577 * time interval is zero or negative.
578 */
579 public MSACRobustTurntableGyroscopeCalibrator(
580 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
581 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
582 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
583 final Matrix initialGg) {
584 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
585 estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
586 }
587
588 /**
589 * Constructor.
590 *
591 * @param position position where body kinematics
592 * measures have been taken.
593 * @param turntableRotationRate constant rotation rate at which
594 * the turntable is spinning. Must
595 * be expressed in radians per
596 * second (rad/s).
597 * @param timeInterval time interval between measurements
598 * being captured expressed in
599 * seconds (s).
600 * @param measurements collection of body kinematics
601 * measurements with standard
602 * deviations taken at the same
603 * position with zero velocity
604 * and unknown different
605 * orientations.
606 * @param commonAxisUsed indicates whether z-axis is
607 * assumed to be common for
608 * accelerometer and gyroscope.
609 * @param estimateGDependentCrossBiases true if G-dependent cross biases
610 * will be estimated, false
611 * otherwise.
612 * @param initialBias initial gyroscope bias to be
613 * used to find a solution. This
614 * must have length 3 and is
615 * expressed in radians per second
616 * (rad/s).
617 * @param initialMg initial gyroscope scale factors
618 * and cross coupling errors
619 * matrix. Must be 3x3.
620 * @param initialGg initial gyroscope G-dependent
621 * cross biases introduced on the
622 * gyroscope by the specific forces
623 * sensed by the accelerometer.
624 * Must be 3x3.
625 * @param listener listener to handle events raised
626 * by this calibrator.
627 * @throws IllegalArgumentException if any of the provided values does
628 * not have proper size or if either
629 * turntable rotation rate or
630 * time interval is zero or negative.
631 */
632 public MSACRobustTurntableGyroscopeCalibrator(
633 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
634 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
635 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
636 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
637 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
638 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
639 }
640
641 /**
642 * Constructor.
643 *
644 * @param position position where body kinematics
645 * measures have been taken.
646 * @param turntableRotationRate constant rotation rate at which
647 * the turntable is spinning. Must
648 * be expressed in radians per
649 * second (rad/s).
650 * @param timeInterval time interval between measurements
651 * being captured expressed in
652 * seconds (s).
653 * @param measurements collection of body kinematics
654 * measurements with standard
655 * deviations taken at the same
656 * position with zero velocity
657 * and unknown different
658 * orientations.
659 * @param commonAxisUsed indicates whether z-axis is
660 * assumed to be common for
661 * accelerometer and gyroscope.
662 * @param estimateGDependentCrossBiases true if G-dependent cross
663 * biases will be estimated,
664 * false otherwise.
665 * @param initialBias initial gyroscope bias to be
666 * used to find a solution. This
667 * must have length 3 and is
668 * expressed in radians per second
669 * (rad/s).
670 * @param initialMg initial gyroscope scale factors
671 * and cross coupling errors
672 * matrix. Must be 3x3.
673 * @param initialGg initial gyroscope G-dependent
674 * cross biases introduced on the
675 * gyroscope by the specific forces
676 * sensed by the accelerometer.
677 * Must be 3x3.
678 * @param accelerometerBias known accelerometer bias. This
679 * must have length 3 and is
680 * expressed in meters per squared
681 * second (m/s^2).
682 * @param accelerometerMa known accelerometer scale factors
683 * and cross coupling matrix. Must
684 * be 3x3.
685 * @throws IllegalArgumentException if any of the provided values does
686 * not have proper size or if either
687 * turntable rotation rate or
688 * time interval is zero or negative.
689 */
690 public MSACRobustTurntableGyroscopeCalibrator(
691 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
692 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
693 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
694 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa) {
695 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
696 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
697 }
698
699 /**
700 * Constructor.
701 *
702 * @param position position where body kinematics
703 * measures have been taken.
704 * @param turntableRotationRate constant rotation rate at which
705 * the turntable is spinning. Must
706 * be expressed in radians per
707 * second (rad/s).
708 * @param timeInterval time interval between measurements
709 * being captured expressed in
710 * seconds (s).
711 * @param measurements collection of body kinematics
712 * measurements with standard
713 * deviations taken at the same
714 * position with zero velocity
715 * and unknown different
716 * orientations.
717 * @param commonAxisUsed indicates whether z-axis is
718 * assumed to be common for
719 * accelerometer and gyroscope.
720 * @param estimateGDependentCrossBiases true if G-dependent cross biases
721 * will be estimated, false
722 * otherwise.
723 * @param initialBias initial gyroscope bias to be used
724 * to find a solution. This must
725 * have length 3 and is expressed
726 * in radians per second (rad/s).
727 * @param initialMg initial gyroscope scale factors
728 * and cross coupling errors matrix.
729 * Must be 3x3.
730 * @param initialGg initial gyroscope G-dependent
731 * cross biases introduced on the
732 * gyroscope by the specific forces
733 * sensed by the accelerometer. Must
734 * be 3x3.
735 * @param accelerometerBias known accelerometer bias. This
736 * must have length 3 and is
737 * expressed in meters per squared
738 * second (m/s^2).
739 * @param accelerometerMa known accelerometer scale factors
740 * and cross coupling matrix. Must
741 * be 3x3.
742 * @param listener listener to handle events raised
743 * by this calibrator.
744 * @throws IllegalArgumentException if any of the provided values does
745 * not have proper size or if either
746 * turntable rotation rate or
747 * time interval is zero or negative.
748 */
749 public MSACRobustTurntableGyroscopeCalibrator(
750 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
751 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
752 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
753 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
754 final RobustTurntableGyroscopeCalibratorListener listener) {
755 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
756 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
757 listener);
758 }
759
760 /**
761 * Constructor.
762 *
763 * @param position position where body kinematics
764 * measures have been taken.
765 * @param turntableRotationRate constant rotation rate at which
766 * the turntable is spinning. Must
767 * be expressed in radians per
768 * second (rad/s).
769 * @param timeInterval time interval between measurements
770 * being captured expressed in
771 * seconds (s).
772 * @param measurements collection of body kinematics
773 * measurements with standard
774 * deviations taken at the same
775 * position with zero velocity and
776 * unknown different orientations.
777 * @param commonAxisUsed indicates whether z-axis is
778 * assumed to be common for
779 * accelerometer and gyroscope.
780 * @param estimateGDependentCrossBiases true if G-dependent cross biases
781 * will be estimated, false
782 * otherwise.
783 * @param initialBias initial gyroscope bias to be
784 * used to find a solution. This
785 * must be 3x1 and is expressed in
786 * radians per second (rad/s).
787 * @param initialMg initial gyroscope scale factors
788 * and cross coupling errors matrix.
789 * Must be 3x3.
790 * @param initialGg initial gyroscope G-dependent
791 * cross biases introduced on the
792 * gyroscope by the specific forces
793 * sensed by the accelerometer. Must
794 * be 3x3.
795 * @param accelerometerBias known accelerometer bias. This
796 * must have length 3 and is
797 * expressed in meters per squared
798 * second (m/s^2).
799 * @param accelerometerMa known accelerometer scale factors
800 * and cross coupling matrix. Must
801 * be 3x3.
802 * @throws IllegalArgumentException if any of the provided values does
803 * not have proper size or if either
804 * turntable rotation rate or
805 * time interval is zero or negative.
806 */
807 public MSACRobustTurntableGyroscopeCalibrator(
808 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
809 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
810 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
811 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa) {
812 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
813 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
814 }
815
816 /**
817 * Constructor.
818 *
819 * @param position position where body kinematics
820 * measures have been taken.
821 * @param turntableRotationRate constant rotation rate at which
822 * the turntable is spinning. Must
823 * be expressed in radians per
824 * second (rad/s).
825 * @param timeInterval time interval between measurements
826 * being captured expressed in
827 * seconds (s).
828 * @param measurements collection of body kinematics
829 * measurements with standard
830 * deviations taken at the same
831 * position with zero velocity and
832 * unknown different orientations.
833 * @param commonAxisUsed indicates whether z-axis is
834 * assumed to be common for
835 * accelerometer and gyroscope.
836 * @param estimateGDependentCrossBiases true if G-dependent cross biases
837 * will be estimated, false
838 * otherwise.
839 * @param initialBias initial gyroscope bias to be used
840 * to find a solution. This must be
841 * 3x1 and is expressed in radians
842 * per second (rad/s).
843 * @param initialMg initial gyroscope scale factors
844 * and cross coupling errors matrix.
845 * Must be 3x3.
846 * @param initialGg initial gyroscope G-dependent
847 * cross biases introduced on the
848 * gyroscope by the specific forces
849 * sensed by the accelerometer. Must
850 * be 3x3.
851 * @param accelerometerBias known accelerometer bias. This
852 * must have length 3 and is
853 * expressed in meters per squared
854 * second (m/s^2).
855 * @param accelerometerMa known accelerometer scale factors
856 * and cross coupling matrix. Must
857 * be 3x3.
858 * @param listener listener to handle events raised
859 * by this calibrator.
860 * @throws IllegalArgumentException if any of the provided values does
861 * not have proper size or if either
862 * turntable rotation rate or
863 * time interval is zero or negative.
864 */
865 public MSACRobustTurntableGyroscopeCalibrator(
866 final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
867 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
868 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
869 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
870 final RobustTurntableGyroscopeCalibratorListener listener) {
871 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
872 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
873 listener);
874 }
875
876 /**
877 * Constructor.
878 *
879 * @param position position where body kinematics measures
880 * have been taken.
881 * @param turntableRotationRate constant rotation rate at which the
882 * turntable is spinning. Must be
883 * expressed in radians per second (rad/s).
884 * @param timeInterval time interval between measurements being
885 * captured expressed in seconds (s).
886 * @param measurements collection of body kinematics
887 * measurements with standard deviations
888 * taken at the same position with zero
889 * velocity and unknown different
890 * orientations.
891 * @param initialBias initial gyroscope bias to be used to
892 * find a solution. This must be 3x1 and
893 * is expressed in radians per second
894 * (rad/s).
895 * @param initialMg initial gyroscope scale factors and
896 * cross coupling errors matrix. Must
897 * be 3x3.
898 * @param initialGg initial gyroscope G-dependent cross
899 * biases introduced on the gyroscope by
900 * the specific forces sensed by the
901 * accelerometer. Must be 3x3.
902 * @throws IllegalArgumentException if any of the provided values does
903 * not have proper size or if either
904 * turntable rotation rate or
905 * time interval is zero or negative.
906 */
907 public MSACRobustTurntableGyroscopeCalibrator(
908 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
909 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
910 final Matrix initialGg) {
911 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg);
912 }
913
914 /**
915 * Constructor.
916 *
917 * @param position position where body kinematics measures
918 * have been taken.
919 * @param turntableRotationRate constant rotation rate at which the
920 * turntable is spinning. Must be
921 * expressed in radians per second (rad/s).
922 * @param timeInterval time interval between measurements being
923 * captured expressed in seconds (s).
924 * @param measurements collection of body kinematics
925 * measurements with standard deviations
926 * taken at the same position with zero
927 * velocity and unknown different
928 * orientations.
929 * @param initialBias initial gyroscope bias to be used to
930 * find a solution. This must be 3x1 and
931 * is expressed in radians per second
932 * (rad/s).
933 * @param initialMg initial gyroscope scale factors and
934 * cross coupling errors matrix. Must
935 * be 3x3.
936 * @param initialGg initial gyroscope G-dependent cross
937 * biases introduced on the gyroscope by
938 * the specific forces sensed by the
939 * accelerometer. Must be 3x3.
940 * @param listener listener to handle events raised by this
941 * calibrator.
942 * @throws IllegalArgumentException if any of the provided values does
943 * not have proper size or if either
944 * turntable rotation rate or
945 * time interval is zero or negative.
946 */
947 public MSACRobustTurntableGyroscopeCalibrator(
948 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
949 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
950 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
951 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg, listener);
952 }
953
954 /**
955 * Constructor.
956 *
957 * @param position position where body kinematics measures
958 * have been taken.
959 * @param turntableRotationRate constant rotation rate at which the
960 * turntable is spinning. Must be
961 * expressed in radians per second (rad/s).
962 * @param timeInterval time interval between measurements being
963 * captured expressed in seconds (s).
964 * @param measurements collection of body kinematics
965 * measurements with standard deviations
966 * taken at the same position with zero
967 * velocity and unknown different
968 * orientations.
969 * @param initialBias initial gyroscope bias to be used to
970 * find a solution. This must have
971 * length 3 and is expressed in radians
972 * per second (rad/s).
973 * @param initialMg initial gyroscope scale factors and
974 * cross coupling errors matrix. Must
975 * be 3x3.
976 * @param initialGg initial gyroscope G-dependent cross
977 * biases introduced on the gyroscope by
978 * the specific forces sensed by the
979 * accelerometer. Must be 3x3.
980 * @throws IllegalArgumentException if any of the provided values does
981 * not have proper size or if either
982 * turntable rotation rate or
983 * time interval is zero or negative.
984 */
985 public MSACRobustTurntableGyroscopeCalibrator(
986 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
987 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
988 final Matrix initialMg, final Matrix initialGg) {
989 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg);
990 }
991
992 /**
993 * Constructor.
994 *
995 * @param position position where body kinematics measures
996 * have been taken.
997 * @param turntableRotationRate constant rotation rate at which the
998 * turntable is spinning. Must be
999 * expressed in radians per second (rad/s).
1000 * @param timeInterval time interval between measurements being
1001 * captured expressed in seconds (s).
1002 * @param measurements collection of body kinematics
1003 * measurements with standard deviations
1004 * taken at the same position with zero
1005 * velocity and unknown different
1006 * orientations.
1007 * @param initialBias initial gyroscope bias to be used to
1008 * find a solution. This must have length
1009 * 3 and is expressed in radians
1010 * per second (rad/s).
1011 * @param initialMg initial gyroscope scale factors and
1012 * cross coupling errors matrix. Must
1013 * be 3x3.
1014 * @param initialGg initial gyroscope G-dependent cross
1015 * biases introduced on the gyroscope by
1016 * the specific forces sensed by the
1017 * accelerometer. Must be 3x3.
1018 * @param listener listener to handle events raised by
1019 * this calibrator.
1020 * @throws IllegalArgumentException if any of the provided values does
1021 * not have proper size or if either
1022 * turntable rotation rate or
1023 * time interval is zero or negative.
1024 */
1025 public MSACRobustTurntableGyroscopeCalibrator(
1026 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1027 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
1028 final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
1029 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg, listener);
1030 }
1031
1032 /**
1033 * Constructor.
1034 *
1035 * @param position position where body kinematics measures
1036 * have been taken.
1037 * @param turntableRotationRate constant rotation rate at which the
1038 * turntable is spinning. Must be
1039 * expressed in radians per second (rad/s).
1040 * @param timeInterval time interval between measurements being
1041 * captured expressed in seconds (s).
1042 * @param measurements collection of body kinematics
1043 * measurements with standard deviations
1044 * taken at the same position with zero
1045 * velocity and unknown different
1046 * orientations.
1047 * @param initialBias initial gyroscope bias to be used to
1048 * find a solution. This must have length
1049 * 3 and is expressed in radians per
1050 * second (rad/s).
1051 * @param initialMg initial gyroscope scale factors and
1052 * cross coupling errors matrix. Must
1053 * be 3x3.
1054 * @param initialGg initial gyroscope G-dependent cross
1055 * biases introduced on the gyroscope by
1056 * the specific forces sensed by the
1057 * accelerometer. Must be 3x3.
1058 * @param accelerometerBias known accelerometer bias. This must
1059 * have length 3 and is expressed in
1060 * meters per squared second
1061 * (m/s^2).
1062 * @param accelerometerMa known accelerometer scale factors and
1063 * cross coupling matrix. Must be 3x3.
1064 * @throws IllegalArgumentException if any of the provided values does
1065 * not have proper size or if either
1066 * turntable rotation rate or
1067 * time interval is zero or negative.
1068 */
1069 public MSACRobustTurntableGyroscopeCalibrator(
1070 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1071 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
1072 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
1073 final Matrix accelerometerMa) {
1074 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
1075 accelerometerBias, accelerometerMa);
1076 }
1077
1078 /**
1079 * Constructor.
1080 *
1081 * @param position position where body kinematics measures
1082 * have been taken.
1083 * @param turntableRotationRate constant rotation rate at which the
1084 * turntable is spinning. Must be
1085 * expressed in radians per second (rad/s).
1086 * @param timeInterval time interval between measurements being
1087 * captured expressed in seconds (s).
1088 * @param measurements collection of body kinematics
1089 * measurements with standard deviations
1090 * taken at the same position with zero
1091 * velocity and unknown different
1092 * orientations.
1093 * @param initialBias initial gyroscope bias to be used to
1094 * find a solution. This must have length
1095 * 3 and is expressed in radians per
1096 * second (rad/s).
1097 * @param initialMg initial gyroscope scale factors and
1098 * cross coupling errors matrix. Must
1099 * be 3x3.
1100 * @param initialGg initial gyroscope G-dependent cross
1101 * biases introduced on the gyroscope by
1102 * the specific forces sensed by the
1103 * accelerometer. Must be 3x3.
1104 * @param accelerometerBias known accelerometer bias. This must
1105 * have length 3 and is expressed in
1106 * meters per squared second (m/s^2).
1107 * @param accelerometerMa known accelerometer scale factors and
1108 * cross coupling matrix. Must be 3x3.
1109 * @param listener listener to handle events raised by
1110 * this calibrator.
1111 * @throws IllegalArgumentException if any of the provided values does
1112 * not have proper size or if either
1113 * turntable rotation rate or
1114 * time interval is zero or negative.
1115 */
1116 public MSACRobustTurntableGyroscopeCalibrator(
1117 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1118 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
1119 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
1120 final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener) {
1121 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
1122 accelerometerBias, accelerometerMa, listener);
1123 }
1124
1125 /**
1126 * Constructor.
1127 *
1128 * @param position position where body kinematics measures
1129 * have been taken.
1130 * @param turntableRotationRate constant rotation rate at which the
1131 * turntable is spinning. Must be
1132 * expressed in radians per second (rad/s).
1133 * @param timeInterval time interval between measurements being
1134 * captured expressed in seconds (s).
1135 * @param measurements collection of body kinematics
1136 * measurements with standard deviations
1137 * taken at the same position with zero
1138 * velocity and unknown different
1139 * orientations.
1140 * @param initialBias initial gyroscope bias to be used to
1141 * find a solution. This must be 3x1 and
1142 * is expressed in radians per second
1143 * (rad/s).
1144 * @param initialMg initial gyroscope scale factors and
1145 * cross coupling errors matrix. Must
1146 * be 3x3.
1147 * @param initialGg initial gyroscope G-dependent cross
1148 * biases introduced on the gyroscope by
1149 * the specific forces sensed by the
1150 * accelerometer. Must be 3x3.
1151 * @param accelerometerBias known accelerometer bias. This must
1152 * have length 3 and is expressed in
1153 * meters per squared second
1154 * (m/s^2).
1155 * @param accelerometerMa known accelerometer scale factors and
1156 * cross coupling matrix. Must be 3x3.
1157 * @throws IllegalArgumentException if any of the provided values does
1158 * not have proper size or if either
1159 * turntable rotation rate or
1160 * time interval is zero or negative.
1161 */
1162 public MSACRobustTurntableGyroscopeCalibrator(
1163 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1164 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
1165 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa) {
1166 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
1167 accelerometerBias, accelerometerMa);
1168 }
1169
1170 /**
1171 * Constructor.
1172 *
1173 * @param position position where body kinematics measures
1174 * have been taken.
1175 * @param turntableRotationRate constant rotation rate at which the
1176 * turntable is spinning. Must be
1177 * expressed in radians per second (rad/s).
1178 * @param timeInterval time interval between measurements being
1179 * captured expressed in seconds (s).
1180 * @param measurements collection of body kinematics
1181 * measurements with standard deviations
1182 * taken at the same position with zero
1183 * velocity and unknown different
1184 * orientations.
1185 * @param initialBias initial gyroscope bias to be used to
1186 * find a solution. This must be 3x1 and
1187 * is expressed in radians per second
1188 * (rad/s).
1189 * @param initialMg initial gyroscope scale factors and
1190 * cross coupling errors matrix. Must
1191 * be 3x3.
1192 * @param initialGg initial gyroscope G-dependent cross
1193 * biases introduced on the gyroscope by
1194 * the specific forces sensed by the
1195 * accelerometer. Must be 3x3.
1196 * @param accelerometerBias known accelerometer bias. This must
1197 * have length 3 and is expressed in
1198 * meters per squared second (m/s^2).
1199 * @param accelerometerMa known accelerometer scale factors and
1200 * cross coupling matrix. Must be 3x3.
1201 * @param listener listener to handle events raised by
1202 * this calibrator.
1203 * @throws IllegalArgumentException if any of the provided values does
1204 * not have proper size or if either
1205 * turntable rotation rate or
1206 * time interval is zero or negative.
1207 */
1208 public MSACRobustTurntableGyroscopeCalibrator(
1209 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1210 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
1211 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
1212 final RobustTurntableGyroscopeCalibratorListener listener) {
1213 super(position, turntableRotationRate, timeInterval, measurements, initialBias, initialMg, initialGg,
1214 accelerometerBias, accelerometerMa, listener);
1215 }
1216
1217 /**
1218 * Constructor.
1219 *
1220 * @param position position where body kinematics
1221 * measures have been taken.
1222 * @param turntableRotationRate constant rotation rate at which
1223 * the turntable is spinning. Must
1224 * be expressed in radians per
1225 * second (rad/s).
1226 * @param timeInterval time interval between measurements
1227 * being captured expressed in
1228 * seconds (s).
1229 * @param measurements collection of body kinematics
1230 * measurements with standard
1231 * deviations taken at the same
1232 * position with zero velocity
1233 * and unknown different
1234 * orientations.
1235 * @param commonAxisUsed indicates whether z-axis is
1236 * assumed to be common for
1237 * accelerometer and gyroscope.
1238 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1239 * will be estimated, false
1240 * otherwise.
1241 * @param initialBias initial gyroscope bias to be
1242 * used to find a solution. This
1243 * must be 3x1 and is expressed in
1244 * radians per second (rad/s).
1245 * @param initialMg initial gyroscope scale factors
1246 * and cross coupling errors matrix.
1247 * Must be 3x3.
1248 * @param initialGg initial gyroscope G-dependent
1249 * cross biases introduced on the
1250 * gyroscope by the specific
1251 * forces sensed by the
1252 * accelerometer. Must 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 public MSACRobustTurntableGyroscopeCalibrator(
1259 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1260 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1261 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
1262 final Matrix initialGg) {
1263 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1264 estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
1265 }
1266
1267 /**
1268 * Constructor.
1269 *
1270 * @param position position where body kinematics
1271 * measures have been taken.
1272 * @param turntableRotationRate constant rotation rate at which
1273 * the turntable is spinning. Must
1274 * be expressed in radians per
1275 * second (rad/s).
1276 * @param timeInterval time interval between measurements
1277 * being captured expressed in
1278 * seconds (s).
1279 * @param measurements collection of body kinematics
1280 * measurements with standard
1281 * deviations taken at the same
1282 * position with zero velocity and
1283 * unknown different orientations.
1284 * @param commonAxisUsed indicates whether z-axis is
1285 * assumed to be common for
1286 * accelerometer and gyroscope.
1287 * @param estimateGDependentCrossBiases true if G-dependent cross
1288 * biases will be estimated, false
1289 * otherwise.
1290 * @param initialBias initial gyroscope bias to be
1291 * used to find a solution. This
1292 * must be 3x1 and is expressed in
1293 * radians per second (rad/s).
1294 * @param initialMg initial gyroscope scale factors
1295 * and cross coupling errors
1296 * matrix. Must be 3x3.
1297 * @param initialGg initial gyroscope G-dependent
1298 * cross biases introduced on the
1299 * gyroscope by the specific
1300 * forces sensed by the
1301 * accelerometer. Must be 3x3.
1302 * @param listener listener to handle events
1303 * raised by this calibrator.
1304 * @throws IllegalArgumentException if any of the provided values does
1305 * not have proper size or if either
1306 * turntable rotation rate or
1307 * time interval is zero or negative.
1308 */
1309 public MSACRobustTurntableGyroscopeCalibrator(
1310 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1311 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1312 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
1313 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
1314 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1315 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
1316 }
1317
1318 /**
1319 * Constructor.
1320 *
1321 * @param position position where body kinematics
1322 * measures have been taken.
1323 * @param turntableRotationRate constant rotation rate at which
1324 * the turntable is spinning. Must
1325 * be expressed in radians per
1326 * second (rad/s).
1327 * @param timeInterval time interval between measurements
1328 * being captured expressed in
1329 * seconds (s).
1330 * @param measurements collection of body kinematics
1331 * measurements with standard
1332 * deviations taken at the same
1333 * position with zero velocity
1334 * and unknown different
1335 * orientations.
1336 * @param commonAxisUsed indicates whether z-axis is
1337 * assumed to be common for
1338 * accelerometer and gyroscope.
1339 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1340 * will be estimated, false
1341 * otherwise.
1342 * @param initialBias initial gyroscope bias to be
1343 * used to find a solution. This
1344 * must have length 3 and is
1345 * expressed in radians per second
1346 * (rad/s).
1347 * @param initialMg initial gyroscope scale factors
1348 * and cross coupling errors matrix.
1349 * Must be 3x3.
1350 * @param initialGg initial gyroscope G-dependent
1351 * cross biases introduced on the
1352 * gyroscope by the specific forces
1353 * sensed by the accelerometer.
1354 * Must be 3x3.
1355 * @throws IllegalArgumentException if any of the provided values does
1356 * not have proper size or if either
1357 * turntable rotation rate or
1358 * time interval is zero or negative.
1359 */
1360 public MSACRobustTurntableGyroscopeCalibrator(
1361 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1362 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1363 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
1364 final Matrix initialGg) {
1365 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1366 estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
1367 }
1368
1369 /**
1370 * Constructor.
1371 *
1372 * @param position position where body kinematics
1373 * measures have been taken.
1374 * @param turntableRotationRate constant rotation rate at which
1375 * the turntable is spinning. Must
1376 * be expressed in radians per
1377 * second (rad/s).
1378 * @param timeInterval time interval between measurements
1379 * being captured expressed in
1380 * seconds (s).
1381 * @param measurements collection of body kinematics
1382 * measurements with standard
1383 * deviations taken at the same
1384 * position with zero velocity
1385 * and unknown different
1386 * orientations.
1387 * @param commonAxisUsed indicates whether z-axis is
1388 * assumed to be common for
1389 * accelerometer and gyroscope.
1390 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1391 * will be estimated, false
1392 * otherwise.
1393 * @param initialBias initial gyroscope bias to be
1394 * used to find a solution. This
1395 * must have length 3 and is
1396 * expressed in radians per second
1397 * (rad/s).
1398 * @param initialMg initial gyroscope scale factors
1399 * and cross coupling errors
1400 * matrix. Must be 3x3.
1401 * @param initialGg initial gyroscope G-dependent
1402 * cross biases introduced on the
1403 * gyroscope by the specific forces
1404 * sensed by the accelerometer.
1405 * Must be 3x3.
1406 * @param listener listener to handle events raised
1407 * by this calibrator.
1408 * @throws IllegalArgumentException if any of the provided values does
1409 * not have proper size or if either
1410 * turntable rotation rate or
1411 * time interval is zero or negative.
1412 */
1413 public MSACRobustTurntableGyroscopeCalibrator(
1414 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1415 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1416 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
1417 final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener) {
1418 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1419 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
1420 }
1421
1422 /**
1423 * Constructor.
1424 *
1425 * @param position position where body kinematics
1426 * measures have been taken.
1427 * @param turntableRotationRate constant rotation rate at which
1428 * the turntable is spinning. Must
1429 * be expressed in radians per
1430 * second (rad/s).
1431 * @param timeInterval time interval between measurements
1432 * being captured expressed in
1433 * seconds (s).
1434 * @param measurements collection of body kinematics
1435 * measurements with standard
1436 * deviations taken at the same
1437 * position with zero velocity
1438 * and unknown different
1439 * orientations.
1440 * @param commonAxisUsed indicates whether z-axis is
1441 * assumed to be common for
1442 * accelerometer and gyroscope.
1443 * @param estimateGDependentCrossBiases true if G-dependent cross
1444 * biases will be estimated,
1445 * false otherwise.
1446 * @param initialBias initial gyroscope bias to be
1447 * used to find a solution. This
1448 * must have length 3 and is
1449 * expressed in radians per second
1450 * (rad/s).
1451 * @param initialMg initial gyroscope scale factors
1452 * and cross coupling errors
1453 * matrix. Must be 3x3.
1454 * @param initialGg initial gyroscope G-dependent
1455 * cross biases introduced on the
1456 * gyroscope by the specific forces
1457 * sensed by the accelerometer.
1458 * Must be 3x3.
1459 * @param accelerometerBias known accelerometer bias. This
1460 * must have length 3 and is
1461 * expressed in meters per squared
1462 * second (m/s^2).
1463 * @param accelerometerMa known accelerometer scale factors
1464 * and cross coupling matrix. Must
1465 * be 3x3.
1466 * @throws IllegalArgumentException if any of the provided values does
1467 * not have proper size or if either
1468 * turntable rotation rate or
1469 * time interval is zero or negative.
1470 */
1471 public MSACRobustTurntableGyroscopeCalibrator(
1472 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1473 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1474 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
1475 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa) {
1476 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1477 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
1478 }
1479
1480 /**
1481 * Constructor.
1482 *
1483 * @param position position where body kinematics
1484 * measures have been taken.
1485 * @param turntableRotationRate constant rotation rate at which
1486 * the turntable is spinning. Must
1487 * be expressed in radians per
1488 * second (rad/s).
1489 * @param timeInterval time interval between measurements
1490 * being captured expressed in
1491 * seconds (s).
1492 * @param measurements collection of body kinematics
1493 * measurements with standard
1494 * deviations taken at the same
1495 * position with zero velocity
1496 * and unknown different
1497 * orientations.
1498 * @param commonAxisUsed indicates whether z-axis is
1499 * assumed to be common for
1500 * accelerometer and gyroscope.
1501 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1502 * will be estimated, false
1503 * otherwise.
1504 * @param initialBias initial gyroscope bias to be used
1505 * to find a solution. This must
1506 * have length 3 and is expressed
1507 * in radians per second (rad/s).
1508 * @param initialMg initial gyroscope scale factors
1509 * and cross coupling errors matrix.
1510 * Must be 3x3.
1511 * @param initialGg initial gyroscope G-dependent
1512 * cross biases introduced on the
1513 * gyroscope by the specific forces
1514 * sensed by the accelerometer. Must
1515 * be 3x3.
1516 * @param accelerometerBias known accelerometer bias. This
1517 * must have length 3 and is
1518 * expressed in meters per squared
1519 * second (m/s^2).
1520 * @param accelerometerMa known accelerometer scale factors
1521 * and cross coupling matrix. Must
1522 * be 3x3.
1523 * @param listener listener to handle events raised
1524 * by this calibrator.
1525 * @throws IllegalArgumentException if any of the provided values does
1526 * not have proper size or if either
1527 * turntable rotation rate or
1528 * time interval is zero or negative.
1529 */
1530 public MSACRobustTurntableGyroscopeCalibrator(
1531 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1532 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1533 final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
1534 final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
1535 final RobustTurntableGyroscopeCalibratorListener listener) {
1536 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1537 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
1538 listener);
1539 }
1540
1541 /**
1542 * Constructor.
1543 *
1544 * @param position position where body kinematics
1545 * measures have been taken.
1546 * @param turntableRotationRate constant rotation rate at which
1547 * the turntable is spinning. Must
1548 * be expressed in radians per
1549 * second (rad/s).
1550 * @param timeInterval time interval between measurements
1551 * being captured expressed in
1552 * seconds (s).
1553 * @param measurements collection of body kinematics
1554 * measurements with standard
1555 * deviations taken at the same
1556 * position with zero velocity and
1557 * unknown different orientations.
1558 * @param commonAxisUsed indicates whether z-axis is
1559 * assumed to be common for
1560 * accelerometer and gyroscope.
1561 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1562 * will be estimated, false
1563 * otherwise.
1564 * @param initialBias initial gyroscope bias to be
1565 * used to find a solution. This
1566 * must be 3x1 and is expressed in
1567 * radians per second (rad/s).
1568 * @param initialMg initial gyroscope scale factors
1569 * and cross coupling errors matrix.
1570 * Must be 3x3.
1571 * @param initialGg initial gyroscope G-dependent
1572 * cross biases introduced on the
1573 * gyroscope by the specific forces
1574 * sensed by the accelerometer. Must
1575 * be 3x3.
1576 * @param accelerometerBias known accelerometer bias. This
1577 * must have length 3 and is
1578 * expressed in meters per squared
1579 * second (m/s^2).
1580 * @param accelerometerMa known accelerometer scale factors
1581 * and cross coupling matrix. Must
1582 * be 3x3.
1583 * @throws IllegalArgumentException if any of the provided values does
1584 * not have proper size or if either
1585 * turntable rotation rate or
1586 * time interval is zero or negative.
1587 */
1588 public MSACRobustTurntableGyroscopeCalibrator(
1589 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1590 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1591 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
1592 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa) {
1593 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1594 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
1595 }
1596
1597 /**
1598 * Constructor.
1599 *
1600 * @param position position where body kinematics
1601 * measures have been taken.
1602 * @param turntableRotationRate constant rotation rate at which
1603 * the turntable is spinning. Must
1604 * be expressed in radians per
1605 * second (rad/s).
1606 * @param timeInterval time interval between measurements
1607 * being captured expressed in
1608 * seconds (s).
1609 * @param measurements collection of body kinematics
1610 * measurements with standard
1611 * deviations taken at the same
1612 * position with zero velocity and
1613 * unknown different orientations.
1614 * @param commonAxisUsed indicates whether z-axis is
1615 * assumed to be common for
1616 * accelerometer and gyroscope.
1617 * @param estimateGDependentCrossBiases true if G-dependent cross biases
1618 * will be estimated, false
1619 * otherwise.
1620 * @param initialBias initial gyroscope bias to be used
1621 * to find a solution. This must be
1622 * 3x1 and is expressed in radians
1623 * per second (rad/s).
1624 * @param initialMg initial gyroscope scale factors
1625 * and cross coupling errors matrix.
1626 * Must be 3x3.
1627 * @param initialGg initial gyroscope G-dependent
1628 * cross biases introduced on the
1629 * gyroscope by the specific forces
1630 * sensed by the accelerometer. Must
1631 * be 3x3.
1632 * @param accelerometerBias known accelerometer bias. This
1633 * must have length 3 and is
1634 * expressed in meters per squared
1635 * second (m/s^2).
1636 * @param accelerometerMa known accelerometer scale factors
1637 * and cross coupling matrix. Must
1638 * be 3x3.
1639 * @param listener listener to handle events raised
1640 * by this calibrator.
1641 * @throws IllegalArgumentException if any of the provided values does
1642 * not have proper size or if either
1643 * turntable rotation rate or
1644 * time interval is zero or negative.
1645 */
1646 public MSACRobustTurntableGyroscopeCalibrator(
1647 final NEDPosition position, final double turntableRotationRate, final double timeInterval,
1648 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1649 final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
1650 final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
1651 final RobustTurntableGyroscopeCalibratorListener listener) {
1652 super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
1653 estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
1654 listener);
1655 }
1656
1657 /**
1658 * Returns threshold to determine whether samples are inliers or not.
1659 *
1660 * @return threshold to determine whether samples are inliers or not.
1661 */
1662 public double getThreshold() {
1663 return threshold;
1664 }
1665
1666 /**
1667 * Sets threshold to determine whether samples are inliers or not.
1668 *
1669 * @param threshold threshold to be set.
1670 * @throws IllegalArgumentException if provided value is equal or less than
1671 * zero.
1672 * @throws LockedException if calibrator is currently running.
1673 */
1674 public void setThreshold(final double threshold) throws LockedException {
1675 if (running) {
1676 throw new LockedException();
1677 }
1678 if (threshold <= MIN_THRESHOLD) {
1679 throw new IllegalArgumentException();
1680 }
1681 this.threshold = threshold;
1682 }
1683
1684 /**
1685 * Estimates gyroscope calibration parameters containing bias, scale factors
1686 * cross-coupling errors and g-dependant cross biases.
1687 *
1688 * @throws LockedException if calibrator is currently running.
1689 * @throws NotReadyException if calibrator is not ready.
1690 * @throws CalibrationException if estimation fails for numerical reasons.
1691 */
1692 @Override
1693 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
1694 if (running) {
1695 throw new LockedException();
1696 }
1697 if (!isReady()) {
1698 throw new NotReadyException();
1699 }
1700
1701 final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
1702 @Override
1703 public double getThreshold() {
1704 return threshold;
1705 }
1706
1707 @Override
1708 public int getTotalSamples() {
1709 return measurements.size();
1710 }
1711
1712 @Override
1713 public int getSubsetSize() {
1714 return preliminarySubsetSize;
1715 }
1716
1717 @Override
1718 public void estimatePreliminarSolutions(
1719 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
1720 computePreliminarySolutions(samplesIndices, solutions);
1721 }
1722
1723 @Override
1724 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
1725 return computeError(measurements.get(i), currentEstimation);
1726 }
1727
1728 @Override
1729 public boolean isReady() {
1730 return MSACRobustTurntableGyroscopeCalibrator.super.isReady();
1731 }
1732
1733 @Override
1734 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
1735 // no action needed
1736 }
1737
1738 @Override
1739 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
1740 // no action needed
1741 }
1742
1743 @Override
1744 public void onEstimateNextIteration(
1745 final RobustEstimator<PreliminaryResult> estimator, final int iteration) {
1746 if (listener != null) {
1747 listener.onCalibrateNextIteration(
1748 MSACRobustTurntableGyroscopeCalibrator.this, iteration);
1749 }
1750 }
1751
1752 @Override
1753 public void onEstimateProgressChange(
1754 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
1755 if (listener != null) {
1756 listener.onCalibrateProgressChange(
1757 MSACRobustTurntableGyroscopeCalibrator.this, progress);
1758 }
1759 }
1760 });
1761
1762 try {
1763 running = true;
1764
1765 if (listener != null) {
1766 listener.onCalibrateStart(this);
1767 }
1768
1769 inliersData = null;
1770 innerEstimator.setConfidence(confidence);
1771 innerEstimator.setMaxIterations(maxIterations);
1772 innerEstimator.setProgressDelta(progressDelta);
1773 final var preliminaryResult = innerEstimator.estimate();
1774 inliersData = innerEstimator.getInliersData();
1775
1776 attemptRefine(preliminaryResult);
1777
1778 if (listener != null) {
1779 listener.onCalibrateEnd(this);
1780 }
1781
1782 } catch (final com.irurueta.numerical.LockedException e) {
1783 throw new LockedException(e);
1784 } catch (final com.irurueta.numerical.NotReadyException e) {
1785 throw new NotReadyException(e);
1786 } catch (final RobustEstimatorException e) {
1787 throw new CalibrationException(e);
1788 } finally {
1789 running = false;
1790 }
1791 }
1792
1793 /**
1794 * Returns method being used for robust estimation.
1795 *
1796 * @return method being used for robust estimation.
1797 */
1798 @Override
1799 public RobustEstimatorMethod getMethod() {
1800 return RobustEstimatorMethod.MSAC;
1801 }
1802
1803 /**
1804 * Indicates whether this calibrator requires quality scores for each
1805 * measurement/sequence or not.
1806 *
1807 * @return true if quality scores are required, false otherwise.
1808 */
1809 @Override
1810 public boolean isQualityScoresRequired() {
1811 return false;
1812 }
1813 }