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