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