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