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