1 /*
2 * Copyright (C) 2020 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.navigation.inertial.calibration.accelerometer;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.navigation.LockedException;
20 import com.irurueta.navigation.NotReadyException;
21 import com.irurueta.navigation.inertial.calibration.CalibrationException;
22 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
23 import com.irurueta.numerical.robust.MSACRobustEstimator;
24 import com.irurueta.numerical.robust.MSACRobustEstimatorListener;
25 import com.irurueta.numerical.robust.RobustEstimator;
26 import com.irurueta.numerical.robust.RobustEstimatorException;
27 import com.irurueta.numerical.robust.RobustEstimatorMethod;
28 import com.irurueta.units.Acceleration;
29
30 import java.util.List;
31
32 /**
33 * Robustly estimates accelerometer biases, cross couplings and scaling factors
34 * using a MSAC algorithm to discard outliers.
35 * <p>
36 * To use this calibrator at least 10 measurements taken at a single position
37 * where gravity norm is known must be taken at 10 different unknown
38 * orientations and zero velocity when common z-axis is assumed, otherwise at
39 * least 13 measurements are required.
40 * <p>
41 * Measured specific force is assumed to follow the model shown below:
42 * <pre>
43 * fmeas = ba + (I + Ma) * ftrue + w
44 * </pre>
45 * Where:
46 * - fmeas is the measured specific force. This is a 3x1 vector.
47 * - ba is accelerometer bias. Ideally, on a perfect accelerometer, this should be a
48 * 3x1 zero vector.
49 * - I is the 3x3 identity matrix.
50 * - Ma is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
51 * a perfect accelerometer, this should be a 3x3 zero matrix.
52 * - ftrue is ground-truth specific force.
53 * - w is measurement noise.
54 */
55 public class MSACRobustKnownGravityNormAccelerometerCalibrator extends RobustKnownGravityNormAccelerometerCalibrator {
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 MSACRobustKnownGravityNormAccelerometerCalibrator() {
79 super();
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 MSACRobustKnownGravityNormAccelerometerCalibrator(
89 final RobustKnownGravityNormAccelerometerCalibratorListener 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 MSACRobustKnownGravityNormAccelerometerCalibrator(final List<StandardDeviationBodyKinematics> measurements) {
101 super(measurements);
102 }
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 MSACRobustKnownGravityNormAccelerometerCalibrator(final boolean commonAxisUsed) {
112 super(commonAxisUsed);
113 }
114
115 /**
116 * Constructor.
117 *
118 * @param initialBias initial accelerometer bias to be used to find a solution.
119 * This must have length 3 and is expressed in meters per
120 * squared second (m/s^2).
121 * @throws IllegalArgumentException if provided bias array does not have length 3.
122 */
123 public MSACRobustKnownGravityNormAccelerometerCalibrator(final double[] initialBias) {
124 super(initialBias);
125 }
126
127 /**
128 * Constructor.
129 *
130 * @param initialBias initial bias to find a solution.
131 * @throws IllegalArgumentException if provided bias matrix is not 3x1.
132 */
133 public MSACRobustKnownGravityNormAccelerometerCalibrator(final Matrix initialBias) {
134 super(initialBias);
135 }
136
137 /**
138 * Constructor.
139 *
140 * @param initialBias initial bias to find a solution.
141 * @param initialMa initial scale factors and cross coupling errors matrix.
142 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
143 * scaling and coupling error matrix is not 3x3.
144 */
145 public MSACRobustKnownGravityNormAccelerometerCalibrator(final Matrix initialBias, final Matrix initialMa) {
146 super(initialBias, initialMa);
147 }
148
149 /**
150 * Constructor.
151 *
152 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
153 * squared second (m/s^2).
154 * @throws IllegalArgumentException if provided gravity norm value is negative.
155 */
156 public MSACRobustKnownGravityNormAccelerometerCalibrator(final Double groundTruthGravityNorm) {
157 super(groundTruthGravityNorm);
158 }
159
160 /**
161 * Constructor.
162 *
163 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
164 * squared second (m/s^2).
165 * @param measurements list of body kinematics measurements taken at a given position with
166 * different unknown orientations and containing the standard deviations
167 * of accelerometer and gyroscope measurements.
168 * @throws IllegalArgumentException if provided gravity norm value is negative.
169 */
170 public MSACRobustKnownGravityNormAccelerometerCalibrator(
171 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements) {
172 super(groundTruthGravityNorm, measurements);
173 }
174
175 /**
176 * Constructor.
177 *
178 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
179 * squared second (m/s^2).
180 * @param measurements list of body kinematics measurements taken at a given position with
181 * different unknown orientations and containing the standard deviations
182 * of accelerometer and gyroscope measurements.
183 * @param listener listener to be notified of events such as when estimation
184 * starts, ends or its progress significantly changes.
185 * @throws IllegalArgumentException if provided gravity norm value is negative.
186 */
187 public MSACRobustKnownGravityNormAccelerometerCalibrator(
188 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
189 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
190 super(groundTruthGravityNorm, measurements, listener);
191 }
192
193 /**
194 * Constructor.
195 *
196 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
197 * squared second (m/s^2).
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 commonAxisUsed indicates whether z-axis is assumed to be common for
202 * accelerometer and gyroscope.
203 * @throws IllegalArgumentException if provided gravity norm value is negative.
204 */
205 public MSACRobustKnownGravityNormAccelerometerCalibrator(
206 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
207 final boolean commonAxisUsed) {
208 super(groundTruthGravityNorm, measurements, commonAxisUsed);
209 }
210
211 /**
212 * Constructor.
213 *
214 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
215 * squared second (m/s^2).
216 * @param measurements list of body kinematics measurements taken at a given position with
217 * different unknown orientations and containing the standard deviations
218 * of accelerometer and gyroscope measurements.
219 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
220 * accelerometer and gyroscope.
221 * @param listener listener to be notified of events such as when estimation
222 * starts, ends or its progress significantly changes.
223 * @throws IllegalArgumentException if provided gravity norm value is negative.
224 */
225 public MSACRobustKnownGravityNormAccelerometerCalibrator(
226 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
227 final boolean commonAxisUsed, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
228 super(groundTruthGravityNorm, measurements, commonAxisUsed, listener);
229 }
230
231 /**
232 * Constructor.
233 *
234 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
235 * squared second (m/s^2).
236 * @param measurements collection of body kinematics measurements with standard
237 * deviations taken at the same position with zero velocity
238 * and unknown different orientations.
239 * @param initialBias initial accelerometer bias to be used to find a solution.
240 * This must have length 3 and is expressed in meters per
241 * squared second (m/s^2).
242 * @throws IllegalArgumentException if provided bias array does not have length 3 or
243 * if provided gravity norm value is negative.
244 */
245 public MSACRobustKnownGravityNormAccelerometerCalibrator(
246 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
247 final double[] initialBias) {
248 super(groundTruthGravityNorm, measurements, initialBias);
249 }
250
251 /**
252 * Constructor.
253 *
254 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
255 * squared second (m/s^2).
256 * @param measurements collection of body kinematics measurements with standard
257 * deviations taken at the same position with zero velocity
258 * and unknown different orientations.
259 * @param initialBias initial accelerometer bias to be used to find a solution.
260 * This must have length 3 and is expressed in meters per
261 * squared second (m/s^2).
262 * @param listener listener to handle events raised by this calibrator.
263 * @throws IllegalArgumentException if provided bias array does not have length 3 or
264 * if provided gravity norm value is negative.
265 */
266 public MSACRobustKnownGravityNormAccelerometerCalibrator(
267 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
268 final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
269 super(groundTruthGravityNorm, measurements, initialBias, listener);
270 }
271
272 /**
273 * Constructor.
274 *
275 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
276 * squared second (m/s^2).
277 * @param measurements collection of body kinematics measurements with standard
278 * deviations taken at the same position with zero velocity
279 * and unknown different orientations.
280 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
281 * accelerometer and gyroscope.
282 * @param initialBias initial accelerometer bias to be used to find a solution.
283 * This must have length 3 and is expressed in meters per
284 * squared second (m/s^2).
285 * @throws IllegalArgumentException if provided bias array does not have length 3 or
286 * if provided gravity norm value is negative.
287 */
288 public MSACRobustKnownGravityNormAccelerometerCalibrator(
289 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
290 final boolean commonAxisUsed, final double[] initialBias) {
291 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
292 }
293
294 /**
295 * Constructor.
296 *
297 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
298 * squared second (m/s^2).
299 * @param measurements collection of body kinematics measurements with standard
300 * deviations taken at the same position with zero velocity
301 * and unknown different orientations.
302 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
303 * accelerometer and gyroscope.
304 * @param initialBias initial accelerometer bias to be used to find a solution.
305 * This must have length 3 and is expressed in meters per
306 * squared second (m/s^2).
307 * @param listener listener to handle events raised by this calibrator.
308 * @throws IllegalArgumentException if provided bias array does not have length 3 or
309 * if provided gravity norm value is negative.
310 */
311 public MSACRobustKnownGravityNormAccelerometerCalibrator(
312 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
313 final boolean commonAxisUsed, final double[] initialBias,
314 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
315 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
316 }
317
318 /**
319 * Constructor.
320 *
321 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
322 * squared second (m/s^2).
323 * @param measurements collection of body kinematics measurements with standard
324 * deviations taken at the same position with zero velocity
325 * and unknown different orientations.
326 * @param initialBias initial bias to find a solution.
327 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
328 * if provided gravity norm value is negative.
329 */
330 public MSACRobustKnownGravityNormAccelerometerCalibrator(
331 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
332 final Matrix initialBias) {
333 super(groundTruthGravityNorm, measurements, initialBias);
334 }
335
336 /**
337 * Constructor.
338 *
339 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
340 * squared second (m/s^2).
341 * @param measurements collection of body kinematics measurements with standard
342 * deviations taken at the same position with zero velocity
343 * and unknown different orientations.
344 * @param initialBias initial bias to find a solution.
345 * @param listener listener to handle events raised by this calibrator.
346 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
347 * if provided gravity norm value is negative.
348 */
349 public MSACRobustKnownGravityNormAccelerometerCalibrator(
350 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
351 final Matrix initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
352 super(groundTruthGravityNorm, measurements, initialBias, listener);
353 }
354
355 /**
356 * Constructor.
357 *
358 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
359 * squared second (m/s^2).
360 * @param measurements collection of body kinematics measurements with standard
361 * deviations taken at the same position with zero velocity
362 * and unknown different orientations.
363 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
364 * accelerometer and gyroscope.
365 * @param initialBias initial bias to find a solution.
366 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
367 * if provided gravity norm value is negative.
368 */
369 public MSACRobustKnownGravityNormAccelerometerCalibrator(
370 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
371 final boolean commonAxisUsed, final Matrix initialBias) {
372 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
373 }
374
375 /**
376 * Constructor.
377 *
378 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
379 * squared second (m/s^2).
380 * @param measurements collection of body kinematics measurements with standard
381 * deviations taken at the same position with zero velocity
382 * and unknown different orientations.
383 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
384 * accelerometer and gyroscope.
385 * @param initialBias initial bias to find a solution.
386 * @param listener listener to handle events raised by this calibrator.
387 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
388 * if provided gravity norm value is negative.
389 */
390 public MSACRobustKnownGravityNormAccelerometerCalibrator(
391 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
392 final boolean commonAxisUsed, final Matrix initialBias,
393 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
394 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
395 }
396
397 /**
398 * Constructor.
399 *
400 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
401 * squared second (m/s^2).
402 * @param measurements collection of body kinematics measurements with standard
403 * deviations taken at the same position with zero velocity
404 * and unknown different orientations.
405 * @param initialBias initial bias to find a solution.
406 * @param initialMa initial scale factors and cross coupling errors matrix.
407 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
408 * scaling and coupling error matrix is not 3x3 or
409 * if provided gravity norm value is negative.
410 */
411 public MSACRobustKnownGravityNormAccelerometerCalibrator(
412 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
413 final Matrix initialBias, final Matrix initialMa) {
414 super(groundTruthGravityNorm, measurements, initialBias, initialMa);
415 }
416
417 /**
418 * Constructor.
419 *
420 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
421 * squared second (m/s^2).
422 * @param measurements collection of body kinematics measurements with standard
423 * deviations taken at the same position with zero velocity
424 * and unknown different orientations.
425 * @param initialBias initial bias to find a solution.
426 * @param initialMa initial scale factors and cross coupling errors matrix.
427 * @param listener listener to handle events raised by this calibrator.
428 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
429 * scaling and coupling error matrix is not 3x3 or
430 * if provided gravity norm value is negative.
431 */
432 public MSACRobustKnownGravityNormAccelerometerCalibrator(
433 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
434 final Matrix initialBias, final Matrix initialMa,
435 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
436 super(groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
437 }
438
439 /**
440 * Constructor.
441 *
442 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
443 * squared second (m/s^2).
444 * @param measurements collection of body kinematics measurements with standard
445 * deviations taken at the same position with zero velocity
446 * and unknown different orientations.
447 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
448 * accelerometer and gyroscope.
449 * @param initialBias initial bias to find a solution.
450 * @param initialMa initial scale factors and cross coupling errors matrix.
451 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
452 * scaling and coupling error matrix is not 3x3 or
453 * if provided gravity norm value is negative.
454 */
455 public MSACRobustKnownGravityNormAccelerometerCalibrator(
456 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
457 final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa) {
458 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
459 }
460
461 /**
462 * Constructor.
463 *
464 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
465 * squared second (m/s^2).
466 * @param measurements collection of body kinematics measurements with standard
467 * deviations taken at the same position with zero velocity
468 * and unknown different orientations.
469 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
470 * accelerometer and gyroscope.
471 * @param initialBias initial bias to find a solution.
472 * @param initialMa initial scale factors and cross coupling errors matrix.
473 * @param listener listener to handle events raised by this calibrator.
474 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
475 * scaling and coupling error matrix is not 3x3 or
476 * if provided gravity norm value is negative.
477 */
478 public MSACRobustKnownGravityNormAccelerometerCalibrator(
479 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
480 final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa,
481 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
482 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
483 }
484
485 /**
486 * Constructor.
487 *
488 * @param groundTruthGravityNorm ground truth gravity norm.
489 * @throws IllegalArgumentException if provided gravity norm value is negative.
490 */
491 public MSACRobustKnownGravityNormAccelerometerCalibrator(final Acceleration groundTruthGravityNorm) {
492 super(groundTruthGravityNorm);
493 }
494
495 /**
496 * Constructor.
497 *
498 * @param groundTruthGravityNorm ground truth gravity norm.
499 * @param measurements list of body kinematics measurements taken at a given position with
500 * different unknown orientations and containing the standard deviations
501 * of accelerometer and gyroscope measurements.
502 * @throws IllegalArgumentException if provided gravity norm value is negative.
503 */
504 public MSACRobustKnownGravityNormAccelerometerCalibrator(
505 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements) {
506 super(groundTruthGravityNorm, measurements);
507 }
508
509 /**
510 * Constructor.
511 *
512 * @param groundTruthGravityNorm ground truth gravity norm.
513 * @param measurements list of body kinematics measurements taken at a given position with
514 * different unknown orientations and containing the standard deviations
515 * of accelerometer and gyroscope measurements.
516 * @param listener listener to be notified of events such as when estimation
517 * starts, ends or its progress significantly changes.
518 * @throws IllegalArgumentException if provided gravity norm value is negative.
519 */
520 public MSACRobustKnownGravityNormAccelerometerCalibrator(
521 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
522 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
523 super(groundTruthGravityNorm, measurements, listener);
524 }
525
526 /**
527 * Constructor.
528 *
529 * @param groundTruthGravityNorm ground truth gravity norm.
530 * @param measurements list of body kinematics measurements taken at a given position with
531 * different unknown orientations and containing the standard deviations
532 * of accelerometer and gyroscope measurements.
533 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
534 * accelerometer and gyroscope.
535 * @throws IllegalArgumentException if provided gravity norm value is negative.
536 */
537 public MSACRobustKnownGravityNormAccelerometerCalibrator(
538 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
539 final boolean commonAxisUsed) {
540 super(groundTruthGravityNorm, measurements, commonAxisUsed);
541 }
542
543 /**
544 * Constructor.
545 *
546 * @param groundTruthGravityNorm ground truth gravity norm.
547 * @param measurements list of body kinematics measurements taken at a given position with
548 * different unknown orientations and containing the standard deviations
549 * of accelerometer and gyroscope measurements.
550 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
551 * accelerometer and gyroscope.
552 * @param listener listener to be notified of events such as when estimation
553 * starts, ends or its progress significantly changes.
554 * @throws IllegalArgumentException if provided gravity norm value is negative.
555 */
556 public MSACRobustKnownGravityNormAccelerometerCalibrator(
557 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
558 final boolean commonAxisUsed, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
559 super(groundTruthGravityNorm, measurements, commonAxisUsed, listener);
560 }
561
562 /**
563 * Constructor.
564 *
565 * @param groundTruthGravityNorm ground truth gravity norm.
566 * @param measurements collection of body kinematics measurements with standard
567 * deviations taken at the same position with zero velocity
568 * and unknown different orientations.
569 * @param initialBias initial accelerometer bias to be used to find a solution.
570 * This must have length 3 and is expressed in meters per
571 * squared second (m/s^2).
572 * @throws IllegalArgumentException if provided bias array does not have length 3 or
573 * if provided gravity norm value is negative.
574 */
575 public MSACRobustKnownGravityNormAccelerometerCalibrator(
576 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
577 final double[] initialBias) {
578 super(groundTruthGravityNorm, measurements, initialBias);
579 }
580
581 /**
582 * Constructor.
583 *
584 * @param groundTruthGravityNorm ground truth gravity norm.
585 * @param measurements collection of body kinematics measurements with standard
586 * deviations taken at the same position with zero velocity
587 * and unknown different orientations.
588 * @param initialBias initial accelerometer bias to be used to find a solution.
589 * This must have length 3 and is expressed in meters per
590 * squared second (m/s^2).
591 * @param listener listener to handle events raised by this calibrator.
592 * @throws IllegalArgumentException if provided bias array does not have length 3 or
593 * if provided gravity norm value is negative.
594 */
595 public MSACRobustKnownGravityNormAccelerometerCalibrator(
596 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
597 final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
598 super(groundTruthGravityNorm, measurements, initialBias, listener);
599 }
600
601 /**
602 * Constructor.
603 *
604 * @param groundTruthGravityNorm ground truth gravity norm.
605 * @param measurements collection of body kinematics measurements with standard
606 * deviations taken at the same position with zero velocity
607 * and unknown different orientations.
608 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
609 * accelerometer and gyroscope.
610 * @param initialBias initial accelerometer bias to be used to find a solution.
611 * This must have length 3 and is expressed in meters per
612 * squared second (m/s^2).
613 * @throws IllegalArgumentException if provided bias array does not have length 3 or
614 * if provided gravity norm value is negative.
615 */
616 public MSACRobustKnownGravityNormAccelerometerCalibrator(
617 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
618 final boolean commonAxisUsed, final double[] initialBias) {
619 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
620 }
621
622 /**
623 * Constructor.
624 *
625 * @param groundTruthGravityNorm ground truth gravity norm.
626 * @param measurements collection of body kinematics measurements with standard
627 * deviations taken at the same position with zero velocity
628 * and unknown different orientations.
629 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
630 * accelerometer and gyroscope.
631 * @param initialBias initial accelerometer bias to be used to find a solution.
632 * This must have length 3 and is expressed in meters per
633 * squared second (m/s^2).
634 * @param listener listener to handle events raised by this calibrator.
635 * @throws IllegalArgumentException if provided bias array does not have length 3 or
636 * if provided gravity norm value is negative.
637 */
638 public MSACRobustKnownGravityNormAccelerometerCalibrator(
639 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
640 final boolean commonAxisUsed, final double[] initialBias,
641 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
642 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
643 }
644
645 /**
646 * Constructor.
647 *
648 * @param groundTruthGravityNorm ground truth gravity norm.
649 * @param measurements collection of body kinematics measurements with standard
650 * deviations taken at the same position with zero velocity
651 * and unknown different orientations.
652 * @param initialBias initial bias to find a solution.
653 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
654 * if provided gravity norm value is negative.
655 */
656 public MSACRobustKnownGravityNormAccelerometerCalibrator(
657 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
658 final Matrix initialBias) {
659 super(groundTruthGravityNorm, measurements, initialBias);
660 }
661
662 /**
663 * Constructor.
664 *
665 * @param groundTruthGravityNorm ground truth gravity norm.
666 * @param measurements collection of body kinematics measurements with standard
667 * deviations taken at the same position with zero velocity
668 * and unknown different orientations.
669 * @param initialBias initial bias to find a solution.
670 * @param listener listener to handle events raised by this calibrator.
671 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
672 * if provided gravity norm value is negative.
673 */
674 public MSACRobustKnownGravityNormAccelerometerCalibrator(
675 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
676 final Matrix initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
677 super(groundTruthGravityNorm, measurements, initialBias, listener);
678 }
679
680 /**
681 * Constructor.
682 *
683 * @param groundTruthGravityNorm ground truth gravity norm.
684 * @param measurements collection of body kinematics measurements with standard
685 * deviations taken at the same position with zero velocity
686 * and unknown different orientations.
687 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
688 * accelerometer and gyroscope.
689 * @param initialBias initial bias to find a solution.
690 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
691 * if provided gravity norm value is negative.
692 */
693 public MSACRobustKnownGravityNormAccelerometerCalibrator(
694 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
695 final boolean commonAxisUsed, final Matrix initialBias) {
696 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
697 }
698
699 /**
700 * Constructor.
701 *
702 * @param groundTruthGravityNorm ground truth gravity norm.
703 * @param measurements collection of body kinematics measurements with standard
704 * deviations taken at the same position with zero velocity
705 * and unknown different orientations.
706 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
707 * accelerometer and gyroscope.
708 * @param initialBias initial bias to find a solution.
709 * @param listener listener to handle events raised by this calibrator.
710 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
711 * if provided gravity norm value is negative.
712 */
713 public MSACRobustKnownGravityNormAccelerometerCalibrator(
714 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
715 final boolean commonAxisUsed, final Matrix initialBias,
716 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
717 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
718 }
719
720 /**
721 * Constructor.
722 *
723 * @param groundTruthGravityNorm ground truth gravity norm.
724 * @param measurements collection of body kinematics measurements with standard
725 * deviations taken at the same position with zero velocity
726 * and unknown different orientations.
727 * @param initialBias initial bias to find a solution.
728 * @param initialMa initial scale factors and cross coupling errors matrix.
729 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
730 * scaling and coupling error matrix is not 3x3 or
731 * if provided gravity norm value is negative.
732 */
733 public MSACRobustKnownGravityNormAccelerometerCalibrator(
734 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
735 final Matrix initialBias, final Matrix initialMa) {
736 super(groundTruthGravityNorm, measurements, initialBias, initialMa);
737 }
738
739 /**
740 * Constructor.
741 *
742 * @param groundTruthGravityNorm ground truth gravity norm.
743 * @param measurements collection of body kinematics measurements with standard
744 * deviations taken at the same position with zero velocity
745 * and unknown different orientations.
746 * @param initialBias initial bias to find a solution.
747 * @param initialMa initial scale factors and cross coupling errors matrix.
748 * @param listener listener to handle events raised by this calibrator.
749 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
750 * scaling and coupling error matrix is not 3x3 or
751 * if provided gravity norm value is negative.
752 */
753 public MSACRobustKnownGravityNormAccelerometerCalibrator(
754 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
755 final Matrix initialBias, final Matrix initialMa,
756 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
757 super(groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
758 }
759
760 /**
761 * Constructor.
762 *
763 * @param groundTruthGravityNorm ground truth gravity norm.
764 * @param measurements collection of body kinematics measurements with standard
765 * deviations taken at the same position with zero velocity
766 * and unknown different orientations.
767 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
768 * accelerometer and gyroscope.
769 * @param initialBias initial bias to find a solution.
770 * @param initialMa initial scale factors and cross coupling errors matrix.
771 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
772 * scaling and coupling error matrix is not 3x3 or
773 * if provided gravity norm value is negative.
774 */
775 public MSACRobustKnownGravityNormAccelerometerCalibrator(
776 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
777 final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa) {
778 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
779 }
780
781 /**
782 * Constructor.
783 *
784 * @param groundTruthGravityNorm ground truth gravity norm.
785 * @param measurements collection of body kinematics measurements with standard
786 * deviations taken at the same position with zero velocity
787 * and unknown different orientations.
788 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
789 * accelerometer and gyroscope.
790 * @param initialBias initial bias to find a solution.
791 * @param initialMa initial scale factors and cross coupling errors matrix.
792 * @param listener listener to handle events raised by this calibrator.
793 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
794 * scaling and coupling error matrix is not 3x3 or
795 * if provided gravity norm value is negative.
796 */
797 public MSACRobustKnownGravityNormAccelerometerCalibrator(
798 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
799 final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa,
800 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
801 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
802 }
803
804 /**
805 * Returns threshold to determine whether samples are inliers or not.
806 *
807 * @return threshold to determine whether samples are inliers or not.
808 */
809 public double getThreshold() {
810 return threshold;
811 }
812
813 /**
814 * Sets threshold to determine whether samples are inliers or not.
815 *
816 * @param threshold threshold to be set.
817 * @throws IllegalArgumentException if provided value is equal or less than
818 * zero.
819 * @throws LockedException if calibrator is currently running.
820 */
821 public void setThreshold(final double threshold) throws LockedException {
822 if (running) {
823 throw new LockedException();
824 }
825 if (threshold <= MIN_THRESHOLD) {
826 throw new IllegalArgumentException();
827 }
828 this.threshold = threshold;
829 }
830
831 /**
832 * Estimates accelerometer calibration parameters containing bias, scale factors
833 * and cross-coupling errors.
834 *
835 * @throws LockedException if calibrator is currently running.
836 * @throws NotReadyException if calibrator is not ready.
837 * @throws CalibrationException if estimation fails for numerical reasons.
838 */
839 @SuppressWarnings("DuplicatedCode")
840 @Override
841 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
842 if (running) {
843 throw new LockedException();
844 }
845 if (!isReady()) {
846 throw new NotReadyException();
847 }
848
849 final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
850 @Override
851 public double getThreshold() {
852 return threshold;
853 }
854
855 @Override
856 public int getTotalSamples() {
857 return measurements.size();
858 }
859
860 @Override
861 public int getSubsetSize() {
862 return preliminarySubsetSize;
863 }
864
865 @Override
866 public void estimatePreliminarSolutions(
867 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
868 computePreliminarySolutions(samplesIndices, solutions);
869 }
870
871 @Override
872 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
873 return computeError(measurements.get(i), currentEstimation);
874 }
875
876 @Override
877 public boolean isReady() {
878 return MSACRobustKnownGravityNormAccelerometerCalibrator.super.isReady();
879 }
880
881 @Override
882 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
883 // no action needed
884 }
885
886 @Override
887 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
888 // no action needed
889 }
890
891 @Override
892 public void onEstimateNextIteration(
893 final RobustEstimator<PreliminaryResult> estimator, final int iteration) {
894 if (listener != null) {
895 listener.onCalibrateNextIteration(
896 MSACRobustKnownGravityNormAccelerometerCalibrator.this, iteration);
897 }
898 }
899
900 @Override
901 public void onEstimateProgressChange(
902 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
903 if (listener != null) {
904 listener.onCalibrateProgressChange(
905 MSACRobustKnownGravityNormAccelerometerCalibrator.this, progress);
906 }
907 }
908 });
909
910 try {
911 running = true;
912
913 if (listener != null) {
914 listener.onCalibrateStart(this);
915 }
916
917 inliersData = null;
918 innerEstimator.setConfidence(confidence);
919 innerEstimator.setMaxIterations(maxIterations);
920 innerEstimator.setProgressDelta(progressDelta);
921 final var preliminaryResult = innerEstimator.estimate();
922 inliersData = innerEstimator.getInliersData();
923
924 attemptRefine(preliminaryResult);
925
926 if (listener != null) {
927 listener.onCalibrateEnd(this);
928 }
929
930 } catch (final com.irurueta.numerical.LockedException e) {
931 throw new LockedException(e);
932 } catch (final com.irurueta.numerical.NotReadyException e) {
933 throw new NotReadyException(e);
934 } catch (final RobustEstimatorException e) {
935 throw new CalibrationException(e);
936 } finally {
937 running = false;
938 }
939 }
940
941 /**
942 * Returns method being used for robust estimation.
943 *
944 * @return method being used for robust estimation.
945 */
946 @Override
947 public RobustEstimatorMethod getMethod() {
948 return RobustEstimatorMethod.MSAC;
949 }
950
951 /**
952 * Indicates whether this calibrator requires quality scores for each
953 * measurement or not.
954 *
955 * @return true if quality scores are required, false otherwise.
956 */
957 @Override
958 public boolean isQualityScoresRequired() {
959 return false;
960 }
961 }