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