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