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.LMedSRobustEstimator;
24 import com.irurueta.numerical.robust.LMedSRobustEstimatorListener;
25 import com.irurueta.numerical.robust.RobustEstimator;
26 import com.irurueta.numerical.robust.RobustEstimatorException;
27 import com.irurueta.numerical.robust.RobustEstimatorMethod;
28 import com.irurueta.units.Acceleration;
29
30 import java.util.List;
31
32 /**
33 * Robustly estimates accelerometer cross couplings and scaling factors
34 * using a LMedS algorithm to discard outliers.
35 * <p>
36 * To use this calibrator at least 7 measurements taken at a single position
37 * where gravity norm is known must be taken at 7 different unknown
38 * orientations and zero velocity when common z-axis
39 * is assumed, otherwise at least 10 measurements are required.
40 * <p>
41 * Measured specific force is assumed to follow the model shown below:
42 * <pre>
43 * fmeas = ba + (I + Ma) * ftrue + w
44 * </pre>
45 * Where:
46 * - fmeas is the measured specific force. This is a 3x1 vector.
47 * - ba is accelerometer bias. Ideally, on a perfect accelerometer, this should be a
48 * 3x1 zero vector.
49 * - I is the 3x3 identity matrix.
50 * - Ma is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
51 * a perfect accelerometer, this should be a 3x3 zero matrix.
52 * - ftrue is ground-truth specific force.
53 * - w is measurement noise.
54 */
55 public class LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator extends
56 RobustKnownBiasAndGravityNormAccelerometerCalibrator {
57
58 /**
59 * Default value to be used for stop threshold. Stop threshold can be used to
60 * avoid keeping the algorithm unnecessarily iterating in case that best
61 * estimated threshold using median of residuals is not small enough. Once a
62 * solution is found that generates a threshold below this value, the
63 * algorithm will stop.
64 * The stop threshold can be used to prevent the LMedS algorithm iterating
65 * too many times in cases where samples have a very similar accuracy.
66 * For instance, in cases where proportion of outliers is very small (close
67 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
68 * iterate for a long time trying to find the best solution when indeed
69 * there is no need to do that if a reasonable threshold has already been
70 * reached.
71 * Because of this behaviour the stop threshold can be set to a value much
72 * lower than the one typically used in RANSAC, and yet the algorithm could
73 * still produce even smaller thresholds in estimated results.
74 */
75 public static final double DEFAULT_STOP_THRESHOLD = 1e-4;
76
77 /**
78 * Minimum allowed stop threshold value.
79 */
80 public static final double MIN_STOP_THRESHOLD = 0.0;
81
82 /**
83 * Threshold to be used to keep the algorithm iterating in case that best
84 * estimated threshold using median of residuals is not small enough. Once
85 * a solution is found that generates a threshold below this value, the
86 * algorithm will stop.
87 * The stop threshold can be used to prevent the LMedS algorithm iterating
88 * too many times in cases where samples have a very similar accuracy.
89 * For instance, in cases where proportion of outliers is very small (close
90 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
91 * iterate for a long time trying to find the best solution when indeed
92 * there is no need to do that if a reasonable threshold has already been
93 * reached.
94 * Because of this behaviour the stop threshold can be set to a value much
95 * lower than the one typically used in LMedS, and yet the algorithm could
96 * still produce even smaller thresholds in estimated results.
97 */
98 private double stopThreshold = DEFAULT_STOP_THRESHOLD;
99
100 /**
101 * Constructor.
102 */
103 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator() {
104 super();
105 }
106
107 /**
108 * Constructor.
109 *
110 * @param listener listener to be notified of events such as when estimation
111 * starts, ends or its progress significantly changes.
112 */
113 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
114 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
115 super(listener);
116 }
117
118 /**
119 * Constructor.
120 *
121 * @param measurements collection of body kinematics measurements with standard
122 * deviations taken at the same position with zero velocity
123 * and unknown different orientations.
124 */
125 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
126 final List<StandardDeviationBodyKinematics> measurements) {
127 super(measurements);
128 }
129
130
131 /**
132 * Constructor.
133 *
134 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
135 * accelerometer and gyroscope.
136 */
137 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(final boolean commonAxisUsed) {
138 super(commonAxisUsed);
139 }
140
141 /**
142 * Constructor.
143 *
144 * @param bias known accelerometer bias. This must have length 3 and is expressed
145 * in meters per squared second (m/s^2).
146 * @throws IllegalArgumentException if provided bias array does not have length 3.
147 */
148 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(final double[] bias) {
149 super(bias);
150 }
151
152 /**
153 * Constructor.
154 *
155 * @param bias known accelerometer bias.
156 * @throws IllegalArgumentException if provided bias matrix is not 3x1.
157 */
158 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(final Matrix bias) {
159 super(bias);
160 }
161
162 /**
163 * Constructor.
164 *
165 * @param bias known accelerometer bias.
166 * @param initialMa initial scale factors and cross coupling errors matrix.
167 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
168 * scaling and coupling error matrix is not 3x3.
169 */
170 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(final Matrix bias, final Matrix initialMa) {
171 super(bias, initialMa);
172 }
173
174 /**
175 * Constructor.
176 *
177 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
178 * squared second (m/s^2).
179 * @throws IllegalArgumentException if provided gravity norm value is negative.
180 */
181 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(final Double groundTruthGravityNorm) {
182 super(groundTruthGravityNorm);
183 }
184
185 /**
186 * Constructor.
187 *
188 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
189 * squared second (m/s^2).
190 * @param measurements list of body kinematics measurements taken at a given position with
191 * different unknown orientations and containing the standard deviations
192 * of accelerometer and gyroscope measurements.
193 * @throws IllegalArgumentException if provided gravity norm value is negative.
194 */
195 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
196 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements) {
197 super(groundTruthGravityNorm, measurements);
198 }
199
200 /**
201 * Constructor.
202 *
203 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
204 * squared second (m/s^2).
205 * @param measurements list of body kinematics measurements taken at a given position with
206 * different unknown orientations and containing the standard deviations
207 * of accelerometer and gyroscope measurements.
208 * @param listener listener to be notified of events such as when estimation
209 * starts, ends or its progress significantly changes.
210 * @throws IllegalArgumentException if provided gravity norm value is negative.
211 */
212 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
213 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
214 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
215 super(groundTruthGravityNorm, measurements, listener);
216 }
217
218 /**
219 * Constructor.
220 *
221 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
222 * squared second (m/s^2).
223 * @param measurements list of body kinematics measurements taken at a given position with
224 * different unknown orientations and containing the standard deviations
225 * of accelerometer and gyroscope measurements.
226 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
227 * accelerometer and gyroscope.
228 * @throws IllegalArgumentException if provided gravity norm value is negative.
229 */
230 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
231 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
232 final boolean commonAxisUsed) {
233 super(groundTruthGravityNorm, measurements, commonAxisUsed);
234 }
235
236 /**
237 * Constructor.
238 *
239 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
240 * squared second (m/s^2).
241 * @param measurements list of body kinematics measurements taken at a given position with
242 * different unknown orientations and containing the standard deviations
243 * of accelerometer and gyroscope measurements.
244 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
245 * accelerometer and gyroscope.
246 * @param listener listener to be notified of events such as when estimation
247 * starts, ends or its progress significantly changes.
248 * @throws IllegalArgumentException if provided gravity norm value is negative.
249 */
250 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
251 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
252 final boolean commonAxisUsed, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
253 super(groundTruthGravityNorm, measurements, commonAxisUsed, listener);
254 }
255
256 /**
257 * Constructor.
258 *
259 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
260 * squared second (m/s^2).
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 bias known accelerometer bias. This must have length 3 and is expressed
265 * in meters per squared second (m/s^2).
266 * @throws IllegalArgumentException if provided bias array does not have length 3 or
267 * if provided gravity norm value is negative.
268 */
269 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
270 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
271 final double[] bias) {
272 super(groundTruthGravityNorm, measurements, bias);
273 }
274
275 /**
276 * Constructor.
277 *
278 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
279 * squared second (m/s^2).
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 bias known accelerometer bias. This must have length 3 and is expressed
284 * in meters per squared second (m/s^2).
285 * @param listener listener to handle events raised by this calibrator.
286 * @throws IllegalArgumentException if provided bias array does not have length 3 or
287 * if provided gravity norm value is negative.
288 */
289 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
290 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
291 final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
292 super(groundTruthGravityNorm, measurements, bias, listener);
293 }
294
295 /**
296 * Constructor.
297 *
298 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
299 * squared second (m/s^2).
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 bias known accelerometer bias. This must have length 3 and is expressed
306 * in meters per squared second (m/s^2).
307 * @throws IllegalArgumentException if provided bias array does not have length 3 or
308 * if provided gravity norm value is negative.
309 */
310 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
311 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
312 final boolean commonAxisUsed, final double[] bias) {
313 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias);
314 }
315
316 /**
317 * Constructor.
318 *
319 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
320 * squared second (m/s^2).
321 * @param measurements collection of body kinematics measurements with standard
322 * deviations taken at the same position with zero velocity
323 * and unknown different orientations.
324 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
325 * accelerometer and gyroscope.
326 * @param bias known accelerometer bias. This must have length 3 and is
327 * expressed in meters per 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 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
333 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
334 final boolean commonAxisUsed, final double[] bias,
335 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
336 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, 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 bias known accelerometer bias.
348 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
349 * if provided gravity norm value is negative.
350 */
351 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
352 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
353 final Matrix bias) {
354 super(groundTruthGravityNorm, measurements, bias);
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 bias known accelerometer bias.
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 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
371 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
372 final Matrix bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
373 super(groundTruthGravityNorm, measurements, bias, 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 bias known accelerometer bias.
387 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
388 * if provided gravity norm value is negative.
389 */
390 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
391 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
392 final boolean commonAxisUsed, final Matrix bias) {
393 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias);
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 bias known accelerometer bias.
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 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
412 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
413 final boolean commonAxisUsed, final Matrix bias,
414 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
415 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, 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 bias known accelerometer bias.
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 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
433 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
434 final Matrix bias, final Matrix initialMa) {
435 super(groundTruthGravityNorm, measurements, bias, 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 bias known accelerometer bias.
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 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
454 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
455 final Matrix bias, final Matrix initialMa,
456 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
457 super(groundTruthGravityNorm, measurements, bias, 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 bias known accelerometer bias.
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 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
477 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
478 final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa) {
479 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, 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 bias known accelerometer bias.
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 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
500 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
501 final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
502 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
503 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, 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 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(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 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
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 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
542 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
543 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener 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 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
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 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
578 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
579 final boolean commonAxisUsed, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener 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 bias known accelerometer bias. This must have length 3 and is expressed
591 * in meters per squared second (m/s^2).
592 * @throws IllegalArgumentException if provided bias array does not have length 3 or
593 * if provided gravity norm value is negative.
594 */
595 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
596 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
597 final double[] bias) {
598 super(groundTruthGravityNorm, measurements, bias);
599 }
600
601 /**
602 * Constructor.
603 *
604 * @param groundTruthGravityNorm ground truth gravity norm.
605 * @param measurements collection of body kinematics measurements with standard
606 * deviations taken at the same position with zero velocity
607 * and unknown different orientations.
608 * @param bias known accelerometer bias. This must have length 3 and is expressed
609 * in meters per 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 or
612 * if provided gravity norm value is negative.
613 */
614 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
615 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
616 final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
617 super(groundTruthGravityNorm, measurements, bias, listener);
618 }
619
620 /**
621 * Constructor.
622 *
623 * @param groundTruthGravityNorm ground truth gravity norm.
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 commonAxisUsed indicates whether z-axis is assumed to be common for
628 * accelerometer and gyroscope.
629 * @param bias known accelerometer bias. This must have length 3 and is expressed
630 * in meters per squared second (m/s^2).
631 * @throws IllegalArgumentException if provided bias array does not have length 3 or
632 * if provided gravity norm value is negative.
633 */
634 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
635 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
636 final boolean commonAxisUsed, final double[] bias) {
637 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias);
638 }
639
640 /**
641 * Constructor.
642 *
643 * @param groundTruthGravityNorm ground truth gravity norm.
644 * @param measurements collection of body kinematics measurements with standard
645 * deviations taken at the same position with zero velocity
646 * and unknown different orientations.
647 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
648 * accelerometer and gyroscope.
649 * @param bias known accelerometer bias. This must have length 3 and is expressed
650 * in meters per squared second (m/s^2).
651 * @param listener listener to handle events raised by this calibrator.
652 * @throws IllegalArgumentException if provided bias array does not have length 3 or
653 * if provided gravity norm value is negative.
654 */
655 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
656 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
657 final boolean commonAxisUsed, final double[] bias,
658 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
659 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
660 }
661
662 /**
663 * Constructor.
664 *
665 * @param groundTruthGravityNorm ground truth gravity norm.
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 bias known accelerometer bias.
670 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
671 * if provided gravity norm value is negative.
672 */
673 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
674 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
675 final Matrix bias) {
676 super(groundTruthGravityNorm, measurements, bias);
677 }
678
679 /**
680 * Constructor.
681 *
682 * @param groundTruthGravityNorm ground truth gravity norm.
683 * @param measurements collection of body kinematics measurements with standard
684 * deviations taken at the same position with zero velocity
685 * and unknown different orientations.
686 * @param bias known accelerometer bias.
687 * @param listener listener to handle events raised by this calibrator.
688 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
689 * if provided gravity norm value is negative.
690 */
691 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
692 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
693 final Matrix bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
694 super(groundTruthGravityNorm, measurements, bias, listener);
695 }
696
697 /**
698 * Constructor.
699 *
700 * @param groundTruthGravityNorm ground truth gravity norm.
701 * @param measurements collection of body kinematics measurements with standard
702 * deviations taken at the same position with zero velocity
703 * and unknown different orientations.
704 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
705 * accelerometer and gyroscope.
706 * @param bias known accelerometer bias.
707 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
708 * if provided gravity norm value is negative.
709 */
710 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
711 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
712 final boolean commonAxisUsed, final Matrix bias) {
713 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias);
714 }
715
716 /**
717 * Constructor.
718 *
719 * @param groundTruthGravityNorm ground truth gravity norm.
720 * @param measurements collection of body kinematics measurements with standard
721 * deviations taken at the same position with zero velocity
722 * and unknown different orientations.
723 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
724 * accelerometer and gyroscope.
725 * @param bias known accelerometer bias.
726 * @param listener listener to handle events raised by this calibrator.
727 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
728 * if provided gravity norm value is negative.
729 */
730 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
731 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
732 final boolean commonAxisUsed, final Matrix bias,
733 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
734 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
735 }
736
737 /**
738 * Constructor.
739 *
740 * @param groundTruthGravityNorm ground truth gravity norm.
741 * @param measurements collection of body kinematics measurements with standard
742 * deviations taken at the same position with zero velocity
743 * and unknown different orientations.
744 * @param bias known accelerometer bias.
745 * @param initialMa initial scale factors and cross coupling errors matrix.
746 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
747 * scaling and coupling error matrix is not 3x3 or
748 * if provided gravity norm value is negative.
749 */
750 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
751 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
752 final Matrix bias, final Matrix initialMa) {
753 super(groundTruthGravityNorm, measurements, bias, initialMa);
754 }
755
756 /**
757 * Constructor.
758 *
759 * @param groundTruthGravityNorm ground truth gravity norm.
760 * @param measurements collection of body kinematics measurements with standard
761 * deviations taken at the same position with zero velocity
762 * and unknown different orientations.
763 * @param bias known accelerometer bias.
764 * @param initialMa initial scale factors and cross coupling errors matrix.
765 * @param listener listener to handle events raised by this calibrator.
766 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
767 * scaling and coupling error matrix is not 3x3 or
768 * if provided gravity norm value is negative.
769 */
770 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
771 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
772 final Matrix bias, final Matrix initialMa,
773 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
774 super(groundTruthGravityNorm, measurements, bias, initialMa, listener);
775 }
776
777 /**
778 * Constructor.
779 *
780 * @param groundTruthGravityNorm ground truth gravity norm.
781 * @param measurements collection of body kinematics measurements with standard
782 * deviations taken at the same position with zero velocity
783 * and unknown different orientations.
784 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
785 * accelerometer and gyroscope.
786 * @param bias known accelerometer bias.
787 * @param initialMa initial scale factors and cross coupling errors matrix.
788 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
789 * scaling and coupling error matrix is not 3x3 or
790 * if provided gravity norm value is negative.
791 */
792 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
793 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
794 final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa) {
795 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
796 }
797
798 /**
799 * Constructor.
800 *
801 * @param groundTruthGravityNorm ground truth gravity norm.
802 * @param measurements collection of body kinematics measurements with standard
803 * deviations taken at the same position with zero velocity
804 * and unknown different orientations.
805 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
806 * accelerometer and gyroscope.
807 * @param bias known accelerometer bias.
808 * @param initialMa initial scale factors and cross coupling errors matrix.
809 * @param listener listener to handle events raised by this calibrator.
810 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
811 * scaling and coupling error matrix is not 3x3 or
812 * if provided gravity norm value is negative.
813 */
814 public LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
815 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
816 final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
817 final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener) {
818 super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
819 }
820
821 /**
822 * Returns threshold to be used to keep the algorithm iterating in case that
823 * best estimated threshold using median of residuals is not small enough.
824 * Once a solution is found that generates a threshold below this value, the
825 * algorithm will stop.
826 * The stop threshold can be used to prevent the LMedS algorithm to iterate
827 * too many times in cases where samples have a very similar accuracy.
828 * For instance, in cases where proportion of outliers is very small (close
829 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
830 * iterate for a long time trying to find the best solution when indeed
831 * there is no need to do that if a reasonable threshold has already been
832 * reached.
833 * Because of this behaviour the stop threshold can be set to a value much
834 * lower than the one typically used in RANSAC, and yet the algorithm could
835 * still produce even smaller thresholds in estimated results.
836 *
837 * @return stop threshold to stop the algorithm prematurely when a certain
838 * accuracy has been reached.
839 */
840 public double getStopThreshold() {
841 return stopThreshold;
842 }
843
844 /**
845 * Sets threshold to be used to keep the algorithm iterating in case that
846 * best estimated threshold using median of residuals is not small enough.
847 * Once a solution is found that generates a threshold below this value,
848 * the algorithm will stop.
849 * The stop threshold can be used to prevent the LMedS algorithm to iterate
850 * too many times in cases where samples have a very similar accuracy.
851 * For instance, in cases where proportion of outliers is very small (close
852 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
853 * iterate for a long time trying to find the best solution when indeed
854 * there is no need to do that if a reasonable threshold has already been
855 * reached.
856 * Because of this behaviour the stop threshold can be set to a value much
857 * lower than the one typically used in RANSAC, and yet the algorithm could
858 * still produce even smaller thresholds in estimated results.
859 *
860 * @param stopThreshold stop threshold to stop the algorithm prematurely
861 * when a certain accuracy has been reached.
862 * @throws IllegalArgumentException if provided value is zero or negative.
863 * @throws LockedException if calibrator is currently running.
864 */
865 public void setStopThreshold(final double stopThreshold) throws LockedException {
866 if (running) {
867 throw new LockedException();
868 }
869 if (stopThreshold <= MIN_STOP_THRESHOLD) {
870 throw new IllegalArgumentException();
871 }
872
873 this.stopThreshold = stopThreshold;
874 }
875
876 /**
877 * Estimates accelerometer calibration parameters containing scale factors
878 * and cross-coupling errors.
879 *
880 * @throws LockedException if calibrator is currently running.
881 * @throws NotReadyException if calibrator is not ready.
882 * @throws CalibrationException if estimation fails for numerical reasons.
883 */
884 @SuppressWarnings("DuplicatedCode")
885 @Override
886 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
887 if (running) {
888 throw new LockedException();
889 }
890 if (!isReady()) {
891 throw new NotReadyException();
892 }
893
894 final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
895 @Override
896 public int getTotalSamples() {
897 return measurements.size();
898 }
899
900 @Override
901 public int getSubsetSize() {
902 return preliminarySubsetSize;
903 }
904
905 @Override
906 public void estimatePreliminarSolutions(
907 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
908 computePreliminarySolutions(samplesIndices, solutions);
909 }
910
911 @Override
912 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
913 return computeError(measurements.get(i), currentEstimation);
914 }
915
916 @Override
917 public boolean isReady() {
918 return LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
919 }
920
921 @Override
922 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
923 // no action needed
924 }
925
926 @Override
927 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
928 // no action needed
929 }
930
931 @Override
932 public void onEstimateNextIteration(
933 final RobustEstimator<PreliminaryResult> estimator, final int iteration) {
934 if (listener != null) {
935 listener.onCalibrateNextIteration(
936 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this, iteration);
937 }
938 }
939
940 @Override
941 public void onEstimateProgressChange(
942 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
943 if (listener != null) {
944 listener.onCalibrateProgressChange(
945 LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this, progress);
946 }
947 }
948 });
949
950 try {
951 running = true;
952
953 if (listener != null) {
954 listener.onCalibrateStart(this);
955 }
956
957 inliersData = null;
958 innerEstimator.setConfidence(confidence);
959 innerEstimator.setMaxIterations(maxIterations);
960 innerEstimator.setProgressDelta(progressDelta);
961 innerEstimator.setStopThreshold(stopThreshold);
962 final var preliminaryResult = innerEstimator.estimate();
963 inliersData = innerEstimator.getInliersData();
964
965 attemptRefine(preliminaryResult);
966
967 if (listener != null) {
968 listener.onCalibrateEnd(this);
969 }
970
971 } catch (final com.irurueta.numerical.LockedException e) {
972 throw new LockedException(e);
973 } catch (final com.irurueta.numerical.NotReadyException e) {
974 throw new NotReadyException(e);
975 } catch (final RobustEstimatorException e) {
976 throw new CalibrationException(e);
977 } finally {
978 running = false;
979 }
980 }
981
982 /**
983 * Returns method being used for robust estimation.
984 *
985 * @return method being used for robust estimation.
986 */
987 @Override
988 public RobustEstimatorMethod getMethod() {
989 return RobustEstimatorMethod.LMEDS;
990 }
991
992 /**
993 * Indicates whether this calibrator requires quality scores for each
994 * measurement or not.
995 *
996 * @return true if quality scores are required, false otherwise.
997 */
998 @Override
999 public boolean isQualityScoresRequired() {
1000 return false;
1001 }
1002 }