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