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.accelerometer;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.navigation.LockedException;
20 import com.irurueta.navigation.NotReadyException;
21 import com.irurueta.navigation.inertial.calibration.CalibrationException;
22 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
23 import com.irurueta.numerical.robust.MSACRobustEstimator;
24 import com.irurueta.numerical.robust.MSACRobustEstimatorListener;
25 import com.irurueta.numerical.robust.RobustEstimator;
26 import com.irurueta.numerical.robust.RobustEstimatorException;
27 import com.irurueta.numerical.robust.RobustEstimatorMethod;
28 import com.irurueta.units.Acceleration;
29
30 import java.util.List;
31
32 /**
33 * Robustly estimates accelerometer cross couplings and scaling factors
34 * using MSAC algorithm to discard outliers.
35 * <p>
36 * To use this calibrator at least 7 measurements taken at a single position
37 * where gravity norm is known must be taken at 7 different unknown
38 * orientations and zero velocity when common z-axis
39 * is assumed, otherwise at least 10 measurements are required.
40 * <p>
41 * Measured specific force is assumed to follow the model shown below:
42 * <pre>
43 * fmeas = ba + (I + Ma) * ftrue + w
44 * </pre>
45 * Where:
46 * - fmeas is the measured specific force. This is a 3x1 vector.
47 * - ba is accelerometer bias. Ideally, on a perfect accelerometer, this should be a
48 * 3x1 zero vector.
49 * - I is the 3x3 identity matrix.
50 * - Ma is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
51 * a perfect accelerometer, this should be a 3x3 zero matrix.
52 * - ftrue is ground-truth specific force.
53 * - w is measurement noise.
54 */
55 public class MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator extends
56 RobustKnownBiasAndGravityNormAccelerometerCalibrator {
57
58 /**
59 * Constant defining default threshold to determine whether samples are
60 * inliers or not.
61 */
62 public static final double DEFAULT_THRESHOLD = 1e-2;
63
64 /**
65 * Minimum value that can be set as threshold.
66 * Threshold must be strictly greater than 0.0.
67 */
68 public static final double MIN_THRESHOLD = 0.0;
69
70 /**
71 * Threshold to determine whether samples are inliers or not when
72 * testing possible estimation solutions.
73 */
74 private double threshold = DEFAULT_THRESHOLD;
75
76 /**
77 * Constructor.
78 */
79 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator() {
80 super();
81 }
82
83 /**
84 * Constructor.
85 *
86 * @param listener listener to be notified of events such as when estimation
87 * starts, ends or its progress significantly changes.
88 */
89 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
90 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
91 super(listener);
92 }
93
94 /**
95 * Constructor.
96 *
97 * @param measurements collection of body kinematics measurements with standard
98 * deviations taken at the same position with zero velocity
99 * and unknown different orientations.
100 */
101 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
102 final List<StandardDeviationBodyKinematics> measurements) {
103 super(measurements);
104 }
105
106
107 /**
108 * Constructor.
109 *
110 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
111 * accelerometer and gyroscope.
112 */
113 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(final boolean commonAxisUsed) {
114 super(commonAxisUsed);
115 }
116
117 /**
118 * Constructor.
119 *
120 * @param bias known accelerometer bias. This must have length 3 and is expressed
121 * in meters per squared second (m/s^2).
122 * @throws IllegalArgumentException if provided bias array does not have length 3.
123 */
124 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(final double[] bias) {
125 super(bias);
126 }
127
128 /**
129 * Constructor.
130 *
131 * @param bias known accelerometer bias.
132 * @throws IllegalArgumentException if provided bias matrix is not 3x1.
133 */
134 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(final Matrix bias) {
135 super(bias);
136 }
137
138 /**
139 * Constructor.
140 *
141 * @param bias known accelerometer bias.
142 * @param initialMa initial scale factors and cross coupling errors matrix.
143 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
144 * scaling and coupling error matrix is not 3x3.
145 */
146 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(final Matrix bias, final Matrix initialMa) {
147 super(bias, initialMa);
148 }
149
150 /**
151 * Constructor.
152 *
153 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
154 * squared second (m/s^2).
155 * @throws IllegalArgumentException if provided gravity norm value is negative.
156 */
157 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(final Double groundTruthGravityNorm) {
158 super(groundTruthGravityNorm);
159 }
160
161 /**
162 * Constructor.
163 *
164 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
165 * squared second (m/s^2).
166 * @param measurements list of body kinematics measurements taken at a given position with
167 * different unknown orientations and containing the standard deviations
168 * of accelerometer and gyroscope measurements.
169 * @throws IllegalArgumentException if provided gravity norm value is negative.
170 */
171 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
172 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements) {
173 super(groundTruthGravityNorm, measurements);
174 }
175
176 /**
177 * Constructor.
178 *
179 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
180 * squared second (m/s^2).
181 * @param measurements list of body kinematics measurements taken at a given position with
182 * different unknown orientations and containing the standard deviations
183 * of accelerometer and gyroscope measurements.
184 * @param listener listener to be notified of events such as when estimation
185 * starts, ends or its progress significantly changes.
186 * @throws IllegalArgumentException if provided gravity norm value is negative.
187 */
188 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
189 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
190 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
191 super(groundTruthGravityNorm, measurements, listener);
192 }
193
194 /**
195 * Constructor.
196 *
197 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
198 * squared second (m/s^2).
199 * @param measurements list of body kinematics measurements taken at a given position with
200 * different unknown orientations and containing the standard deviations
201 * of accelerometer and gyroscope measurements.
202 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
203 * accelerometer and gyroscope.
204 * @throws IllegalArgumentException if provided gravity norm value is negative.
205 */
206 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
207 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
208 final boolean commonAxisUsed) {
209 super(groundTruthGravityNorm, measurements, commonAxisUsed);
210 }
211
212 /**
213 * Constructor.
214 *
215 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
216 * squared second (m/s^2).
217 * @param measurements list of body kinematics measurements taken at a given position with
218 * different unknown orientations and containing the standard deviations
219 * of accelerometer and gyroscope measurements.
220 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
221 * accelerometer and gyroscope.
222 * @param listener listener to be notified of events such as when estimation
223 * starts, ends or its progress significantly changes.
224 * @throws IllegalArgumentException if provided gravity norm value is negative.
225 */
226 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
227 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
228 final boolean commonAxisUsed, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
229 super(groundTruthGravityNorm, measurements, commonAxisUsed, listener);
230 }
231
232 /**
233 * Constructor.
234 *
235 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
236 * squared second (m/s^2).
237 * @param measurements collection of body kinematics measurements with standard
238 * deviations taken at the same position with zero velocity
239 * and unknown different orientations.
240 * @param bias known accelerometer bias. This must have length 3 and is expressed
241 * in meters per squared second (m/s^2).
242 * @throws IllegalArgumentException if provided bias array does not have length 3 or
243 * if provided gravity norm value is negative.
244 */
245 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
246 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
247 final double[] bias) {
248 super(groundTruthGravityNorm, measurements, bias);
249 }
250
251 /**
252 * Constructor.
253 *
254 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
255 * squared second (m/s^2).
256 * @param measurements collection of body kinematics measurements with standard
257 * deviations taken at the same position with zero velocity
258 * and unknown different orientations.
259 * @param bias known accelerometer bias. This must have length 3 and is expressed
260 * in meters per squared second (m/s^2).
261 * @param listener listener to handle events raised by this calibrator.
262 * @throws IllegalArgumentException if provided bias array does not have length 3 or
263 * if provided gravity norm value is negative.
264 */
265 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
266 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
267 final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
268 super(groundTruthGravityNorm, measurements, bias, listener);
269 }
270
271 /**
272 * Constructor.
273 *
274 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
275 * squared second (m/s^2).
276 * @param measurements collection of body kinematics measurements with standard
277 * deviations taken at the same position with zero velocity
278 * and unknown different orientations.
279 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
280 * accelerometer and gyroscope.
281 * @param bias known accelerometer bias. This must have length 3 and is expressed
282 * in meters per squared second (m/s^2).
283 * @throws IllegalArgumentException if provided bias array does not have length 3 or
284 * if provided gravity norm value is negative.
285 */
286 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
287 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
288 final boolean commonAxisUsed, final double[] bias) {
289 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias);
290 }
291
292 /**
293 * Constructor.
294 *
295 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
296 * squared second (m/s^2).
297 * @param measurements collection of body kinematics measurements with standard
298 * deviations taken at the same position with zero velocity
299 * and unknown different orientations.
300 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
301 * accelerometer and gyroscope.
302 * @param bias known accelerometer bias. This must have length 3 and is
303 * expressed in meters per squared second (m/s^2).
304 * @param listener listener to handle events raised by this calibrator.
305 * @throws IllegalArgumentException if provided bias array does not have length 3 or
306 * if provided gravity norm value is negative.
307 */
308 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
309 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
310 final boolean commonAxisUsed, final double[] bias,
311 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
312 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
313 }
314
315 /**
316 * Constructor.
317 *
318 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
319 * squared second (m/s^2).
320 * @param measurements collection of body kinematics measurements with standard
321 * deviations taken at the same position with zero velocity
322 * and unknown different orientations.
323 * @param bias known accelerometer bias.
324 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
325 * if provided gravity norm value is negative.
326 */
327 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
328 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
329 final Matrix bias) {
330 super(groundTruthGravityNorm, measurements, bias);
331 }
332
333 /**
334 * Constructor.
335 *
336 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
337 * squared second (m/s^2).
338 * @param measurements collection of body kinematics measurements with standard
339 * deviations taken at the same position with zero velocity
340 * and unknown different orientations.
341 * @param bias known accelerometer bias.
342 * @param listener listener to handle events raised by this calibrator.
343 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
344 * if provided gravity norm value is negative.
345 */
346 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
347 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
348 final Matrix bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
349 super(groundTruthGravityNorm, measurements, bias, listener);
350 }
351
352 /**
353 * Constructor.
354 *
355 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
356 * squared second (m/s^2).
357 * @param measurements collection of body kinematics measurements with standard
358 * deviations taken at the same position with zero velocity
359 * and unknown different orientations.
360 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
361 * accelerometer and gyroscope.
362 * @param bias known accelerometer bias.
363 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
364 * if provided gravity norm value is negative.
365 */
366 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
367 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
368 final boolean commonAxisUsed, final Matrix bias) {
369 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias);
370 }
371
372 /**
373 * Constructor.
374 *
375 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
376 * squared second (m/s^2).
377 * @param measurements collection of body kinematics measurements with standard
378 * deviations taken at the same position with zero velocity
379 * and unknown different orientations.
380 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
381 * accelerometer and gyroscope.
382 * @param bias known accelerometer bias.
383 * @param listener listener to handle events raised by this calibrator.
384 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
385 * if provided gravity norm value is negative.
386 */
387 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
388 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
389 final boolean commonAxisUsed, final Matrix bias,
390 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
391 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
392 }
393
394 /**
395 * Constructor.
396 *
397 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
398 * squared second (m/s^2).
399 * @param measurements collection of body kinematics measurements with standard
400 * deviations taken at the same position with zero velocity
401 * and unknown different orientations.
402 * @param bias known accelerometer bias.
403 * @param initialMa initial scale factors and cross coupling errors matrix.
404 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
405 * scaling and coupling error matrix is not 3x3 or
406 * if provided gravity norm value is negative.
407 */
408 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
409 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
410 final Matrix bias, final Matrix initialMa) {
411 super(groundTruthGravityNorm, measurements, bias, initialMa);
412 }
413
414 /**
415 * Constructor.
416 *
417 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
418 * squared second (m/s^2).
419 * @param measurements collection of body kinematics measurements with standard
420 * deviations taken at the same position with zero velocity
421 * and unknown different orientations.
422 * @param bias known accelerometer bias.
423 * @param initialMa initial scale factors and cross coupling errors matrix.
424 * @param listener listener to handle events raised by this calibrator.
425 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
426 * scaling and coupling error matrix is not 3x3 or
427 * if provided gravity norm value is negative.
428 */
429 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
430 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
431 final Matrix bias, final Matrix initialMa,
432 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
433 super(groundTruthGravityNorm, measurements, bias, initialMa, listener);
434 }
435
436 /**
437 * Constructor.
438 *
439 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
440 * squared second (m/s^2).
441 * @param measurements collection of body kinematics measurements with standard
442 * deviations taken at the same position with zero velocity
443 * and unknown different orientations.
444 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
445 * accelerometer and gyroscope.
446 * @param bias known accelerometer bias.
447 * @param initialMa initial scale factors and cross coupling errors matrix.
448 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
449 * scaling and coupling error matrix is not 3x3 or
450 * if provided gravity norm value is negative.
451 */
452 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
453 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
454 final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa) {
455 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
456 }
457
458 /**
459 * Constructor.
460 *
461 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
462 * squared second (m/s^2).
463 * @param measurements collection of body kinematics measurements with standard
464 * deviations taken at the same position with zero velocity
465 * and unknown different orientations.
466 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
467 * accelerometer and gyroscope.
468 * @param bias known accelerometer bias.
469 * @param initialMa initial scale factors and cross coupling errors matrix.
470 * @param listener listener to handle events raised by this calibrator.
471 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
472 * scaling and coupling error matrix is not 3x3 or
473 * if provided gravity norm value is negative.
474 */
475 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
476 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
477 final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
478 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
479 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
480 }
481
482 /**
483 * Constructor.
484 *
485 * @param groundTruthGravityNorm ground truth gravity norm.
486 * @throws IllegalArgumentException if provided gravity norm value is negative.
487 */
488 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(final Acceleration groundTruthGravityNorm) {
489 super(groundTruthGravityNorm);
490 }
491
492 /**
493 * Constructor.
494 *
495 * @param groundTruthGravityNorm ground truth gravity norm.
496 * @param measurements list of body kinematics measurements taken at a given position with
497 * different unknown orientations and containing the standard deviations
498 * of accelerometer and gyroscope measurements.
499 * @throws IllegalArgumentException if provided gravity norm value is negative.
500 */
501 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
502 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements) {
503 super(groundTruthGravityNorm, measurements);
504 }
505
506 /**
507 * Constructor.
508 *
509 * @param groundTruthGravityNorm ground truth gravity norm.
510 * @param measurements list of body kinematics measurements taken at a given position with
511 * different unknown orientations and containing the standard deviations
512 * of accelerometer and gyroscope measurements.
513 * @param listener listener to be notified of events such as when estimation
514 * starts, ends or its progress significantly changes.
515 * @throws IllegalArgumentException if provided gravity norm value is negative.
516 */
517 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
518 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
519 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
520 super(groundTruthGravityNorm, measurements, listener);
521 }
522
523 /**
524 * Constructor.
525 *
526 * @param groundTruthGravityNorm ground truth gravity norm.
527 * @param measurements list of body kinematics measurements taken at a given position with
528 * different unknown orientations and containing the standard deviations
529 * of accelerometer and gyroscope measurements.
530 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
531 * accelerometer and gyroscope.
532 * @throws IllegalArgumentException if provided gravity norm value is negative.
533 */
534 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
535 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
536 final boolean commonAxisUsed) {
537 super(groundTruthGravityNorm, measurements, commonAxisUsed);
538 }
539
540 /**
541 * Constructor.
542 *
543 * @param groundTruthGravityNorm ground truth gravity norm.
544 * @param measurements list of body kinematics measurements taken at a given position with
545 * different unknown orientations and containing the standard deviations
546 * of accelerometer and gyroscope measurements.
547 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
548 * accelerometer and gyroscope.
549 * @param listener listener to be notified of events such as when estimation
550 * starts, ends or its progress significantly changes.
551 * @throws IllegalArgumentException if provided gravity norm value is negative.
552 */
553 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
554 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
555 final boolean commonAxisUsed, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
556 super(groundTruthGravityNorm, measurements, commonAxisUsed, listener);
557 }
558
559 /**
560 * Constructor.
561 *
562 * @param groundTruthGravityNorm ground truth gravity norm.
563 * @param measurements collection of body kinematics measurements with standard
564 * deviations taken at the same position with zero velocity
565 * and unknown different orientations.
566 * @param bias known accelerometer bias. This must have length 3 and is expressed
567 * in meters per squared second (m/s^2).
568 * @throws IllegalArgumentException if provided bias array does not have length 3 or
569 * if provided gravity norm value is negative.
570 */
571 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
572 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
573 final double[] bias) {
574 super(groundTruthGravityNorm, measurements, bias);
575 }
576
577 /**
578 * Constructor.
579 *
580 * @param groundTruthGravityNorm ground truth gravity norm.
581 * @param measurements collection of body kinematics measurements with standard
582 * deviations taken at the same position with zero velocity
583 * and unknown different orientations.
584 * @param bias known accelerometer bias. This must have length 3 and is expressed
585 * in meters per squared second (m/s^2).
586 * @param listener listener to handle events raised by this calibrator.
587 * @throws IllegalArgumentException if provided bias array does not have length 3 or
588 * if provided gravity norm value is negative.
589 */
590 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
591 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
592 final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
593 super(groundTruthGravityNorm, measurements, bias, listener);
594 }
595
596 /**
597 * Constructor.
598 *
599 * @param groundTruthGravityNorm ground truth gravity norm.
600 * @param measurements collection of body kinematics measurements with standard
601 * deviations taken at the same position with zero velocity
602 * and unknown different orientations.
603 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
604 * accelerometer and gyroscope.
605 * @param bias known accelerometer bias. This must have length 3 and is expressed
606 * in meters per squared second (m/s^2).
607 * @throws IllegalArgumentException if provided bias array does not have length 3 or
608 * if provided gravity norm value is negative.
609 */
610 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
611 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
612 final boolean commonAxisUsed, final double[] bias) {
613 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias);
614 }
615
616 /**
617 * Constructor.
618 *
619 * @param groundTruthGravityNorm ground truth gravity norm.
620 * @param measurements collection of body kinematics measurements with standard
621 * deviations taken at the same position with zero velocity
622 * and unknown different orientations.
623 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
624 * accelerometer and gyroscope.
625 * @param bias known accelerometer bias. This must have length 3 and is expressed
626 * in meters per squared second (m/s^2).
627 * @param listener listener to handle events raised by this calibrator.
628 * @throws IllegalArgumentException if provided bias array does not have length 3 or
629 * if provided gravity norm value is negative.
630 */
631 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
632 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
633 final boolean commonAxisUsed, final double[] bias,
634 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
635 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
636 }
637
638 /**
639 * Constructor.
640 *
641 * @param groundTruthGravityNorm ground truth gravity norm.
642 * @param measurements collection of body kinematics measurements with standard
643 * deviations taken at the same position with zero velocity
644 * and unknown different orientations.
645 * @param bias known accelerometer bias.
646 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
647 * if provided gravity norm value is negative.
648 */
649 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
650 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
651 final Matrix bias) {
652 super(groundTruthGravityNorm, measurements, bias);
653 }
654
655 /**
656 * Constructor.
657 *
658 * @param groundTruthGravityNorm ground truth gravity norm.
659 * @param measurements collection of body kinematics measurements with standard
660 * deviations taken at the same position with zero velocity
661 * and unknown different orientations.
662 * @param bias known accelerometer bias.
663 * @param listener listener to handle events raised by this calibrator.
664 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
665 * if provided gravity norm value is negative.
666 */
667 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
668 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
669 final Matrix bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
670 super(groundTruthGravityNorm, measurements, bias, listener);
671 }
672
673 /**
674 * Constructor.
675 *
676 * @param groundTruthGravityNorm ground truth gravity norm.
677 * @param measurements collection of body kinematics measurements with standard
678 * deviations taken at the same position with zero velocity
679 * and unknown different orientations.
680 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
681 * accelerometer and gyroscope.
682 * @param bias known accelerometer bias.
683 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
684 * if provided gravity norm value is negative.
685 */
686 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
687 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
688 final boolean commonAxisUsed, final Matrix bias) {
689 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias);
690 }
691
692 /**
693 * Constructor.
694 *
695 * @param groundTruthGravityNorm ground truth gravity norm.
696 * @param measurements collection of body kinematics measurements with standard
697 * deviations taken at the same position with zero velocity
698 * and unknown different orientations.
699 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
700 * accelerometer and gyroscope.
701 * @param bias known accelerometer bias.
702 * @param listener listener to handle events raised by this calibrator.
703 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
704 * if provided gravity norm value is negative.
705 */
706 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
707 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
708 final boolean commonAxisUsed, final Matrix bias,
709 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
710 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
711 }
712
713 /**
714 * Constructor.
715 *
716 * @param groundTruthGravityNorm ground truth gravity norm.
717 * @param measurements collection of body kinematics measurements with standard
718 * deviations taken at the same position with zero velocity
719 * and unknown different orientations.
720 * @param bias known accelerometer bias.
721 * @param initialMa initial scale factors and cross coupling errors matrix.
722 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
723 * scaling and coupling error matrix is not 3x3 or
724 * if provided gravity norm value is negative.
725 */
726 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
727 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
728 final Matrix bias, final Matrix initialMa) {
729 super(groundTruthGravityNorm, measurements, bias, initialMa);
730 }
731
732 /**
733 * Constructor.
734 *
735 * @param groundTruthGravityNorm ground truth gravity norm.
736 * @param measurements collection of body kinematics measurements with standard
737 * deviations taken at the same position with zero velocity
738 * and unknown different orientations.
739 * @param bias known accelerometer bias.
740 * @param initialMa initial scale factors and cross coupling errors matrix.
741 * @param listener listener to handle events raised by this calibrator.
742 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
743 * scaling and coupling error matrix is not 3x3 or
744 * if provided gravity norm value is negative.
745 */
746 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
747 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
748 final Matrix bias, final Matrix initialMa,
749 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
750 super(groundTruthGravityNorm, measurements, bias, initialMa, listener);
751 }
752
753 /**
754 * Constructor.
755 *
756 * @param groundTruthGravityNorm ground truth gravity norm.
757 * @param measurements collection of body kinematics measurements with standard
758 * deviations taken at the same position with zero velocity
759 * and unknown different orientations.
760 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
761 * accelerometer and gyroscope.
762 * @param bias known accelerometer bias.
763 * @param initialMa initial scale factors and cross coupling errors matrix.
764 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
765 * scaling and coupling error matrix is not 3x3 or
766 * if provided gravity norm value is negative.
767 */
768 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
769 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
770 final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa) {
771 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
772 }
773
774 /**
775 * Constructor.
776 *
777 * @param groundTruthGravityNorm ground truth gravity norm.
778 * @param measurements collection of body kinematics measurements with standard
779 * deviations taken at the same position with zero velocity
780 * and unknown different orientations.
781 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
782 * accelerometer and gyroscope.
783 * @param bias known accelerometer bias.
784 * @param initialMa initial scale factors and cross coupling errors matrix.
785 * @param listener listener to handle events raised by this calibrator.
786 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
787 * scaling and coupling error matrix is not 3x3 or
788 * if provided gravity norm value is negative.
789 */
790 public MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
791 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
792 final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
793 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
794 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
795 }
796
797 /**
798 * Returns threshold to determine whether samples are inliers or not.
799 *
800 * @return threshold to determine whether samples are inliers or not.
801 */
802 public double getThreshold() {
803 return threshold;
804 }
805
806 /**
807 * Sets threshold to determine whether samples are inliers or not.
808 *
809 * @param threshold threshold to be set.
810 * @throws IllegalArgumentException if provided value is equal or less than
811 * zero.
812 * @throws LockedException if calibrator is currently running.
813 */
814 public void setThreshold(final double threshold) throws LockedException {
815 if (running) {
816 throw new LockedException();
817 }
818 if (threshold <= MIN_THRESHOLD) {
819 throw new IllegalArgumentException();
820 }
821 this.threshold = threshold;
822 }
823
824 /**
825 * Estimates accelerometer calibration parameters containing bias, scale factors
826 * and cross-coupling errors.
827 *
828 * @throws LockedException if calibrator is currently running.
829 * @throws NotReadyException if calibrator is not ready.
830 * @throws CalibrationException if estimation fails for numerical reasons.
831 */
832 @SuppressWarnings("DuplicatedCode")
833 @Override
834 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
835 if (running) {
836 throw new LockedException();
837 }
838 if (!isReady()) {
839 throw new NotReadyException();
840 }
841
842 final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
843 @Override
844 public double getThreshold() {
845 return threshold;
846 }
847
848 @Override
849 public int getTotalSamples() {
850 return measurements.size();
851 }
852
853 @Override
854 public int getSubsetSize() {
855 return preliminarySubsetSize;
856 }
857
858 @Override
859 public void estimatePreliminarSolutions(
860 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
861 computePreliminarySolutions(samplesIndices, solutions);
862 }
863
864 @Override
865 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
866 return computeError(measurements.get(i), currentEstimation);
867 }
868
869 @Override
870 public boolean isReady() {
871 return MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
872 }
873
874 @Override
875 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
876 // no action needed
877 }
878
879 @Override
880 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
881 // no action needed
882 }
883
884 @Override
885 public void onEstimateNextIteration(
886 final RobustEstimator<PreliminaryResult> estimator, final int iteration) {
887 if (listener != null) {
888 listener.onCalibrateNextIteration(
889 MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.this, iteration);
890 }
891 }
892
893 @Override
894 public void onEstimateProgressChange(
895 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
896 if (listener != null) {
897 listener.onCalibrateProgressChange(
898 MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.this, progress);
899 }
900 }
901 });
902
903 try {
904 running = true;
905
906 if (listener != null) {
907 listener.onCalibrateStart(this);
908 }
909
910 inliersData = null;
911 innerEstimator.setConfidence(confidence);
912 innerEstimator.setMaxIterations(maxIterations);
913 innerEstimator.setProgressDelta(progressDelta);
914 final var preliminaryResult = innerEstimator.estimate();
915 inliersData = innerEstimator.getInliersData();
916
917 attemptRefine(preliminaryResult);
918
919 if (listener != null) {
920 listener.onCalibrateEnd(this);
921 }
922
923 } catch (final com.irurueta.numerical.LockedException e) {
924 throw new LockedException(e);
925 } catch (final com.irurueta.numerical.NotReadyException e) {
926 throw new NotReadyException(e);
927 } catch (final RobustEstimatorException e) {
928 throw new CalibrationException(e);
929 } finally {
930 running = false;
931 }
932 }
933
934 /**
935 * Returns method being used for robust estimation.
936 *
937 * @return method being used for robust estimation.
938 */
939 @Override
940 public RobustEstimatorMethod getMethod() {
941 return RobustEstimatorMethod.MSAC;
942 }
943
944 /**
945 * Indicates whether this calibrator requires quality scores for each
946 * measurement or not.
947 *
948 * @return true if quality scores are required, false otherwise.
949 */
950 @Override
951 public boolean isQualityScoresRequired() {
952 return false;
953 }
954 }