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