1 /*
2 * Copyright (C) 2020 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.navigation.inertial.calibration.gyroscope;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.navigation.LockedException;
21 import com.irurueta.navigation.NotReadyException;
22 import com.irurueta.navigation.inertial.calibration.BodyKinematicsSequence;
23 import com.irurueta.navigation.inertial.calibration.CalibrationException;
24 import com.irurueta.navigation.inertial.calibration.StandardDeviationTimedBodyKinematics;
25 import com.irurueta.numerical.robust.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 * <p>
38 * This calibrator assumes that the IMU is at a more or less fixed location on
39 * Earth, and evaluates sequences of measured body kinematics to perform
40 * calibration for unknown orientations on those provided sequences.
41 * Each provided sequence will be preceded by a static period where mean
42 * specific force will be measured to determine gravity (and hence partial
43 * body attitude).
44 * <p>
45 * Measured gyroscope angular rates is assumed to follow the model shown below:
46 * <pre>
47 * Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
48 * </pre>
49 * Where:
50 * - Ωmeas is the measured gyroscope angular rates. This is a 3x1 vector.
51 * - bg is the gyroscope bias. Ideally, on a perfect gyroscope, this should be a
52 * 3x1 zero vector.
53 * - I is the 3x3 identity matrix.
54 * - Mg is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
55 * a perfect gyroscope, this should be a 3x3 zero matrix.
56 * - Ωtrue is ground-truth gyroscope angular rates.
57 * - Gg is the G-dependent cross biases introduced by the specific forces sensed
58 * by the accelerometer. Ideally, on a perfect gyroscope, this should be a 3x3
59 * zero matrix.
60 * - ftrue is ground-truth specific force. This is a 3x1 vector.
61 * - w is measurement noise. This is a 3x1 vector.
62 */
63 public class MSACRobustKnownBiasEasyGyroscopeCalibrator extends RobustKnownBiasEasyGyroscopeCalibrator {
64
65 /**
66 * Constant defining default threshold to determine whether samples are
67 * inliers or not.
68 */
69 public static final double DEFAULT_THRESHOLD = 1e-3;
70
71 /**
72 * Minimum value that can be set as threshold.
73 * Threshold must be strictly greater than 0.0.
74 */
75 public static final double MIN_THRESHOLD = 0.0;
76
77 /**
78 * Threshold to determine whether samples are inliers or not when
79 * testing possible estimation solutions.
80 */
81 private double threshold = DEFAULT_THRESHOLD;
82
83 /**
84 * Constructor.
85 */
86 public MSACRobustKnownBiasEasyGyroscopeCalibrator() {
87 super();
88 }
89
90 /**
91 * Constructor.
92 *
93 * @param sequences collection of sequences containing timestamped body
94 * kinematics measurements.
95 * @param bias gyroscope known bias. This must be 3x1 and is
96 * expressed in radians per second (rad/s).
97 * @param initialMg initial gyroscope scale factors and cross coupling
98 * errors matrix. Must be 3x3.
99 * @param initialGg initial gyroscope G-dependent cross biases
100 * introduced on the gyroscope by the specific forces
101 * sensed by the accelerometer. Must be 3x3.
102 * @throws IllegalArgumentException if any of the provided values does
103 * not have proper size.
104 */
105 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
106 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences, final Matrix bias,
107 final Matrix initialMg, final Matrix initialGg) {
108 super(sequences, bias, initialMg, initialGg);
109 }
110
111 /**
112 * Constructor.
113 *
114 * @param sequences collection of sequences containing timestamped body
115 * kinematics measurements.
116 * @param bias gyroscope known bias. This must be 3x1 and is
117 * expressed in radians per second (rad/s).
118 * @param initialMg initial gyroscope scale factors and cross coupling
119 * errors matrix. Must be 3x3.
120 * @param initialGg initial gyroscope G-dependent cross biases
121 * introduced on the gyroscope by the specific forces
122 * sensed by the accelerometer. Must be 3x3.
123 * @param listener listener to handle events raised by this
124 * calibrator.
125 * @throws IllegalArgumentException if any of the provided values does
126 * not have proper size.
127 */
128 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
129 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences, final Matrix bias,
130 final Matrix initialMg, final Matrix initialGg,
131 final RobustKnownBiasEasyGyroscopeCalibratorListener listener) {
132 super(sequences, bias, initialMg, initialGg, listener);
133 }
134
135 /**
136 * Constructor.
137 *
138 * @param sequences collection of sequences containing timestamped body
139 * kinematics measurements.
140 * @param bias gyroscope known bias. This must have length 3 and is
141 * expressed in radians per second (rad/s).
142 * @param initialMg initial gyroscope scale factors and cross coupling
143 * errors matrix. Must be 3x3.
144 * @param initialGg initial gyroscope G-dependent cross biases
145 * introduced on the gyroscope by the specific forces
146 * sensed by the accelerometer. Must be 3x3.
147 * @throws IllegalArgumentException if any of the provided values does
148 * not have proper size.
149 */
150 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
151 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences, final double[] bias,
152 final Matrix initialMg, final Matrix initialGg) {
153 super(sequences, bias, initialMg, initialGg);
154 }
155
156 /**
157 * Constructor.
158 *
159 * @param sequences collection of sequences containing timestamped body
160 * kinematics measurements.
161 * @param bias gyroscope known bias. This must have length 3 and is
162 * expressed in radians per second (rad/s).
163 * @param initialMg initial gyroscope scale factors and cross coupling
164 * errors matrix. Must be 3x3.
165 * @param initialGg initial gyroscope G-dependent cross biases
166 * introduced on the gyroscope by the specific forces
167 * sensed by the accelerometer. Must be 3x3.
168 * @param listener listener to handle events raised by this
169 * calibrator.
170 * @throws IllegalArgumentException if any of the provided values does
171 * not have proper size.
172 */
173 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
174 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences, final double[] bias,
175 final Matrix initialMg, final Matrix initialGg,
176 final RobustKnownBiasEasyGyroscopeCalibratorListener listener) {
177 super(sequences, bias, initialMg, initialGg, listener);
178 }
179
180 /**
181 * Constructor.
182 *
183 * @param sequences collection of sequences containing timestamped body
184 * kinematics measurements.
185 * @param bias gyroscope known bias. This must have length 3 and is
186 * expressed in radians per second (rad/s).
187 * @param initialMg initial gyroscope scale factors and cross coupling
188 * errors matrix. Must be 3x3.
189 * @param initialGg initial gyroscope G-dependent cross biases
190 * introduced on the gyroscope by the specific forces
191 * sensed by the accelerometer. Must be 3x3.
192 * @param accelerometerBias known accelerometer bias. This must
193 * have length 3 and is expressed in
194 * meters per squared second
195 * (m/s^2).
196 * @param accelerometerMa known accelerometer scale factors and
197 * cross coupling matrix. Must be 3x3.
198 * @throws IllegalArgumentException if any of the provided values does
199 * not have proper size.
200 */
201 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
202 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences, final double[] bias,
203 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
204 final Matrix accelerometerMa) {
205 super(sequences, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
206 }
207
208 /**
209 * Constructor.
210 *
211 * @param sequences collection of sequences containing timestamped body
212 * kinematics measurements.
213 * @param bias gyroscope known bias. This must have length 3 and is
214 * expressed in radians per second (rad/s).
215 * @param initialMg initial gyroscope scale factors and cross coupling
216 * errors matrix. Must be 3x3.
217 * @param initialGg initial gyroscope G-dependent cross biases
218 * introduced on the gyroscope by the specific forces
219 * sensed by the accelerometer. Must be 3x3.
220 * @param accelerometerBias known accelerometer bias. This must
221 * have length 3 and is expressed in
222 * meters per squared second
223 * (m/s^2).
224 * @param accelerometerMa known accelerometer scale factors and
225 * cross coupling matrix. Must be 3x3.
226 * @param listener listener to handle events raised by this
227 * calibrator.
228 * @throws IllegalArgumentException if any of the provided values does
229 * not have proper size.
230 */
231 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
232 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences, final double[] bias,
233 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
234 final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener) {
235 super(sequences, bias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
236 }
237
238 /**
239 * Constructor.
240 *
241 * @param sequences collection of sequences containing timestamped body
242 * kinematics measurements.
243 * @param bias gyroscope known bias. This must be 3x1 and is
244 * expressed in radians per second (rad/s).
245 * @param initialMg initial gyroscope scale factors and cross coupling
246 * errors matrix. Must be 3x3.
247 * @param initialGg initial gyroscope G-dependent cross biases
248 * introduced on the gyroscope by the specific forces
249 * sensed by the accelerometer. Must be 3x3.
250 * @param accelerometerBias known accelerometer bias. This must be 3x1
251 * and is expressed in meters per squared
252 * second (m/s^2).
253 * @param accelerometerMa known accelerometer scale factors and
254 * cross coupling matrix. Must be 3x3.
255 * @throws IllegalArgumentException if any of the provided values does
256 * not have proper size.
257 */
258 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
259 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences, final Matrix bias,
260 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
261 final Matrix accelerometerMa) {
262 super(sequences, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
263 }
264
265 /**
266 * Constructor.
267 *
268 * @param sequences collection of sequences containing timestamped body
269 * kinematics measurements.
270 * @param bias gyroscope known bias. This must be 3x1 and is
271 * expressed in radians per second (rad/s).
272 * @param initialMg initial gyroscope scale factors and cross coupling
273 * errors matrix. Must be 3x3.
274 * @param initialGg initial gyroscope G-dependent cross biases
275 * introduced on the gyroscope by the specific forces
276 * sensed by the accelerometer. Must be 3x3.
277 * @param accelerometerBias known accelerometer bias. This must be 3x1
278 * and is expressed in meters per squared
279 * second (m/s^2).
280 * @param accelerometerMa known accelerometer scale factors and
281 * cross coupling matrix. Must be 3x3.
282 * @param listener listener to handle events raised by this
283 * calibrator.
284 * @throws IllegalArgumentException if any of the provided values does
285 * not have proper size.
286 */
287 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
288 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences, final Matrix bias,
289 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
290 final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener) {
291 super(sequences, bias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
292 }
293
294 /**
295 * Constructor.
296 *
297 * @param sequences collection of sequences containing timestamped body
298 * kinematics measurements.
299 * @param commonAxisUsed indicates whether z-axis is
300 * assumed to be common for
301 * accelerometer and gyroscope.
302 * @param estimateGDependentCrossBiases true if G-dependent cross biases
303 * will be estimated, false
304 * otherwise.
305 * @param bias gyroscope known bias. This must be 3x1 and is
306 * expressed in radians per second (rad/s).
307 * @param initialMg initial gyroscope scale factors and cross coupling
308 * errors matrix. Must be 3x3.
309 * @param initialGg initial gyroscope G-dependent cross biases
310 * introduced on the gyroscope by the specific forces
311 * sensed by the accelerometer. Must be 3x3.
312 * @throws IllegalArgumentException if any of the provided values does
313 * not have proper size.
314 */
315 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
316 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
317 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
318 final Matrix initialMg, final Matrix initialGg) {
319 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg);
320 }
321
322 /**
323 * Constructor.
324 *
325 * @param sequences collection of sequences containing timestamped body
326 * kinematics measurements.
327 * @param commonAxisUsed indicates whether z-axis is
328 * assumed to be common for
329 * accelerometer and gyroscope.
330 * @param estimateGDependentCrossBiases true if G-dependent cross biases
331 * will be estimated, false
332 * otherwise.
333 * @param bias gyroscope known bias. This must be 3x1 and is
334 * expressed in radians per second (rad/s).
335 * @param initialMg initial gyroscope scale factors and cross coupling
336 * errors matrix. Must be 3x3.
337 * @param initialGg initial gyroscope G-dependent cross biases
338 * introduced on the gyroscope by the specific forces
339 * sensed by the accelerometer. Must be 3x3.
340 * @param listener listener to handle events raised by this
341 * calibrator.
342 * @throws IllegalArgumentException if any of the provided values does
343 * not have proper size.
344 */
345 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
346 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
347 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
348 final Matrix initialMg, final Matrix initialGg,
349 final RobustKnownBiasEasyGyroscopeCalibratorListener listener) {
350 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
351 }
352
353 /**
354 * Constructor.
355 *
356 * @param sequences collection of sequences containing timestamped body
357 * kinematics measurements.
358 * @param commonAxisUsed indicates whether z-axis is
359 * assumed to be common for
360 * accelerometer and gyroscope.
361 * @param estimateGDependentCrossBiases true if G-dependent cross biases
362 * will be estimated, false
363 * otherwise.
364 * @param bias gyroscope known bias. This must have length 3 and is
365 * expressed in radians per second (rad/s).
366 * @param initialMg initial gyroscope scale factors and cross coupling
367 * errors matrix. Must be 3x3.
368 * @param initialGg initial gyroscope G-dependent cross biases
369 * introduced on the gyroscope by the specific forces
370 * sensed by the accelerometer. Must be 3x3.
371 * @throws IllegalArgumentException if any of the provided values does
372 * not have proper size.
373 */
374 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
375 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
376 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
377 final Matrix initialMg, final Matrix initialGg) {
378 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg);
379 }
380
381 /**
382 * Constructor.
383 *
384 * @param sequences collection of sequences containing timestamped body
385 * kinematics measurements.
386 * @param commonAxisUsed indicates whether z-axis is
387 * assumed to be common for
388 * accelerometer and gyroscope.
389 * @param estimateGDependentCrossBiases true if G-dependent cross biases
390 * will be estimated, false
391 * otherwise.
392 * @param bias gyroscope known bias. This must have length 3 and is
393 * expressed in radians per second (rad/s).
394 * @param initialMg initial gyroscope scale factors and cross coupling
395 * errors matrix. Must be 3x3.
396 * @param initialGg initial gyroscope G-dependent cross biases
397 * introduced on the gyroscope by the specific forces
398 * sensed by the accelerometer. Must be 3x3.
399 * @param listener listener to handle events raised by this
400 * calibrator.
401 * @throws IllegalArgumentException if any of the provided values does
402 * not have proper size.
403 */
404 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
405 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
406 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
407 final Matrix initialMg, final Matrix initialGg,
408 final RobustKnownBiasEasyGyroscopeCalibratorListener listener) {
409 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
410 }
411
412 /**
413 * Constructor.
414 *
415 * @param sequences collection of sequences containing timestamped body
416 * kinematics measurements.
417 * @param commonAxisUsed indicates whether z-axis is
418 * assumed to be common for
419 * accelerometer and gyroscope.
420 * @param estimateGDependentCrossBiases true if G-dependent cross biases
421 * will be estimated, false
422 * otherwise.
423 * @param bias gyroscope known bias. This must have length 3 and is
424 * expressed in radians per second (rad/s).
425 * @param initialMg initial gyroscope scale factors and cross coupling
426 * errors matrix. Must be 3x3.
427 * @param initialGg initial gyroscope G-dependent cross biases
428 * introduced on the gyroscope by the specific forces
429 * sensed by the accelerometer. Must be 3x3.
430 * @param accelerometerBias known accelerometer bias. This
431 * must have length 3 and is
432 * expressed in meters per squared
433 * second (m/s^2).
434 * @param accelerometerMa known accelerometer scale factors
435 * and cross coupling matrix. Must
436 * be 3x3.
437 * @throws IllegalArgumentException if any of the provided values does
438 * not have proper size.
439 */
440 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
441 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
442 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
443 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
444 final Matrix accelerometerMa) {
445 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg,
446 accelerometerBias, accelerometerMa);
447 }
448
449 /**
450 * Constructor.
451 *
452 * @param sequences collection of sequences containing timestamped body
453 * kinematics measurements.
454 * @param commonAxisUsed indicates whether z-axis is
455 * assumed to be common for
456 * accelerometer and gyroscope.
457 * @param estimateGDependentCrossBiases true if G-dependent cross biases
458 * will be estimated, false
459 * otherwise.
460 * @param bias gyroscope known bias. This must have length 3 and is
461 * expressed in radians per second (rad/s).
462 * @param initialMg initial gyroscope scale factors and cross coupling
463 * errors matrix. Must be 3x3.
464 * @param initialGg initial gyroscope G-dependent cross biases
465 * introduced on the gyroscope by the specific forces
466 * sensed by the accelerometer. Must be 3x3.
467 * @param accelerometerBias known accelerometer bias. This
468 * must have length 3 and is
469 * expressed in meters per squared
470 * second (m/s^2).
471 * @param accelerometerMa known accelerometer scale factors
472 * and cross coupling matrix. Must
473 * be 3x3.
474 * @param listener listener to handle events raised by this
475 * calibrator.
476 * @throws IllegalArgumentException if any of the provided values does
477 * not have proper size.
478 */
479 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
480 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
481 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
482 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
483 final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener) {
484 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
485 accelerometerMa, listener);
486 }
487
488 /**
489 * Constructor.
490 *
491 * @param sequences collection of sequences containing timestamped body
492 * kinematics measurements.
493 * @param commonAxisUsed indicates whether z-axis is
494 * assumed to be common for
495 * accelerometer and gyroscope.
496 * @param estimateGDependentCrossBiases true if G-dependent cross biases
497 * will be estimated, false
498 * otherwise.
499 * @param bias gyroscope known bias. This must be 3x1 and is
500 * expressed in radians per second (rad/s).
501 * @param initialMg initial gyroscope scale factors and cross coupling
502 * errors matrix. Must be 3x3.
503 * @param initialGg initial gyroscope G-dependent cross biases
504 * introduced on the gyroscope by the specific forces
505 * sensed by the accelerometer. Must be 3x3.
506 * @param accelerometerBias known accelerometer bias. This
507 * must have length 3 and is
508 * expressed in meters per squared
509 * second (m/s^2).
510 * @param accelerometerMa known accelerometer scale factors
511 * and cross coupling matrix. Must
512 * be 3x3.
513 * @throws IllegalArgumentException if any of the provided values does
514 * not have proper size.
515 */
516 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
517 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
518 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
519 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
520 final Matrix accelerometerMa) {
521 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
522 accelerometerMa);
523 }
524
525 /**
526 * Constructor.
527 *
528 * @param sequences collection of sequences containing timestamped body
529 * kinematics measurements.
530 * @param commonAxisUsed indicates whether z-axis is
531 * assumed to be common for
532 * accelerometer and gyroscope.
533 * @param estimateGDependentCrossBiases true if G-dependent cross biases
534 * will be estimated, false
535 * otherwise.
536 * @param bias gyroscope known bias. This must be 3x1 and is
537 * expressed in radians per second (rad/s).
538 * @param initialMg initial gyroscope scale factors and cross coupling
539 * errors matrix. Must be 3x3.
540 * @param initialGg initial gyroscope G-dependent cross biases
541 * introduced on the gyroscope by the specific forces
542 * sensed by the accelerometer. Must be 3x3.
543 * @param accelerometerBias known accelerometer bias. This
544 * must have length 3 and is
545 * expressed in meters per squared
546 * second (m/s^2).
547 * @param accelerometerMa known accelerometer scale factors
548 * and cross coupling matrix. Must
549 * be 3x3.
550 * @param listener listener to handle events raised by this
551 * calibrator.
552 * @throws IllegalArgumentException if any of the provided values does
553 * not have proper size.
554 */
555 public MSACRobustKnownBiasEasyGyroscopeCalibrator(
556 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
557 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
558 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
559 final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener) {
560 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
561 accelerometerMa, listener);
562 }
563
564 /**
565 * Returns threshold to determine whether samples are inliers or not.
566 *
567 * @return threshold to determine whether samples are inliers or not.
568 */
569 public double getThreshold() {
570 return threshold;
571 }
572
573 /**
574 * Sets threshold to determine whether samples are inliers or not.
575 *
576 * @param threshold threshold to be set.
577 * @throws IllegalArgumentException if provided value is equal or less than
578 * zero.
579 * @throws LockedException if calibrator is currently running.
580 */
581 public void setThreshold(final double threshold) throws LockedException {
582 if (running) {
583 throw new LockedException();
584 }
585 if (threshold <= MIN_THRESHOLD) {
586 throw new IllegalArgumentException();
587 }
588 this.threshold = threshold;
589 }
590
591 /**
592 * Estimates gyroscope calibration parameters containing scale factors,
593 * cross-coupling errors and G-dependent coupling.
594 *
595 * @throws LockedException if calibrator is currently running.
596 * @throws NotReadyException if calibrator is not ready.
597 * @throws CalibrationException if estimation fails for numerical reasons.
598 */
599 @SuppressWarnings("DuplicatedCode")
600 @Override
601 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
602 if (running) {
603 throw new LockedException();
604 }
605 if (!isReady()) {
606 throw new NotReadyException();
607 }
608
609 final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
610 @Override
611 public double getThreshold() {
612 return threshold;
613 }
614
615 @Override
616 public int getTotalSamples() {
617 return sequences.size();
618 }
619
620 @Override
621 public int getSubsetSize() {
622 return preliminarySubsetSize;
623 }
624
625 @Override
626 public void estimatePreliminarSolutions(
627 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
628 computePreliminarySolutions(samplesIndices, solutions);
629 }
630
631 @Override
632 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
633 return computeError(sequences.get(i), currentEstimation);
634 }
635
636 @Override
637 public boolean isReady() {
638 return MSACRobustKnownBiasEasyGyroscopeCalibrator.super.isReady();
639 }
640
641 @Override
642 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
643 // no action needed
644 }
645
646 @Override
647 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
648 // no action needed
649 }
650
651 @Override
652 public void onEstimateNextIteration(final RobustEstimator<PreliminaryResult> estimator,
653 final int iteration) {
654 if (listener != null) {
655 listener.onCalibrateNextIteration(
656 MSACRobustKnownBiasEasyGyroscopeCalibrator.this, iteration);
657 }
658 }
659
660 @Override
661 public void onEstimateProgressChange(
662 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
663 if (listener != null) {
664 listener.onCalibrateProgressChange(
665 MSACRobustKnownBiasEasyGyroscopeCalibrator.this, progress);
666 }
667 }
668 });
669
670 try {
671 running = true;
672
673 if (listener != null) {
674 listener.onCalibrateStart(this);
675 }
676
677 setupAccelerationFixer();
678
679 inliersData = null;
680 innerEstimator.setConfidence(confidence);
681 innerEstimator.setMaxIterations(maxIterations);
682 innerEstimator.setProgressDelta(progressDelta);
683 final var preliminaryResult = innerEstimator.estimate();
684 inliersData = innerEstimator.getInliersData();
685
686 attemptRefine(preliminaryResult);
687
688 if (listener != null) {
689 listener.onCalibrateEnd(this);
690 }
691
692 } catch (final com.irurueta.numerical.LockedException e) {
693 throw new LockedException(e);
694 } catch (final com.irurueta.numerical.NotReadyException e) {
695 throw new NotReadyException(e);
696 } catch (final RobustEstimatorException | AlgebraException e) {
697 throw new CalibrationException(e);
698 } finally {
699 running = false;
700 }
701 }
702
703 /**
704 * Returns method being used for robust estimation.
705 *
706 * @return method being used for robust estimation.
707 */
708 @Override
709 public RobustEstimatorMethod getMethod() {
710 return RobustEstimatorMethod.MSAC;
711 }
712
713 /**
714 * Indicates whether this calibrator requires quality scores for each
715 * measurement/sequence or not.
716 *
717 * @return true if quality scores are required, false otherwise.
718 */
719 @Override
720 public boolean isQualityScoresRequired() {
721 return false;
722 }
723 }