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