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 biases, cross couplings and scaling factors
35 * along with G-dependent cross biases introduced on the gyroscope by the
36 * specific forces sensed by the accelerometer using MSAC robust estimator.
37 * <p>
38 * This calibrator assumes that the IMU is 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 MSACRobustEasyGyroscopeCalibrator extends RobustEasyGyroscopeCalibrator {
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 MSACRobustEasyGyroscopeCalibrator() {
87 super();
88 }
89
90 /**
91 * Constructor.
92 *
93 * @param sequences collection of sequences containing timestamped body
94 * kinematics measurements.
95 * @param initialBias initial gyroscope bias to be used to find a solution.
96 * This must be 3x1 and is expressed in radians per
97 * second (rad/s).
98 * @param initialMg initial gyroscope scale factors and cross coupling
99 * errors matrix. Must be 3x3.
100 * @param initialGg initial gyroscope G-dependent cross biases
101 * introduced on the gyroscope by the specific forces
102 * sensed by the accelerometer. Must be 3x3.
103 * @throws IllegalArgumentException if any of the provided values does
104 * not have proper size.
105 */
106 public MSACRobustEasyGyroscopeCalibrator(
107 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
108 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg) {
109 super(sequences, initialBias, initialMg, initialGg);
110 }
111
112 /**
113 * Constructor.
114 *
115 * @param sequences collection of sequences containing timestamped body
116 * kinematics measurements.
117 * @param initialBias initial gyroscope bias to be used to find a solution.
118 * This must be 3x1 and is expressed in radians per
119 * second (rad/s).
120 * @param initialMg initial gyroscope scale factors and cross coupling
121 * errors matrix. Must be 3x3.
122 * @param initialGg initial gyroscope G-dependent cross biases
123 * introduced on the gyroscope by the specific forces
124 * sensed by the accelerometer. Must be 3x3.
125 * @param listener listener to handle events raised by this
126 * calibrator.
127 * @throws IllegalArgumentException if any of the provided values does
128 * not have proper size.
129 */
130 public MSACRobustEasyGyroscopeCalibrator(
131 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
132 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
133 final RobustEasyGyroscopeCalibratorListener listener) {
134 super(sequences, initialBias, initialMg, initialGg, listener);
135 }
136
137 /**
138 * Constructor.
139 *
140 * @param sequences collection of sequences containing timestamped body
141 * kinematics measurements.
142 * @param initialBias initial gyroscope bias to be used to find a
143 * solution. This must have length 3 and is expressed
144 * in radians per second (rad/s).
145 * @param initialMg initial gyroscope scale factors and cross coupling
146 * errors matrix. Must be 3x3.
147 * @param initialGg initial gyroscope G-dependent cross biases
148 * introduced on the gyroscope by the specific forces
149 * sensed by the accelerometer. Must be 3x3.
150 * @throws IllegalArgumentException if any of the provided values does
151 * not have proper size.
152 */
153 public MSACRobustEasyGyroscopeCalibrator(
154 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
155 final double[] initialBias, final Matrix initialMg, final Matrix initialGg) {
156 super(sequences, initialBias, initialMg, initialGg);
157 }
158
159 /**
160 * Constructor.
161 *
162 * @param sequences collection of sequences containing timestamped body
163 * kinematics measurements.
164 * @param initialBias initial gyroscope bias to be used to find a
165 * solution. This must have length 3 and is expressed
166 * in radians per second (rad/s).
167 * @param initialMg initial gyroscope scale factors and cross coupling
168 * errors matrix. Must be 3x3.
169 * @param initialGg initial gyroscope G-dependent cross biases
170 * introduced on the gyroscope by the specific forces
171 * sensed by the accelerometer. Must be 3x3.
172 * @param listener listener to handle events raised by this
173 * calibrator.
174 * @throws IllegalArgumentException if any of the provided values does
175 * not have proper size.
176 */
177 public MSACRobustEasyGyroscopeCalibrator(
178 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
179 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
180 final RobustEasyGyroscopeCalibratorListener listener) {
181 super(sequences, initialBias, initialMg, initialGg, listener);
182 }
183
184 /**
185 * Constructor.
186 *
187 * @param sequences collection of sequences containing timestamped body
188 * kinematics measurements.
189 * @param initialBias initial gyroscope bias to be used to find a
190 * solution. This must have length 3 and is expressed
191 * in radians per second (rad/s).
192 * @param initialMg initial gyroscope scale factors and cross coupling
193 * errors matrix. Must be 3x3.
194 * @param initialGg initial gyroscope G-dependent cross biases
195 * introduced on the gyroscope by the specific forces
196 * sensed by the accelerometer. Must be 3x3.
197 * @param accelerometerBias known accelerometer bias. This must
198 * have length 3 and is expressed in
199 * meters per squared second
200 * (m/s^2).
201 * @param accelerometerMa known accelerometer scale factors and
202 * cross coupling matrix. Must be 3x3.
203 * @throws IllegalArgumentException if any of the provided values does
204 * not have proper size.
205 */
206 public MSACRobustEasyGyroscopeCalibrator(
207 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
208 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
209 final double[] accelerometerBias, final Matrix accelerometerMa) {
210 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
211 }
212
213 /**
214 * Constructor.
215 *
216 * @param sequences collection of sequences containing timestamped body
217 * kinematics measurements.
218 * @param initialBias initial gyroscope bias to be used to find a
219 * solution. This must have length 3 and is expressed
220 * in radians per second (rad/s).
221 * @param initialMg initial gyroscope scale factors and cross coupling
222 * errors matrix. Must be 3x3.
223 * @param initialGg initial gyroscope G-dependent cross biases
224 * introduced on the gyroscope by the specific forces
225 * sensed by the accelerometer. Must be 3x3.
226 * @param accelerometerBias known accelerometer bias. This must
227 * have length 3 and is expressed in
228 * meters per squared second
229 * (m/s^2).
230 * @param accelerometerMa known accelerometer scale factors and
231 * cross coupling matrix. Must be 3x3.
232 * @param listener listener to handle events raised by this
233 * calibrator.
234 * @throws IllegalArgumentException if any of the provided values does
235 * not have proper size.
236 */
237 public MSACRobustEasyGyroscopeCalibrator(
238 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
239 final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
240 final double[] accelerometerBias, final Matrix accelerometerMa,
241 final RobustEasyGyroscopeCalibratorListener listener) {
242 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
243 }
244
245 /**
246 * Constructor.
247 *
248 * @param sequences collection of sequences containing timestamped body
249 * kinematics measurements.
250 * @param initialBias initial gyroscope bias to be used to find a
251 * solution. This must be 3x1 and is expressed
252 * in radians per second (rad/s).
253 * @param initialMg initial gyroscope scale factors and cross coupling
254 * errors matrix. Must be 3x3.
255 * @param initialGg initial gyroscope G-dependent cross biases
256 * introduced on the gyroscope by the specific forces
257 * sensed by the accelerometer. Must be 3x3.
258 * @param accelerometerBias known accelerometer bias. This must be 3x1
259 * and is expressed in meters per squared
260 * second (m/s^2).
261 * @param accelerometerMa known accelerometer scale factors and
262 * cross coupling matrix. Must be 3x3.
263 * @throws IllegalArgumentException if any of the provided values does
264 * not have proper size.
265 */
266 public MSACRobustEasyGyroscopeCalibrator(
267 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
268 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
269 final Matrix accelerometerMa) {
270 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
271 }
272
273 /**
274 * Constructor.
275 *
276 * @param sequences collection of sequences containing timestamped body
277 * kinematics measurements.
278 * @param initialBias initial gyroscope bias to be used to find a
279 * solution. This must be 3x1 and is expressed
280 * in radians per second (rad/s).
281 * @param initialMg initial gyroscope scale factors and cross coupling
282 * errors matrix. Must be 3x3.
283 * @param initialGg initial gyroscope G-dependent cross biases
284 * introduced on the gyroscope by the specific forces
285 * sensed by the accelerometer. Must be 3x3.
286 * @param accelerometerBias known accelerometer bias. This must be 3x1
287 * and is expressed in meters per squared
288 * second (m/s^2).
289 * @param accelerometerMa known accelerometer scale factors and
290 * cross coupling matrix. Must be 3x3.
291 * @param listener listener to handle events raised by this
292 * calibrator.
293 * @throws IllegalArgumentException if any of the provided values does
294 * not have proper size.
295 */
296 public MSACRobustEasyGyroscopeCalibrator(
297 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
298 final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
299 final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
300 super(sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
301 }
302
303 /**
304 * Constructor.
305 *
306 * @param sequences collection of sequences containing timestamped body
307 * kinematics measurements.
308 * @param commonAxisUsed indicates whether z-axis is
309 * assumed to be common for
310 * accelerometer and gyroscope.
311 * @param estimateGDependentCrossBiases true if G-dependent cross biases
312 * will be estimated, false
313 * otherwise.
314 * @param initialBias initial gyroscope bias to be used to find a
315 * solution. This must be 3x1 and is expressed
316 * in radians per second (rad/s).
317 * @param initialMg initial gyroscope scale factors and cross coupling
318 * errors matrix. Must be 3x3.
319 * @param initialGg initial gyroscope G-dependent cross biases
320 * introduced on the gyroscope by the specific forces
321 * sensed by the accelerometer. Must be 3x3.
322 * @throws IllegalArgumentException if any of the provided values does
323 * not have proper size.
324 */
325 public MSACRobustEasyGyroscopeCalibrator(
326 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
327 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
328 final Matrix initialMg, final Matrix initialGg) {
329 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
330 }
331
332 /**
333 * Constructor.
334 *
335 * @param sequences collection of sequences containing timestamped body
336 * kinematics measurements.
337 * @param commonAxisUsed indicates whether z-axis is
338 * assumed to be common for
339 * accelerometer and gyroscope.
340 * @param estimateGDependentCrossBiases true if G-dependent cross biases
341 * will be estimated, false
342 * otherwise.
343 * @param initialBias initial gyroscope bias to be used to find a
344 * solution. This must be 3x1 and is expressed
345 * in radians per second (rad/s).
346 * @param initialMg initial gyroscope scale factors and cross coupling
347 * errors matrix. Must be 3x3.
348 * @param initialGg initial gyroscope G-dependent cross biases
349 * introduced on the gyroscope by the specific forces
350 * sensed by the accelerometer. Must be 3x3.
351 * @param listener listener to handle events raised by this
352 * calibrator.
353 * @throws IllegalArgumentException if any of the provided values does
354 * not have proper size.
355 */
356 public MSACRobustEasyGyroscopeCalibrator(
357 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
358 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
359 final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener) {
360 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
361 }
362
363 /**
364 * Constructor.
365 *
366 * @param sequences collection of sequences containing timestamped body
367 * kinematics measurements.
368 * @param commonAxisUsed indicates whether z-axis is
369 * assumed to be common for
370 * accelerometer and gyroscope.
371 * @param estimateGDependentCrossBiases true if G-dependent cross biases
372 * will be estimated, false
373 * otherwise.
374 * @param initialBias initial gyroscope bias to be used to find a
375 * solution. This must have length 3 and is expressed
376 * in radians per second (rad/s).
377 * @param initialMg initial gyroscope scale factors and cross coupling
378 * errors matrix. Must be 3x3.
379 * @param initialGg initial gyroscope G-dependent cross biases
380 * introduced on the gyroscope by the specific forces
381 * sensed by the accelerometer. Must be 3x3.
382 * @throws IllegalArgumentException if any of the provided values does
383 * not have proper size.
384 */
385 public MSACRobustEasyGyroscopeCalibrator(
386 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
387 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
388 final Matrix initialMg, final Matrix initialGg) {
389 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
390 }
391
392 /**
393 * Constructor.
394 *
395 * @param sequences collection of sequences containing timestamped body
396 * kinematics measurements.
397 * @param commonAxisUsed indicates whether z-axis is
398 * assumed to be common for
399 * accelerometer and gyroscope.
400 * @param estimateGDependentCrossBiases true if G-dependent cross biases
401 * will be estimated, false
402 * otherwise.
403 * @param initialBias initial gyroscope bias to be used to find a
404 * solution. This must have length 3 and is expressed
405 * in radians per second (rad/s).
406 * @param initialMg initial gyroscope scale factors and cross coupling
407 * errors matrix. Must be 3x3.
408 * @param initialGg initial gyroscope G-dependent cross biases
409 * introduced on the gyroscope by the specific forces
410 * sensed by the accelerometer. Must be 3x3.
411 * @param listener listener to handle events raised by this
412 * calibrator.
413 * @throws IllegalArgumentException if any of the provided values does
414 * not have proper size.
415 */
416 public MSACRobustEasyGyroscopeCalibrator(
417 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
418 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
419 final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener) {
420 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
421 }
422
423 /**
424 * Constructor.
425 *
426 * @param sequences collection of sequences containing timestamped body
427 * kinematics measurements.
428 * @param commonAxisUsed indicates whether z-axis is
429 * assumed to be common for
430 * accelerometer and gyroscope.
431 * @param estimateGDependentCrossBiases true if G-dependent cross biases
432 * will be estimated, false
433 * otherwise.
434 * @param initialBias initial gyroscope bias to be used to find a
435 * solution. This must have length 3 and is expressed
436 * in radians per second (rad/s).
437 * @param initialMg initial gyroscope scale factors and cross coupling
438 * errors matrix. Must be 3x3.
439 * @param initialGg initial gyroscope G-dependent cross biases
440 * introduced on the gyroscope by the specific forces
441 * sensed by the accelerometer. Must be 3x3.
442 * @param accelerometerBias known accelerometer bias. This
443 * must have length 3 and is
444 * expressed in meters per squared
445 * second (m/s^2).
446 * @param accelerometerMa known accelerometer scale factors
447 * and cross coupling matrix. Must
448 * be 3x3.
449 * @throws IllegalArgumentException if any of the provided values does
450 * not have proper size.
451 */
452 public MSACRobustEasyGyroscopeCalibrator(
453 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
454 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
455 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
456 final Matrix accelerometerMa) {
457 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
458 accelerometerBias, accelerometerMa);
459 }
460
461 /**
462 * Constructor.
463 *
464 * @param sequences collection of sequences containing timestamped body
465 * kinematics measurements.
466 * @param commonAxisUsed indicates whether z-axis is
467 * assumed to be common for
468 * accelerometer and gyroscope.
469 * @param estimateGDependentCrossBiases true if G-dependent cross biases
470 * will be estimated, false
471 * otherwise.
472 * @param initialBias initial gyroscope bias to be used to find a
473 * solution. This must have length 3 and is expressed
474 * in radians per second (rad/s).
475 * @param initialMg initial gyroscope scale factors and cross coupling
476 * errors matrix. Must be 3x3.
477 * @param initialGg initial gyroscope G-dependent cross biases
478 * introduced on the gyroscope by the specific forces
479 * sensed by the accelerometer. Must be 3x3.
480 * @param accelerometerBias known accelerometer bias. This
481 * must have length 3 and is
482 * expressed in meters per squared
483 * second (m/s^2).
484 * @param accelerometerMa known accelerometer scale factors
485 * and cross coupling matrix. Must
486 * be 3x3.
487 * @param listener listener to handle events raised by this
488 * calibrator.
489 * @throws IllegalArgumentException if any of the provided values does
490 * not have proper size.
491 */
492 public MSACRobustEasyGyroscopeCalibrator(
493 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
494 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
495 final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
496 final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
497 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
498 accelerometerBias, accelerometerMa, listener);
499 }
500
501 /**
502 * Constructor.
503 *
504 * @param sequences collection of sequences containing timestamped body
505 * kinematics measurements.
506 * @param commonAxisUsed indicates whether z-axis is
507 * assumed to be common for
508 * accelerometer and gyroscope.
509 * @param estimateGDependentCrossBiases true if G-dependent cross biases
510 * will be estimated, false
511 * otherwise.
512 * @param initialBias initial gyroscope bias to be used to find a
513 * solution. This must be 3x1 and is expressed
514 * in radians per second (rad/s).
515 * @param initialMg initial gyroscope scale factors and cross coupling
516 * errors matrix. Must be 3x3.
517 * @param initialGg initial gyroscope G-dependent cross biases
518 * introduced on the gyroscope by the specific forces
519 * sensed by the accelerometer. Must be 3x3.
520 * @param accelerometerBias known accelerometer bias. This
521 * must have length 3 and is
522 * expressed in meters per squared
523 * second (m/s^2).
524 * @param accelerometerMa known accelerometer scale factors
525 * and cross coupling matrix. Must
526 * be 3x3.
527 * @throws IllegalArgumentException if any of the provided values does
528 * not have proper size.
529 */
530 public MSACRobustEasyGyroscopeCalibrator(
531 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
532 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
533 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
534 final Matrix accelerometerMa) {
535 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
536 accelerometerBias, accelerometerMa);
537 }
538
539 /**
540 * Constructor.
541 *
542 * @param sequences collection of sequences containing timestamped body
543 * kinematics measurements.
544 * @param commonAxisUsed indicates whether z-axis is
545 * assumed to be common for
546 * accelerometer and gyroscope.
547 * @param estimateGDependentCrossBiases true if G-dependent cross biases
548 * will be estimated, false
549 * otherwise.
550 * @param initialBias initial gyroscope bias to be used to find a
551 * solution. This must be 3x1 and is expressed
552 * in radians per second (rad/s).
553 * @param initialMg initial gyroscope scale factors and cross coupling
554 * errors matrix. Must be 3x3.
555 * @param initialGg initial gyroscope G-dependent cross biases
556 * introduced on the gyroscope by the specific forces
557 * sensed by the accelerometer. Must be 3x3.
558 * @param accelerometerBias known accelerometer bias. This
559 * must have length 3 and is
560 * expressed in meters per squared
561 * second (m/s^2).
562 * @param accelerometerMa known accelerometer scale factors
563 * and cross coupling matrix. Must
564 * be 3x3.
565 * @param listener listener to handle events raised by this
566 * calibrator.
567 * @throws IllegalArgumentException if any of the provided values does
568 * not have proper size.
569 */
570 public MSACRobustEasyGyroscopeCalibrator(
571 final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
572 final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
573 final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
574 final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
575 super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
576 accelerometerBias, accelerometerMa, listener);
577 }
578
579 /**
580 * Returns threshold to determine whether samples are inliers or not.
581 *
582 * @return threshold to determine whether samples are inliers or not.
583 */
584 public double getThreshold() {
585 return threshold;
586 }
587
588 /**
589 * Sets threshold to determine whether samples are inliers or not.
590 *
591 * @param threshold threshold to be set.
592 * @throws IllegalArgumentException if provided value is equal or less than
593 * zero.
594 * @throws LockedException if calibrator is currently running.
595 */
596 public void setThreshold(final double threshold) throws LockedException {
597 if (running) {
598 throw new LockedException();
599 }
600 if (threshold <= MIN_THRESHOLD) {
601 throw new IllegalArgumentException();
602 }
603 this.threshold = threshold;
604 }
605
606 /**
607 * Estimates gyroscope calibration parameters containing bias, scale factors,
608 * cross-coupling errors and G-dependent coupling.
609 *
610 * @throws LockedException if calibrator is currently running.
611 * @throws NotReadyException if calibrator is not ready.
612 * @throws CalibrationException if estimation fails for numerical reasons.
613 */
614 @SuppressWarnings("DuplicatedCode")
615 @Override
616 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
617 if (running) {
618 throw new LockedException();
619 }
620 if (!isReady()) {
621 throw new NotReadyException();
622 }
623
624 final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
625 @Override
626 public double getThreshold() {
627 return threshold;
628 }
629
630 @Override
631 public int getTotalSamples() {
632 return sequences.size();
633 }
634
635 @Override
636 public int getSubsetSize() {
637 return preliminarySubsetSize;
638 }
639
640 @Override
641 public void estimatePreliminarSolutions(
642 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
643 computePreliminarySolutions(samplesIndices, solutions);
644 }
645
646 @Override
647 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
648 return computeError(sequences.get(i), currentEstimation);
649 }
650
651 @Override
652 public boolean isReady() {
653 return MSACRobustEasyGyroscopeCalibrator.super.isReady();
654 }
655
656 @Override
657 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
658 // no action needed
659 }
660
661 @Override
662 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
663 // no action needed
664 }
665
666 @Override
667 public void onEstimateNextIteration(final RobustEstimator<PreliminaryResult> estimator,
668 final int iteration) {
669 if (listener != null) {
670 listener.onCalibrateNextIteration(MSACRobustEasyGyroscopeCalibrator.this, iteration);
671 }
672 }
673
674 @Override
675 public void onEstimateProgressChange(final RobustEstimator<PreliminaryResult> estimator,
676 final float progress) {
677 if (listener != null) {
678 listener.onCalibrateProgressChange(MSACRobustEasyGyroscopeCalibrator.this, progress);
679 }
680 }
681 });
682
683 try {
684 running = true;
685
686 if (listener != null) {
687 listener.onCalibrateStart(this);
688 }
689
690 setupAccelerationFixer();
691
692 inliersData = null;
693 innerEstimator.setConfidence(confidence);
694 innerEstimator.setMaxIterations(maxIterations);
695 innerEstimator.setProgressDelta(progressDelta);
696 final var preliminaryResult = innerEstimator.estimate();
697 inliersData = innerEstimator.getInliersData();
698
699 attemptRefine(preliminaryResult);
700
701 if (listener != null) {
702 listener.onCalibrateEnd(this);
703 }
704
705 } catch (final com.irurueta.numerical.LockedException e) {
706 throw new LockedException(e);
707 } catch (final com.irurueta.numerical.NotReadyException e) {
708 throw new NotReadyException(e);
709 } catch (final RobustEstimatorException | AlgebraException e) {
710 throw new CalibrationException(e);
711 } finally {
712 running = false;
713 }
714 }
715
716 /**
717 * Returns method being used for robust estimation.
718 *
719 * @return method being used for robust estimation.
720 */
721 @Override
722 public RobustEstimatorMethod getMethod() {
723 return RobustEstimatorMethod.MSAC;
724 }
725
726 /**
727 * Indicates whether this calibrator requires quality scores for each
728 * measurement/sequence or not.
729 *
730 * @return true if quality scores are required, false otherwise.
731 */
732 @Override
733 public boolean isQualityScoresRequired() {
734 return false;
735 }
736 }