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