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.PROSACRobustEstimator;
24 import com.irurueta.numerical.robust.PROSACRobustEstimatorListener;
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 PROSAC 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 PROSACRobustKnownGravityNormAccelerometerCalibrator extends RobustKnownGravityNormAccelerometerCalibrator {
56
57 /**
58 * Constant defining default threshold to determine whether samples are inliers or not.
59 */
60 public static final double DEFAULT_THRESHOLD = 1e-2;
61
62 /**
63 * Minimum value that can be set as threshold.
64 * Threshold must be strictly greater than 0.0.
65 */
66 public static final double MIN_THRESHOLD = 0.0;
67
68 /**
69 * Indicates that by default inliers will only be computed but not kept.
70 */
71 public static final boolean DEFAULT_COMPUTE_AND_KEEP_INLIERS = false;
72
73 /**
74 * Indicates that by default residuals will only be computed but not kept.
75 */
76 public static final boolean DEFAULT_COMPUTE_AND_KEEP_RESIDUALS = false;
77
78 /**
79 * Threshold to determine whether samples are inliers or not when testing possible solutions.
80 * The threshold refers to the amount of error on distance between estimated position and
81 * distances provided for each sample.
82 */
83 private double threshold = DEFAULT_THRESHOLD;
84
85 /**
86 * Indicates whether inliers must be computed and kept.
87 */
88 private boolean computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
89
90 /**
91 * Indicates whether residuals must be computed and kept.
92 */
93 private boolean computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
94
95 /**
96 * Quality scores corresponding to each provided sample.
97 * The larger the score value the better the quality of the sample.
98 */
99 private double[] qualityScores;
100
101 /**
102 * Constructor.
103 */
104 public PROSACRobustKnownGravityNormAccelerometerCalibrator() {
105 super();
106 }
107
108 /**
109 * Constructor.
110 *
111 * @param listener listener to be notified of events such as when estimation
112 * starts, ends or its progress significantly changes.
113 */
114 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
115 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
116 super(listener);
117 }
118
119 /**
120 * Constructor.
121 *
122 * @param measurements collection of body kinematics measurements with standard
123 * deviations taken at the same position with zero velocity
124 * and unknown different orientations.
125 */
126 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
127 final List<StandardDeviationBodyKinematics> measurements) {
128 super(measurements);
129 }
130
131
132 /**
133 * Constructor.
134 *
135 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
136 * accelerometer and gyroscope.
137 */
138 public PROSACRobustKnownGravityNormAccelerometerCalibrator(final boolean commonAxisUsed) {
139 super(commonAxisUsed);
140 }
141
142 /**
143 * Constructor.
144 *
145 * @param initialBias initial accelerometer bias to be used to find a solution.
146 * This must have length 3 and is expressed in meters per
147 * squared second (m/s^2).
148 * @throws IllegalArgumentException if provided bias array does not have length 3.
149 */
150 public PROSACRobustKnownGravityNormAccelerometerCalibrator(final double[] initialBias) {
151 super(initialBias);
152 }
153
154 /**
155 * Constructor.
156 *
157 * @param initialBias initial bias to find a solution.
158 * @throws IllegalArgumentException if provided bias matrix is not 3x1.
159 */
160 public PROSACRobustKnownGravityNormAccelerometerCalibrator(final Matrix initialBias) {
161 super(initialBias);
162 }
163
164 /**
165 * Constructor.
166 *
167 * @param initialBias initial bias to find a solution.
168 * @param initialMa initial scale factors and cross coupling errors matrix.
169 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
170 * scaling and coupling error matrix is not 3x3.
171 */
172 public PROSACRobustKnownGravityNormAccelerometerCalibrator(final Matrix initialBias, final Matrix initialMa) {
173 super(initialBias, initialMa);
174 }
175
176 /**
177 * Constructor.
178 *
179 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
180 * squared second (m/s^2).
181 * @throws IllegalArgumentException if provided gravity norm value is negative.
182 */
183 public PROSACRobustKnownGravityNormAccelerometerCalibrator(final Double groundTruthGravityNorm) {
184 super(groundTruthGravityNorm);
185 }
186
187 /**
188 * Constructor.
189 *
190 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
191 * squared second (m/s^2).
192 * @param measurements list of body kinematics measurements taken at a given position with
193 * different unknown orientations and containing the standard deviations
194 * of accelerometer and gyroscope measurements.
195 * @throws IllegalArgumentException if provided gravity norm value is negative.
196 */
197 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
198 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements) {
199 super(groundTruthGravityNorm, measurements);
200 }
201
202 /**
203 * Constructor.
204 *
205 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
206 * squared second (m/s^2).
207 * @param measurements list of body kinematics measurements taken at a given position with
208 * different unknown orientations and containing the standard deviations
209 * of accelerometer and gyroscope measurements.
210 * @param listener listener to be notified of events such as when estimation
211 * starts, ends or its progress significantly changes.
212 * @throws IllegalArgumentException if provided gravity norm value is negative.
213 */
214 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
215 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
216 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
217 super(groundTruthGravityNorm, measurements, listener);
218 }
219
220 /**
221 * Constructor.
222 *
223 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
224 * squared second (m/s^2).
225 * @param measurements list of body kinematics measurements taken at a given position with
226 * different unknown orientations and containing the standard deviations
227 * of accelerometer and gyroscope measurements.
228 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
229 * accelerometer and gyroscope.
230 * @throws IllegalArgumentException if provided gravity norm value is negative.
231 */
232 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
233 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
234 final boolean commonAxisUsed) {
235 super(groundTruthGravityNorm, measurements, commonAxisUsed);
236 }
237
238 /**
239 * Constructor.
240 *
241 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
242 * squared second (m/s^2).
243 * @param measurements list of body kinematics measurements taken at a given position with
244 * different unknown orientations and containing the standard deviations
245 * of accelerometer and gyroscope measurements.
246 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
247 * accelerometer and gyroscope.
248 * @param listener listener to be notified of events such as when estimation
249 * starts, ends or its progress significantly changes.
250 * @throws IllegalArgumentException if provided gravity norm value is negative.
251 */
252 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
253 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
254 final boolean commonAxisUsed, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
255 super(groundTruthGravityNorm, measurements, commonAxisUsed, listener);
256 }
257
258 /**
259 * Constructor.
260 *
261 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
262 * squared second (m/s^2).
263 * @param measurements collection of body kinematics measurements with standard
264 * deviations taken at the same position with zero velocity
265 * and unknown different orientations.
266 * @param initialBias initial accelerometer bias to be used to find a solution.
267 * This must have length 3 and is expressed in meters per
268 * squared second (m/s^2).
269 * @throws IllegalArgumentException if provided bias array does not have length 3 or
270 * if provided gravity norm value is negative.
271 */
272 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
273 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
274 final double[] initialBias) {
275 super(groundTruthGravityNorm, measurements, initialBias);
276 }
277
278 /**
279 * Constructor.
280 *
281 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
282 * squared second (m/s^2).
283 * @param measurements collection of body kinematics measurements with standard
284 * deviations taken at the same position with zero velocity
285 * and unknown different orientations.
286 * @param initialBias initial accelerometer bias to be used to find a solution.
287 * This must have length 3 and is expressed in meters per
288 * squared second (m/s^2).
289 * @param listener listener to handle events raised by this calibrator.
290 * @throws IllegalArgumentException if provided bias array does not have length 3 or
291 * if provided gravity norm value is negative.
292 */
293 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
294 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
295 final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
296 super(groundTruthGravityNorm, measurements, initialBias, listener);
297 }
298
299 /**
300 * Constructor.
301 *
302 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
303 * squared second (m/s^2).
304 * @param measurements collection of body kinematics measurements with standard
305 * deviations taken at the same position with zero velocity
306 * and unknown different orientations.
307 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
308 * accelerometer and gyroscope.
309 * @param initialBias initial accelerometer bias to be used to find a solution.
310 * This must have length 3 and is expressed in meters per
311 * squared second (m/s^2).
312 * @throws IllegalArgumentException if provided bias array does not have length 3 or
313 * if provided gravity norm value is negative.
314 */
315 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
316 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
317 final boolean commonAxisUsed, final double[] initialBias) {
318 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
319 }
320
321 /**
322 * Constructor.
323 *
324 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
325 * squared second (m/s^2).
326 * @param measurements collection of body kinematics measurements with standard
327 * deviations taken at the same position with zero velocity
328 * and unknown different orientations.
329 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
330 * accelerometer and gyroscope.
331 * @param initialBias initial accelerometer bias to be used to find a solution.
332 * This must have length 3 and is expressed in meters per
333 * squared second (m/s^2).
334 * @param listener listener to handle events raised by this calibrator.
335 * @throws IllegalArgumentException if provided bias array does not have length 3 or
336 * if provided gravity norm value is negative.
337 */
338 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
339 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
340 final boolean commonAxisUsed, final double[] initialBias,
341 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
342 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
343 }
344
345 /**
346 * Constructor.
347 *
348 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
349 * squared second (m/s^2).
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 initialBias initial bias to find a solution.
354 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
355 * if provided gravity norm value is negative.
356 */
357 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
358 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
359 final Matrix initialBias) {
360 super(groundTruthGravityNorm, measurements, initialBias);
361 }
362
363 /**
364 * Constructor.
365 *
366 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
367 * squared second (m/s^2).
368 * @param measurements collection of body kinematics measurements with standard
369 * deviations taken at the same position with zero velocity
370 * and unknown different orientations.
371 * @param initialBias initial bias to find a solution.
372 * @param listener listener to handle events raised by this calibrator.
373 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
374 * if provided gravity norm value is negative.
375 */
376 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
377 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
378 final Matrix initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
379 super(groundTruthGravityNorm, measurements, initialBias, listener);
380 }
381
382 /**
383 * Constructor.
384 *
385 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
386 * squared second (m/s^2).
387 * @param measurements collection of body kinematics measurements with standard
388 * deviations taken at the same position with zero velocity
389 * and unknown different orientations.
390 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
391 * accelerometer and gyroscope.
392 * @param initialBias initial bias to find a solution.
393 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
394 * if provided gravity norm value is negative.
395 */
396 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
397 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
398 final boolean commonAxisUsed, final Matrix initialBias) {
399 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
400 }
401
402 /**
403 * Constructor.
404 *
405 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
406 * squared second (m/s^2).
407 * @param measurements collection of body kinematics measurements with standard
408 * deviations taken at the same position with zero velocity
409 * and unknown different orientations.
410 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
411 * accelerometer and gyroscope.
412 * @param initialBias initial bias to find a solution.
413 * @param listener listener to handle events raised by this calibrator.
414 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
415 * if provided gravity norm value is negative.
416 */
417 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
418 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
419 final boolean commonAxisUsed, final Matrix initialBias,
420 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
421 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
422 }
423
424 /**
425 * Constructor.
426 *
427 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
428 * squared second (m/s^2).
429 * @param measurements collection of body kinematics measurements with standard
430 * deviations taken at the same position with zero velocity
431 * and unknown different orientations.
432 * @param initialBias initial bias to find a solution.
433 * @param initialMa initial scale factors and cross coupling errors matrix.
434 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
435 * scaling and coupling error matrix is not 3x3 or
436 * if provided gravity norm value is negative.
437 */
438 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
439 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
440 final Matrix initialBias, final Matrix initialMa) {
441 super(groundTruthGravityNorm, measurements, initialBias, initialMa);
442 }
443
444 /**
445 * Constructor.
446 *
447 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
448 * squared second (m/s^2).
449 * @param measurements collection of body kinematics measurements with standard
450 * deviations taken at the same position with zero velocity
451 * and unknown different orientations.
452 * @param initialBias initial bias to find a solution.
453 * @param initialMa initial scale factors and cross coupling errors matrix.
454 * @param listener listener to handle events raised by this calibrator.
455 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
456 * scaling and coupling error matrix is not 3x3 or
457 * if provided gravity norm value is negative.
458 */
459 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
460 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
461 final Matrix initialBias, final Matrix initialMa,
462 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
463 super(groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
464 }
465
466 /**
467 * Constructor.
468 *
469 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
470 * squared second (m/s^2).
471 * @param measurements collection of body kinematics measurements with standard
472 * deviations taken at the same position with zero velocity
473 * and unknown different orientations.
474 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
475 * accelerometer and gyroscope.
476 * @param initialBias initial bias to find a solution.
477 * @param initialMa initial scale factors and cross coupling errors matrix.
478 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
479 * scaling and coupling error matrix is not 3x3 or
480 * if provided gravity norm value is negative.
481 */
482 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
483 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
484 final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa) {
485 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
486 }
487
488 /**
489 * Constructor.
490 *
491 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
492 * squared second (m/s^2).
493 * @param measurements collection of body kinematics measurements with standard
494 * deviations taken at the same position with zero velocity
495 * and unknown different orientations.
496 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
497 * accelerometer and gyroscope.
498 * @param initialBias initial bias to find a solution.
499 * @param initialMa initial scale factors and cross coupling errors matrix.
500 * @param listener listener to handle events raised by this calibrator.
501 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
502 * scaling and coupling error matrix is not 3x3 or
503 * if provided gravity norm value is negative.
504 */
505 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
506 final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
507 final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa,
508 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
509 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
510 }
511
512 /**
513 * Constructor.
514 *
515 * @param groundTruthGravityNorm ground truth gravity norm.
516 * @throws IllegalArgumentException if provided gravity norm value is negative.
517 */
518 public PROSACRobustKnownGravityNormAccelerometerCalibrator(final Acceleration groundTruthGravityNorm) {
519 super(groundTruthGravityNorm);
520 }
521
522 /**
523 * Constructor.
524 *
525 * @param groundTruthGravityNorm ground truth gravity norm.
526 * @param measurements list of body kinematics measurements taken at a given position with
527 * different unknown orientations and containing the standard deviations
528 * of accelerometer and gyroscope measurements.
529 * @throws IllegalArgumentException if provided gravity norm value is negative.
530 */
531 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
532 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements) {
533 super(groundTruthGravityNorm, measurements);
534 }
535
536 /**
537 * Constructor.
538 *
539 * @param groundTruthGravityNorm ground truth gravity norm.
540 * @param measurements list of body kinematics measurements taken at a given position with
541 * different unknown orientations and containing the standard deviations
542 * of accelerometer and gyroscope measurements.
543 * @param listener listener to be notified of events such as when estimation
544 * starts, ends or its progress significantly changes.
545 * @throws IllegalArgumentException if provided gravity norm value is negative.
546 */
547 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
548 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
549 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
550 super(groundTruthGravityNorm, measurements, listener);
551 }
552
553 /**
554 * Constructor.
555 *
556 * @param groundTruthGravityNorm ground truth gravity norm.
557 * @param measurements list of body kinematics measurements taken at a given position with
558 * different unknown orientations and containing the standard deviations
559 * of accelerometer and gyroscope measurements.
560 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
561 * accelerometer and gyroscope.
562 * @throws IllegalArgumentException if provided gravity norm value is negative.
563 */
564 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
565 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
566 final boolean commonAxisUsed) {
567 super(groundTruthGravityNorm, measurements, commonAxisUsed);
568 }
569
570 /**
571 * Constructor.
572 *
573 * @param groundTruthGravityNorm ground truth gravity norm.
574 * @param measurements list of body kinematics measurements taken at a given position with
575 * different unknown orientations and containing the standard deviations
576 * of accelerometer and gyroscope measurements.
577 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
578 * accelerometer and gyroscope.
579 * @param listener listener to be notified of events such as when estimation
580 * starts, ends or its progress significantly changes.
581 * @throws IllegalArgumentException if provided gravity norm value is negative.
582 */
583 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
584 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
585 final boolean commonAxisUsed, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
586 super(groundTruthGravityNorm, measurements, commonAxisUsed, listener);
587 }
588
589 /**
590 * Constructor.
591 *
592 * @param groundTruthGravityNorm ground truth gravity norm.
593 * @param measurements collection of body kinematics measurements with standard
594 * deviations taken at the same position with zero velocity
595 * and unknown different orientations.
596 * @param initialBias initial accelerometer bias to be used to find a solution.
597 * This must have length 3 and is expressed in meters per
598 * squared second (m/s^2).
599 * @throws IllegalArgumentException if provided bias array does not have length 3 or
600 * if provided gravity norm value is negative.
601 */
602 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
603 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
604 final double[] initialBias) {
605 super(groundTruthGravityNorm, measurements, initialBias);
606 }
607
608 /**
609 * Constructor.
610 *
611 * @param groundTruthGravityNorm ground truth gravity norm.
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 initialBias initial accelerometer bias to be used to find a solution.
616 * This must have length 3 and is expressed in meters per
617 * squared second (m/s^2).
618 * @param listener listener to handle events raised by this calibrator.
619 * @throws IllegalArgumentException if provided bias array does not have length 3 or
620 * if provided gravity norm value is negative.
621 */
622 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
623 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
624 final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
625 super(groundTruthGravityNorm, measurements, initialBias, listener);
626 }
627
628 /**
629 * Constructor.
630 *
631 * @param groundTruthGravityNorm ground truth gravity norm.
632 * @param measurements collection of body kinematics measurements with standard
633 * deviations taken at the same position with zero velocity
634 * and unknown different orientations.
635 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
636 * accelerometer and gyroscope.
637 * @param initialBias initial accelerometer bias to be used to find a solution.
638 * This must have length 3 and is expressed in meters per
639 * squared second (m/s^2).
640 * @throws IllegalArgumentException if provided bias array does not have length 3 or
641 * if provided gravity norm value is negative.
642 */
643 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
644 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
645 final boolean commonAxisUsed, final double[] initialBias) {
646 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
647 }
648
649 /**
650 * Constructor.
651 *
652 * @param groundTruthGravityNorm ground truth gravity norm.
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 accelerometer bias to be used to find a solution.
659 * This must have length 3 and is expressed in meters per
660 * squared second (m/s^2).
661 * @param listener listener to handle events raised by this calibrator.
662 * @throws IllegalArgumentException if provided bias array does not have length 3 or
663 * if provided gravity norm value is negative.
664 */
665 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
666 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
667 final boolean commonAxisUsed, final double[] initialBias,
668 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
669 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
670 }
671
672 /**
673 * Constructor.
674 *
675 * @param groundTruthGravityNorm ground truth gravity norm.
676 * @param measurements collection of body kinematics measurements with standard
677 * deviations taken at the same position with zero velocity
678 * and unknown different orientations.
679 * @param initialBias initial bias to find a solution.
680 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
681 * if provided gravity norm value is negative.
682 */
683 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
684 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
685 final Matrix initialBias) {
686 super(groundTruthGravityNorm, measurements, initialBias);
687 }
688
689 /**
690 * Constructor.
691 *
692 * @param groundTruthGravityNorm ground truth gravity norm.
693 * @param measurements collection of body kinematics measurements with standard
694 * deviations taken at the same position with zero velocity
695 * and unknown different orientations.
696 * @param initialBias initial bias to find a solution.
697 * @param listener listener to handle events raised by this calibrator.
698 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
699 * if provided gravity norm value is negative.
700 */
701 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
702 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
703 final Matrix initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
704 super(groundTruthGravityNorm, measurements, initialBias, listener);
705 }
706
707 /**
708 * Constructor.
709 *
710 * @param groundTruthGravityNorm ground truth gravity norm.
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 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
718 * if provided gravity norm value is negative.
719 */
720 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
721 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
722 final boolean commonAxisUsed, final Matrix initialBias) {
723 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
724 }
725
726 /**
727 * Constructor.
728 *
729 * @param groundTruthGravityNorm ground truth gravity norm.
730 * @param measurements collection of body kinematics measurements with standard
731 * deviations taken at the same position with zero velocity
732 * and unknown different orientations.
733 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
734 * accelerometer and gyroscope.
735 * @param initialBias initial bias to find a solution.
736 * @param listener listener to handle events raised by this calibrator.
737 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
738 * if provided gravity norm value is negative.
739 */
740 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
741 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
742 final boolean commonAxisUsed, final Matrix initialBias,
743 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
744 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
745 }
746
747 /**
748 * Constructor.
749 *
750 * @param groundTruthGravityNorm ground truth gravity norm.
751 * @param measurements collection of body kinematics measurements with standard
752 * deviations taken at the same position with zero velocity
753 * and unknown different orientations.
754 * @param initialBias initial bias to find a solution.
755 * @param initialMa initial scale factors and cross coupling errors matrix.
756 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
757 * scaling and coupling error matrix is not 3x3 or
758 * if provided gravity norm value is negative.
759 */
760 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
761 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
762 final Matrix initialBias, final Matrix initialMa) {
763 super(groundTruthGravityNorm, measurements, initialBias, initialMa);
764 }
765
766 /**
767 * Constructor.
768 *
769 * @param groundTruthGravityNorm ground truth gravity norm.
770 * @param measurements collection of body kinematics measurements with standard
771 * deviations taken at the same position with zero velocity
772 * and unknown different orientations.
773 * @param initialBias initial bias to find a solution.
774 * @param initialMa initial scale factors and cross coupling errors matrix.
775 * @param listener listener to handle events raised by this calibrator.
776 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
777 * scaling and coupling error matrix is not 3x3 or
778 * if provided gravity norm value is negative.
779 */
780 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
781 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
782 final Matrix initialBias, final Matrix initialMa,
783 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
784 super(groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
785 }
786
787 /**
788 * Constructor.
789 *
790 * @param groundTruthGravityNorm ground truth gravity norm.
791 * @param measurements collection of body kinematics measurements with standard
792 * deviations taken at the same position with zero velocity
793 * and unknown different orientations.
794 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
795 * accelerometer and gyroscope.
796 * @param initialBias initial bias to find a solution.
797 * @param initialMa initial scale factors and cross coupling errors matrix.
798 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
799 * scaling and coupling error matrix is not 3x3 or
800 * if provided gravity norm value is negative.
801 */
802 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
803 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
804 final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa) {
805 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
806 }
807
808 /**
809 * Constructor.
810 *
811 * @param groundTruthGravityNorm ground truth gravity norm.
812 * @param measurements collection of body kinematics measurements with standard
813 * deviations taken at the same position with zero velocity
814 * and unknown different orientations.
815 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
816 * accelerometer and gyroscope.
817 * @param initialBias initial bias to find a solution.
818 * @param initialMa initial scale factors and cross coupling errors matrix.
819 * @param listener listener to handle events raised by this calibrator.
820 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
821 * scaling and coupling error matrix is not 3x3 or
822 * if provided gravity norm value is negative.
823 */
824 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
825 final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
826 final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa,
827 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
828 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
829 }
830
831 /**
832 * Constructor.
833 *
834 * @param qualityScores quality scores corresponding to each provided
835 * measurement. The larger the score value the better
836 * the quality of the sample.
837 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
838 * squared second (m/s^2).
839 * @param measurements list of body kinematics measurements taken at a given position with
840 * different unknown orientations and containing the standard deviations
841 * of accelerometer and gyroscope measurements.
842 * @throws IllegalArgumentException if provided gravity norm value is negative or
843 * if provided quality scores length is
844 * smaller than 13 samples.
845 */
846 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
847 final double[] qualityScores, final Double groundTruthGravityNorm,
848 final List<StandardDeviationBodyKinematics> measurements) {
849 super(groundTruthGravityNorm, measurements);
850 internalSetQualityScores(qualityScores);
851 }
852
853 /**
854 * Constructor.
855 *
856 * @param qualityScores quality scores corresponding to each provided
857 * measurement. The larger the score value the better
858 * the quality of the sample.
859 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
860 * squared second (m/s^2).
861 * @param measurements list of body kinematics measurements taken at a given position with
862 * different unknown orientations and containing the standard deviations
863 * of accelerometer and gyroscope measurements.
864 * @param listener listener to be notified of events such as when estimation
865 * starts, ends or its progress significantly changes.
866 * @throws IllegalArgumentException if provided gravity norm value is negative or
867 * if provided quality scores length is
868 * smaller than 13 samples.
869 */
870 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
871 final double[] qualityScores, final Double groundTruthGravityNorm,
872 final List<StandardDeviationBodyKinematics> measurements,
873 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
874 super(groundTruthGravityNorm, measurements, listener);
875 internalSetQualityScores(qualityScores);
876 }
877
878 /**
879 * Constructor.
880 *
881 * @param qualityScores quality scores corresponding to each provided
882 * measurement. The larger the score value the better
883 * the quality of the sample.
884 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
885 * squared second (m/s^2).
886 * @param measurements list of body kinematics measurements taken at a given position with
887 * different unknown orientations and containing the standard deviations
888 * of accelerometer and gyroscope measurements.
889 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
890 * accelerometer and gyroscope.
891 * @throws IllegalArgumentException if provided gravity norm value is negative or
892 * if provided quality scores length is
893 * smaller than 10 or 13 samples.
894 */
895 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
896 final double[] qualityScores, final Double groundTruthGravityNorm,
897 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed) {
898 super(groundTruthGravityNorm, measurements, commonAxisUsed);
899 internalSetQualityScores(qualityScores);
900 }
901
902 /**
903 * Constructor.
904 *
905 * @param qualityScores quality scores corresponding to each provided
906 * measurement. The larger the score value the better
907 * the quality of the sample.
908 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
909 * squared second (m/s^2).
910 * @param measurements list of body kinematics measurements taken at a given position with
911 * different unknown orientations and containing the standard deviations
912 * of accelerometer and gyroscope measurements.
913 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
914 * accelerometer and gyroscope.
915 * @param listener listener to be notified of events such as when estimation
916 * starts, ends or its progress significantly changes.
917 * @throws IllegalArgumentException if provided gravity norm value is negative or
918 * if provided quality scores length is
919 * smaller than 10 or 13 samples.
920 */
921 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
922 final double[] qualityScores, final Double groundTruthGravityNorm,
923 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
924 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
925 super(groundTruthGravityNorm, measurements, commonAxisUsed, listener);
926 internalSetQualityScores(qualityScores);
927 }
928
929 /**
930 * Constructor.
931 *
932 * @param qualityScores quality scores corresponding to each provided
933 * measurement. The larger the score value the better
934 * the quality of the sample.
935 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
936 * squared second (m/s^2).
937 * @param measurements collection of body kinematics measurements with standard
938 * deviations taken at the same position with zero velocity
939 * and unknown different orientations.
940 * @param initialBias initial accelerometer bias to be used to find a solution.
941 * This must have length 3 and is expressed in meters per
942 * squared second (m/s^2).
943 * @throws IllegalArgumentException if provided bias array does not have length 3 or
944 * if provided gravity norm value is negative or
945 * if provided quality scores length is
946 * smaller than 13 samples.
947 */
948 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
949 final double[] qualityScores, final Double groundTruthGravityNorm,
950 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias) {
951 super(groundTruthGravityNorm, measurements, initialBias);
952 internalSetQualityScores(qualityScores);
953 }
954
955 /**
956 * Constructor.
957 *
958 * @param qualityScores quality scores corresponding to each provided
959 * measurement. The larger the score value the better
960 * the quality of the sample.
961 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
962 * squared second (m/s^2).
963 * @param measurements collection of body kinematics measurements with standard
964 * deviations taken at the same position with zero velocity
965 * and unknown different orientations.
966 * @param initialBias initial accelerometer bias to be used to find a solution.
967 * This must have length 3 and is expressed in meters per
968 * squared second (m/s^2).
969 * @param listener listener to handle events raised by this calibrator.
970 * @throws IllegalArgumentException if provided bias array does not have length 3 or
971 * if provided gravity norm value is negative or
972 * if provided quality scores length is
973 * smaller than 13 samples.
974 */
975 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
976 final double[] qualityScores, final Double groundTruthGravityNorm,
977 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
978 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
979 super(groundTruthGravityNorm, measurements, initialBias, listener);
980 internalSetQualityScores(qualityScores);
981 }
982
983 /**
984 * Constructor.
985 *
986 * @param qualityScores quality scores corresponding to each provided
987 * measurement. The larger the score value the better
988 * the quality of the sample.
989 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
990 * squared second (m/s^2).
991 * @param measurements collection of body kinematics measurements with standard
992 * deviations taken at the same position with zero velocity
993 * and unknown different orientations.
994 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
995 * accelerometer and gyroscope.
996 * @param initialBias initial accelerometer bias to be used to find a solution.
997 * This must have length 3 and is expressed in meters per
998 * squared second (m/s^2).
999 * @throws IllegalArgumentException if provided bias array does not have length 3 or
1000 * if provided gravity norm value is negative or
1001 * if provided quality scores length is
1002 * smaller than 10 or 13 samples.
1003 */
1004 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1005 final double[] qualityScores, final Double groundTruthGravityNorm,
1006 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1007 final double[] initialBias) {
1008 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
1009 internalSetQualityScores(qualityScores);
1010 }
1011
1012 /**
1013 * Constructor.
1014 *
1015 * @param qualityScores quality scores corresponding to each provided
1016 * measurement. The larger the score value the better
1017 * the quality of the sample.
1018 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1019 * squared second (m/s^2).
1020 * @param measurements collection of body kinematics measurements with standard
1021 * deviations taken at the same position with zero velocity
1022 * and unknown different orientations.
1023 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1024 * accelerometer and gyroscope.
1025 * @param initialBias initial accelerometer bias to be used to find a solution.
1026 * This must have length 3 and is expressed in meters per
1027 * squared second (m/s^2).
1028 * @param listener listener to handle events raised by this calibrator.
1029 * @throws IllegalArgumentException if provided bias array does not have length 3 or
1030 * if provided gravity norm value is negative or
1031 * if provided quality scores length is
1032 * smaller than 10 or 13 samples.
1033 */
1034 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1035 final double[] qualityScores, final Double groundTruthGravityNorm,
1036 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1037 final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
1038 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
1039 internalSetQualityScores(qualityScores);
1040 }
1041
1042 /**
1043 * Constructor.
1044 *
1045 * @param qualityScores quality scores corresponding to each provided
1046 * measurement. The larger the score value the better
1047 * the quality of the sample.
1048 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1049 * squared second (m/s^2).
1050 * @param measurements collection of body kinematics measurements with standard
1051 * deviations taken at the same position with zero velocity
1052 * and unknown different orientations.
1053 * @param initialBias initial bias to find a solution.
1054 * @throws IllegalArgumentException if provided bias array does not have length 3 or
1055 * if provided gravity norm value is negative or
1056 * if provided quality scores length is
1057 * smaller than 13 samples.
1058 */
1059 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1060 final double[] qualityScores, final Double groundTruthGravityNorm,
1061 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias) {
1062 super(groundTruthGravityNorm, measurements, initialBias);
1063 internalSetQualityScores(qualityScores);
1064 }
1065
1066 /**
1067 * Constructor.
1068 *
1069 * @param qualityScores quality scores corresponding to each provided
1070 * measurement. The larger the score value the better
1071 * the quality of the sample.
1072 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1073 * squared second (m/s^2).
1074 * @param measurements collection of body kinematics measurements with standard
1075 * deviations taken at the same position with zero velocity
1076 * and unknown different orientations.
1077 * @param initialBias initial bias to find a solution.
1078 * @param listener listener to handle events raised by this calibrator.
1079 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
1080 * if provided gravity norm value is negative or
1081 * if provided quality scores length is
1082 * smaller than 13 samples.
1083 */
1084 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1085 final double[] qualityScores, final Double groundTruthGravityNorm,
1086 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
1087 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
1088 super(groundTruthGravityNorm, measurements, initialBias, listener);
1089 internalSetQualityScores(qualityScores);
1090 }
1091
1092 /**
1093 * Constructor.
1094 *
1095 * @param qualityScores quality scores corresponding to each provided
1096 * measurement. The larger the score value the better
1097 * the quality of the sample.
1098 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1099 * squared second (m/s^2).
1100 * @param measurements collection of body kinematics measurements with standard
1101 * deviations taken at the same position with zero velocity
1102 * and unknown different orientations.
1103 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1104 * accelerometer and gyroscope.
1105 * @param initialBias initial bias to find a solution.
1106 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
1107 * if provided gravity norm value is negative or
1108 * if provided quality scores length is
1109 * smaller than 10 or 13 samples.
1110 */
1111 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1112 final double[] qualityScores, final Double groundTruthGravityNorm,
1113 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1114 final Matrix initialBias) {
1115 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
1116 internalSetQualityScores(qualityScores);
1117 }
1118
1119 /**
1120 * Constructor.
1121 *
1122 * @param qualityScores quality scores corresponding to each provided
1123 * measurement. The larger the score value the better
1124 * the quality of the sample.
1125 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1126 * squared second (m/s^2).
1127 * @param measurements collection of body kinematics measurements with standard
1128 * deviations taken at the same position with zero velocity
1129 * and unknown different orientations.
1130 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1131 * accelerometer and gyroscope.
1132 * @param initialBias initial bias to find a solution.
1133 * @param listener listener to handle events raised by this calibrator.
1134 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
1135 * if provided gravity norm value is negative or
1136 * if provided quality scores length is
1137 * smaller than 10 or 13 samples.
1138 */
1139 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1140 final double[] qualityScores, final Double groundTruthGravityNorm,
1141 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1142 final Matrix initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
1143 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
1144 internalSetQualityScores(qualityScores);
1145 }
1146
1147 /**
1148 * Constructor.
1149 *
1150 * @param qualityScores quality scores corresponding to each provided
1151 * measurement. The larger the score value the better
1152 * the quality of the sample.
1153 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1154 * squared second (m/s^2).
1155 * @param measurements collection of body kinematics measurements with standard
1156 * deviations taken at the same position with zero velocity
1157 * and unknown different orientations.
1158 * @param initialBias initial bias to find a solution.
1159 * @param initialMa initial scale factors and cross coupling errors matrix.
1160 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1161 * scaling and coupling error matrix is not 3x3 or
1162 * if provided gravity norm value is negative or
1163 * if provided quality scores length is
1164 * smaller than 13 samples.
1165 */
1166 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1167 final double[] qualityScores, final Double groundTruthGravityNorm,
1168 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
1169 final Matrix initialMa) {
1170 super(groundTruthGravityNorm, measurements, initialBias, initialMa);
1171 internalSetQualityScores(qualityScores);
1172 }
1173
1174 /**
1175 * Constructor.
1176 *
1177 * @param qualityScores quality scores corresponding to each provided
1178 * measurement. The larger the score value the better
1179 * the quality of the sample.
1180 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1181 * squared second (m/s^2).
1182 * @param measurements collection of body kinematics measurements with standard
1183 * deviations taken at the same position with zero velocity
1184 * and unknown different orientations.
1185 * @param initialBias initial bias to find a solution.
1186 * @param initialMa initial scale factors and cross coupling errors matrix.
1187 * @param listener listener to handle events raised by this calibrator.
1188 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1189 * scaling and coupling error matrix is not 3x3 or
1190 * if provided gravity norm value is negative or
1191 * if provided quality scores length is
1192 * smaller than 13 samples.
1193 */
1194 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1195 final double[] qualityScores, final Double groundTruthGravityNorm,
1196 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMa,
1197 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
1198 super(groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
1199 internalSetQualityScores(qualityScores);
1200 }
1201
1202 /**
1203 * Constructor.
1204 *
1205 * @param qualityScores quality scores corresponding to each provided
1206 * measurement. The larger the score value the better
1207 * the quality of the sample.
1208 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1209 * squared second (m/s^2).
1210 * @param measurements collection of body kinematics measurements with standard
1211 * deviations taken at the same position with zero velocity
1212 * and unknown different orientations.
1213 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1214 * accelerometer and gyroscope.
1215 * @param initialBias initial bias to find a solution.
1216 * @param initialMa initial scale factors and cross coupling errors matrix.
1217 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1218 * scaling and coupling error matrix is not 3x3 or
1219 * if provided gravity norm value is negative or
1220 * if provided quality scores length is
1221 * smaller than 10 or 13 samples.
1222 */
1223 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1224 final double[] qualityScores, final Double groundTruthGravityNorm,
1225 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1226 final Matrix initialBias, final Matrix initialMa) {
1227 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
1228 internalSetQualityScores(qualityScores);
1229 }
1230
1231 /**
1232 * Constructor.
1233 *
1234 * @param qualityScores quality scores corresponding to each provided
1235 * measurement. The larger the score value the better
1236 * the quality of the sample.
1237 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1238 * squared second (m/s^2).
1239 * @param measurements collection of body kinematics measurements with standard
1240 * deviations taken at the same position with zero velocity
1241 * and unknown different orientations.
1242 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1243 * accelerometer and gyroscope.
1244 * @param initialBias initial bias to find a solution.
1245 * @param initialMa initial scale factors and cross coupling errors matrix.
1246 * @param listener listener to handle events raised by this calibrator.
1247 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1248 * scaling and coupling error matrix is not 3x3 or
1249 * if provided gravity norm value is negative or
1250 * if provided quality scores length is
1251 * smaller than 10 or 13 samples.
1252 */
1253 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1254 final double[] qualityScores, final Double groundTruthGravityNorm,
1255 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1256 final Matrix initialBias, final Matrix initialMa,
1257 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
1258 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
1259 internalSetQualityScores(qualityScores);
1260 }
1261
1262 /**
1263 * Constructor.
1264 *
1265 * @param qualityScores quality scores corresponding to each provided
1266 * measurement. The larger the score value the better
1267 * the quality of the sample.
1268 * @param groundTruthGravityNorm ground truth gravity norm.
1269 * @throws IllegalArgumentException if provided gravity norm value is negative or
1270 * if provided quality scores length is smaller
1271 * than 13 samples.
1272 */
1273 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1274 final double[] qualityScores, final Acceleration groundTruthGravityNorm) {
1275 super(groundTruthGravityNorm);
1276 internalSetQualityScores(qualityScores);
1277 }
1278
1279 /**
1280 * Constructor.
1281 *
1282 * @param qualityScores quality scores corresponding to each provided
1283 * measurement. The larger the score value the better
1284 * the quality of the sample.
1285 * @param groundTruthGravityNorm ground truth gravity norm.
1286 * @param measurements list of body kinematics measurements taken at a given position with
1287 * different unknown orientations and containing the standard deviations
1288 * of accelerometer and gyroscope measurements.
1289 * @throws IllegalArgumentException if provided gravity norm value is negative or
1290 * if provided quality scores length is smaller
1291 * than 13 samples.
1292 */
1293 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1294 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1295 final List<StandardDeviationBodyKinematics> measurements) {
1296 super(groundTruthGravityNorm, measurements);
1297 internalSetQualityScores(qualityScores);
1298 }
1299
1300 /**
1301 * Constructor.
1302 *
1303 * @param qualityScores quality scores corresponding to each provided
1304 * measurement. The larger the score value the better
1305 * the quality of the sample.
1306 * @param groundTruthGravityNorm ground truth gravity norm.
1307 * @param measurements list of body kinematics measurements taken at a given position with
1308 * different unknown orientations and containing the standard deviations
1309 * of accelerometer and gyroscope measurements.
1310 * @param listener listener to be notified of events such as when estimation
1311 * starts, ends or its progress significantly changes.
1312 * @throws IllegalArgumentException if provided gravity norm value is negative or
1313 * if provided quality scores length is smaller
1314 * than 13 samples.
1315 */
1316 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1317 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1318 final List<StandardDeviationBodyKinematics> measurements,
1319 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
1320 super(groundTruthGravityNorm, measurements, listener);
1321 internalSetQualityScores(qualityScores);
1322 }
1323
1324 /**
1325 * Constructor.
1326 *
1327 * @param qualityScores quality scores corresponding to each provided
1328 * measurement. The larger the score value the better
1329 * the quality of the sample.
1330 * @param groundTruthGravityNorm ground truth gravity norm.
1331 * @param measurements list of body kinematics measurements taken at a given position with
1332 * different unknown orientations and containing the standard deviations
1333 * of accelerometer and gyroscope measurements.
1334 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1335 * accelerometer and gyroscope.
1336 * @throws IllegalArgumentException if provided gravity norm value is negative or
1337 * if provided quality scores length is smaller
1338 * than 10 or 13 samples.
1339 */
1340 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1341 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1342 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed) {
1343 super(groundTruthGravityNorm, measurements, commonAxisUsed);
1344 internalSetQualityScores(qualityScores);
1345 }
1346
1347 /**
1348 * Constructor.
1349 *
1350 * @param qualityScores quality scores corresponding to each provided
1351 * measurement. The larger the score value the better
1352 * the quality of the sample.
1353 * @param groundTruthGravityNorm ground truth gravity norm.
1354 * @param measurements list of body kinematics measurements taken at a given position with
1355 * different unknown orientations and containing the standard deviations
1356 * of accelerometer and gyroscope measurements.
1357 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1358 * accelerometer and gyroscope.
1359 * @param listener listener to be notified of events such as when estimation
1360 * starts, ends or its progress significantly changes.
1361 * @throws IllegalArgumentException if provided gravity norm value is negative or
1362 * if provided quality scores length is smaller
1363 * than 10 or 13 samples.
1364 */
1365 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1366 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1367 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1368 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
1369 super(groundTruthGravityNorm, measurements, commonAxisUsed, listener);
1370 internalSetQualityScores(qualityScores);
1371 }
1372
1373 /**
1374 * Constructor.
1375 *
1376 * @param qualityScores quality scores corresponding to each provided
1377 * measurement. The larger the score value the better
1378 * the quality of the sample.
1379 * @param groundTruthGravityNorm ground truth gravity norm.
1380 * @param measurements collection of body kinematics measurements with standard
1381 * deviations taken at the same position with zero velocity
1382 * and unknown different orientations.
1383 * @param initialBias initial accelerometer bias to be used to find a solution.
1384 * This must have length 3 and is expressed in meters per
1385 * squared second (m/s^2).
1386 * @throws IllegalArgumentException if provided bias array does not have length 3 or
1387 * if provided gravity norm value is negative or
1388 * if provided quality scores length is smaller
1389 * than 13 samples.
1390 */
1391 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1392 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1393 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias) {
1394 super(groundTruthGravityNorm, measurements, initialBias);
1395 internalSetQualityScores(qualityScores);
1396 }
1397
1398 /**
1399 * Constructor.
1400 *
1401 * @param qualityScores quality scores corresponding to each provided
1402 * measurement. The larger the score value the better
1403 * the quality of the sample.
1404 * @param groundTruthGravityNorm ground truth gravity norm.
1405 * @param measurements collection of body kinematics measurements with standard
1406 * deviations taken at the same position with zero velocity
1407 * and unknown different orientations.
1408 * @param initialBias initial accelerometer bias to be used to find a solution.
1409 * This must have length 3 and is expressed in meters per
1410 * squared second (m/s^2).
1411 * @param listener listener to handle events raised by this calibrator.
1412 * @throws IllegalArgumentException if provided bias array does not have length 3 or
1413 * if provided gravity norm value is negative or
1414 * if provided quality scores length is smaller
1415 * than 13 samples.
1416 */
1417 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1418 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1419 final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
1420 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
1421 super(groundTruthGravityNorm, measurements, initialBias, listener);
1422 internalSetQualityScores(qualityScores);
1423 }
1424
1425 /**
1426 * Constructor.
1427 *
1428 * @param qualityScores quality scores corresponding to each provided
1429 * measurement. The larger the score value the better
1430 * the quality of the sample.
1431 * @param groundTruthGravityNorm ground truth gravity norm.
1432 * @param measurements collection of body kinematics measurements with standard
1433 * deviations taken at the same position with zero velocity
1434 * and unknown different orientations.
1435 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1436 * accelerometer and gyroscope.
1437 * @param initialBias initial accelerometer bias to be used to find a solution.
1438 * This must have length 3 and is expressed in meters per
1439 * squared second (m/s^2).
1440 * @throws IllegalArgumentException if provided bias array does not have length 3 or
1441 * if provided gravity norm value is negative or
1442 * if provided quality scores length is smaller
1443 * than 10 or 13 samples.
1444 */
1445 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1446 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1447 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1448 final double[] initialBias) {
1449 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
1450 internalSetQualityScores(qualityScores);
1451 }
1452
1453 /**
1454 * Constructor.
1455 *
1456 * @param qualityScores quality scores corresponding to each provided
1457 * measurement. The larger the score value the better
1458 * the quality of the sample.
1459 * @param groundTruthGravityNorm ground truth gravity norm.
1460 * @param measurements collection of body kinematics measurements with standard
1461 * deviations taken at the same position with zero velocity
1462 * and unknown different orientations.
1463 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1464 * accelerometer and gyroscope.
1465 * @param initialBias initial accelerometer bias to be used to find a solution.
1466 * This must have length 3 and is expressed in meters per
1467 * squared second (m/s^2).
1468 * @param listener listener to handle events raised by this calibrator.
1469 * @throws IllegalArgumentException if provided bias array does not have length 3 or
1470 * if provided gravity norm value is negative or
1471 * if provided quality scores length is smaller
1472 * than 10 or 13 samples.
1473 */
1474 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1475 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1476 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1477 final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
1478 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
1479 internalSetQualityScores(qualityScores);
1480 }
1481
1482 /**
1483 * Constructor.
1484 *
1485 * @param qualityScores quality scores corresponding to each provided
1486 * measurement. The larger the score value the better
1487 * the quality of the sample.
1488 * @param groundTruthGravityNorm ground truth gravity norm.
1489 * @param measurements collection of body kinematics measurements with standard
1490 * deviations taken at the same position with zero velocity
1491 * and unknown different orientations.
1492 * @param initialBias initial bias to find a solution.
1493 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
1494 * if provided gravity norm value is negative or
1495 * if provided quality scores length is smaller
1496 * than 13 samples.
1497 */
1498 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1499 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1500 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias) {
1501 super(groundTruthGravityNorm, measurements, initialBias);
1502 internalSetQualityScores(qualityScores);
1503 }
1504
1505 /**
1506 * Constructor.
1507 *
1508 * @param qualityScores quality scores corresponding to each provided
1509 * measurement. The larger the score value the better
1510 * the quality of the sample.
1511 * @param groundTruthGravityNorm ground truth gravity norm.
1512 * @param measurements collection of body kinematics measurements with standard
1513 * deviations taken at the same position with zero velocity
1514 * and unknown different orientations.
1515 * @param initialBias initial bias to find a solution.
1516 * @param listener listener to handle events raised by this calibrator.
1517 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
1518 * if provided gravity norm value is negative or
1519 * if provided quality scores length is smaller
1520 * than 13 samples.
1521 */
1522 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1523 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1524 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
1525 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
1526 super(groundTruthGravityNorm, measurements, initialBias, listener);
1527 internalSetQualityScores(qualityScores);
1528 }
1529
1530 /**
1531 * Constructor.
1532 *
1533 * @param qualityScores quality scores corresponding to each provided
1534 * measurement. The larger the score value the better
1535 * the quality of the sample.
1536 * @param groundTruthGravityNorm ground truth gravity norm.
1537 * @param measurements collection of body kinematics measurements with standard
1538 * deviations taken at the same position with zero velocity
1539 * and unknown different orientations.
1540 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1541 * accelerometer and gyroscope.
1542 * @param initialBias initial bias to find a solution.
1543 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
1544 * if provided gravity norm value is negative or
1545 * if provided quality scores length is smaller
1546 * than 10 or 13 samples.
1547 */
1548 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1549 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1550 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1551 final Matrix initialBias) {
1552 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
1553 internalSetQualityScores(qualityScores);
1554 }
1555
1556 /**
1557 * Constructor.
1558 *
1559 * @param qualityScores quality scores corresponding to each provided
1560 * measurement. The larger the score value the better
1561 * the quality of the sample.
1562 * @param groundTruthGravityNorm ground truth gravity norm.
1563 * @param measurements collection of body kinematics measurements with standard
1564 * deviations taken at the same position with zero velocity
1565 * and unknown different orientations.
1566 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1567 * accelerometer and gyroscope.
1568 * @param initialBias initial bias to find a solution.
1569 * @param listener listener to handle events raised by this calibrator.
1570 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
1571 * if provided gravity norm value is negative or
1572 * if provided quality scores length is smaller
1573 * than 10 or 13 samples.
1574 */
1575 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1576 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1577 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1578 final Matrix initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
1579 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
1580 internalSetQualityScores(qualityScores);
1581 }
1582
1583 /**
1584 * Constructor.
1585 *
1586 * @param qualityScores quality scores corresponding to each provided
1587 * measurement. The larger the score value the better
1588 * the quality of the sample.
1589 * @param groundTruthGravityNorm ground truth gravity norm.
1590 * @param measurements collection of body kinematics measurements with standard
1591 * deviations taken at the same position with zero velocity
1592 * and unknown different orientations.
1593 * @param initialBias initial bias to find a solution.
1594 * @param initialMa initial scale factors and cross coupling errors matrix.
1595 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1596 * scaling and coupling error matrix is not 3x3 or
1597 * if provided gravity norm value is negative or
1598 * if provided quality scores length is smaller
1599 * than 13 samples.
1600 */
1601 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1602 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1603 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
1604 final Matrix initialMa) {
1605 super(groundTruthGravityNorm, measurements, initialBias, initialMa);
1606 internalSetQualityScores(qualityScores);
1607 }
1608
1609 /**
1610 * Constructor.
1611 *
1612 * @param qualityScores quality scores corresponding to each provided
1613 * measurement. The larger the score value the better
1614 * the quality of the sample.
1615 * @param groundTruthGravityNorm ground truth gravity norm.
1616 * @param measurements collection of body kinematics measurements with standard
1617 * deviations taken at the same position with zero velocity
1618 * and unknown different orientations.
1619 * @param initialBias initial bias to find a solution.
1620 * @param initialMa initial scale factors and cross coupling errors matrix.
1621 * @param listener listener to handle events raised by this calibrator.
1622 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1623 * scaling and coupling error matrix is not 3x3 or
1624 * if provided gravity norm value is negative or
1625 * if provided quality scores length is smaller
1626 * than 13 samples.
1627 */
1628 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1629 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1630 final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMa,
1631 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
1632 super(groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
1633 internalSetQualityScores(qualityScores);
1634 }
1635
1636 /**
1637 * Constructor.
1638 *
1639 * @param qualityScores quality scores corresponding to each provided
1640 * measurement. The larger the score value the better
1641 * the quality of the sample.
1642 * @param groundTruthGravityNorm ground truth gravity norm.
1643 * @param measurements collection of body kinematics measurements with standard
1644 * deviations taken at the same position with zero velocity
1645 * and unknown different orientations.
1646 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1647 * accelerometer and gyroscope.
1648 * @param initialBias initial bias to find a solution.
1649 * @param initialMa initial scale factors and cross coupling errors matrix.
1650 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1651 * scaling and coupling error matrix is not 3x3 or
1652 * if provided gravity norm value is negative or
1653 * if provided quality scores length is smaller
1654 * than 10 or 13 samples.
1655 */
1656 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1657 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1658 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1659 final Matrix initialBias, final Matrix initialMa) {
1660 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
1661 internalSetQualityScores(qualityScores);
1662 }
1663
1664 /**
1665 * Constructor.
1666 *
1667 * @param qualityScores quality scores corresponding to each provided
1668 * measurement. The larger the score value the better
1669 * the quality of the sample.
1670 * @param groundTruthGravityNorm ground truth gravity norm.
1671 * @param measurements collection of body kinematics measurements with standard
1672 * deviations taken at the same position with zero velocity
1673 * and unknown different orientations.
1674 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1675 * accelerometer and gyroscope.
1676 * @param initialBias initial bias to find a solution.
1677 * @param initialMa initial scale factors and cross coupling errors matrix.
1678 * @param listener listener to handle events raised by this calibrator.
1679 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1680 * scaling and coupling error matrix is not 3x3 or
1681 * if provided gravity norm value is negative or
1682 * if provided quality scores length is smaller
1683 * than 10 or 13 samples.
1684 */
1685 public PROSACRobustKnownGravityNormAccelerometerCalibrator(
1686 final double[] qualityScores, final Acceleration groundTruthGravityNorm,
1687 final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1688 final Matrix initialBias, final Matrix initialMa,
1689 final RobustKnownGravityNormAccelerometerCalibratorListener listener) {
1690 super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
1691 internalSetQualityScores(qualityScores);
1692 }
1693
1694 /**
1695 * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
1696 * The threshold refers to the amount of error on norm between measured specific forces and the
1697 * ones generated with estimated calibration parameters provided for each sample.
1698 *
1699 * @return threshold to determine whether samples are inliers or not.
1700 */
1701 public double getThreshold() {
1702 return threshold;
1703 }
1704
1705 /**
1706 * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
1707 * The threshold refers to the amount of error on norm between measured specific forces and the
1708 * ones generated with estimated calibration parameters provided for each sample.
1709 *
1710 * @param threshold threshold to determine whether samples are inliers or not.
1711 * @throws IllegalArgumentException if provided value is equal or less than zero.
1712 * @throws LockedException if calibrator is currently running.
1713 */
1714 public void setThreshold(final double threshold) throws LockedException {
1715 if (running) {
1716 throw new LockedException();
1717 }
1718 if (threshold <= MIN_THRESHOLD) {
1719 throw new IllegalArgumentException();
1720 }
1721 this.threshold = threshold;
1722 }
1723
1724 /**
1725 * Returns quality scores corresponding to each provided sample.
1726 * The larger the score value the better the quality of the sample.
1727 *
1728 * @return quality scores corresponding to each sample.
1729 */
1730 @Override
1731 public double[] getQualityScores() {
1732 return qualityScores;
1733 }
1734
1735 /**
1736 * Sets quality scores corresponding to each provided sample.
1737 * The larger the score value the better the quality of the sample.
1738 *
1739 * @param qualityScores quality scores corresponding to each sample.
1740 * @throws IllegalArgumentException if provided quality scores length
1741 * is smaller than minimum required samples
1742 * (10 or 13).
1743 * @throws LockedException if calibrator is currently running.
1744 */
1745 @Override
1746 public void setQualityScores(final double[] qualityScores) throws LockedException {
1747 if (running) {
1748 throw new LockedException();
1749 }
1750 internalSetQualityScores(qualityScores);
1751 }
1752
1753 /**
1754 * Indicates whether calibrator is ready to find a solution.
1755 *
1756 * @return true if calibrator is ready, false otherwise.
1757 */
1758 @Override
1759 public boolean isReady() {
1760 return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
1761 }
1762
1763 /**
1764 * Indicates whether inliers must be computed and kept.
1765 *
1766 * @return true if inliers must be computed and kept, false if inliers
1767 * only need to be computed but not kept.
1768 */
1769 public boolean isComputeAndKeepInliersEnabled() {
1770 return computeAndKeepInliers;
1771 }
1772
1773 /**
1774 * Specifies whether inliers must be computed and kept.
1775 *
1776 * @param computeAndKeepInliers true if inliers must be computed and kept,
1777 * false if inliers only need to be computed but not kept.
1778 * @throws LockedException if calibrator is currently running.
1779 */
1780 public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
1781 if (running) {
1782 throw new LockedException();
1783 }
1784 this.computeAndKeepInliers = computeAndKeepInliers;
1785 }
1786
1787 /**
1788 * Indicates whether residuals must be computed and kept.
1789 *
1790 * @return true if residuals must be computed and kept, false if residuals
1791 * only need to be computed but not kept.
1792 */
1793 public boolean isComputeAndKeepResiduals() {
1794 return computeAndKeepResiduals;
1795 }
1796
1797 /**
1798 * Specifies whether residuals must be computed and kept.
1799 *
1800 * @param computeAndKeepResiduals true if residuals must be computed and kept,
1801 * false if residuals only need to be computed but not kept.
1802 * @throws LockedException if calibrator is currently running.
1803 */
1804 public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
1805 if (running) {
1806 throw new LockedException();
1807 }
1808 this.computeAndKeepResiduals = computeAndKeepResiduals;
1809 }
1810
1811 /**
1812 * Estimates accelerometer calibration parameters containing bias, scale factors
1813 * and cross-coupling errors.
1814 *
1815 * @throws LockedException if calibrator is currently running.
1816 * @throws NotReadyException if calibrator is not ready.
1817 * @throws CalibrationException if estimation fails for numerical reasons.
1818 */
1819 @Override
1820 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
1821 if (running) {
1822 throw new LockedException();
1823 }
1824 if (!isReady()) {
1825 throw new NotReadyException();
1826 }
1827
1828 final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
1829 @Override
1830 public double[] getQualityScores() {
1831 return qualityScores;
1832 }
1833
1834 @Override
1835 public double getThreshold() {
1836 return threshold;
1837 }
1838
1839 @Override
1840 public int getTotalSamples() {
1841 return measurements.size();
1842 }
1843
1844 @Override
1845 public int getSubsetSize() {
1846 return preliminarySubsetSize;
1847 }
1848
1849 @Override
1850 public void estimatePreliminarSolutions(
1851 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
1852 computePreliminarySolutions(samplesIndices, solutions);
1853 }
1854
1855 @Override
1856 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
1857 return computeError(measurements.get(i), currentEstimation);
1858 }
1859
1860 @Override
1861 public boolean isReady() {
1862 return PROSACRobustKnownGravityNormAccelerometerCalibrator.this.isReady();
1863 }
1864
1865 @Override
1866 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
1867 // no action needed
1868 }
1869
1870 @Override
1871 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
1872 // no action needed
1873 }
1874
1875 @Override
1876 public void onEstimateNextIteration(
1877 final RobustEstimator<PreliminaryResult> estimator, final int iteration) {
1878 if (listener != null) {
1879 listener.onCalibrateNextIteration(
1880 PROSACRobustKnownGravityNormAccelerometerCalibrator.this, iteration);
1881 }
1882 }
1883
1884 @Override
1885 public void onEstimateProgressChange(
1886 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
1887 if (listener != null) {
1888 listener.onCalibrateProgressChange(
1889 PROSACRobustKnownGravityNormAccelerometerCalibrator.this, progress);
1890 }
1891 }
1892 });
1893
1894 try {
1895 running = true;
1896
1897 if (listener != null) {
1898 listener.onCalibrateStart(this);
1899 }
1900
1901 inliersData = null;
1902 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
1903 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
1904 innerEstimator.setConfidence(confidence);
1905 innerEstimator.setMaxIterations(maxIterations);
1906 innerEstimator.setProgressDelta(progressDelta);
1907 final var preliminaryResult = innerEstimator.estimate();
1908 inliersData = innerEstimator.getInliersData();
1909
1910 attemptRefine(preliminaryResult);
1911
1912 if (listener != null) {
1913 listener.onCalibrateEnd(this);
1914 }
1915
1916 } catch (final com.irurueta.numerical.LockedException e) {
1917 throw new LockedException(e);
1918 } catch (final com.irurueta.numerical.NotReadyException e) {
1919 throw new NotReadyException(e);
1920 } catch (final RobustEstimatorException e) {
1921 throw new CalibrationException(e);
1922 } finally {
1923 running = false;
1924 }
1925 }
1926
1927 /**
1928 * Returns method being used for robust estimation.
1929 *
1930 * @return method being used for robust estimation.
1931 */
1932 @Override
1933 public RobustEstimatorMethod getMethod() {
1934 return RobustEstimatorMethod.PROSAC;
1935 }
1936
1937 /**
1938 * Indicates whether this calibrator requires quality scores for each
1939 * measurement or not.
1940 *
1941 * @return true if quality scores are required, false otherwise.
1942 */
1943 @Override
1944 public boolean isQualityScoresRequired() {
1945 return true;
1946 }
1947
1948 /**
1949 * Sets quality scores corresponding to each provided sample.
1950 * This method is used internally and does not check whether instance is
1951 * locked or not.
1952 *
1953 * @param qualityScores quality scores to be set.
1954 * @throws IllegalArgumentException if provided quality scores length
1955 * is smaller than the minimum required
1956 * number of samples (10 or 13).
1957 */
1958 private void internalSetQualityScores(final double[] qualityScores) {
1959 if (qualityScores == null || qualityScores.length < getMinimumRequiredMeasurements()) {
1960 throw new IllegalArgumentException();
1961 }
1962
1963 this.qualityScores = qualityScores;
1964 }
1965 }