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.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.Utils;
21 import com.irurueta.algebra.WrongSizeException;
22 import com.irurueta.navigation.LockedException;
23 import com.irurueta.navigation.NotReadyException;
24 import com.irurueta.navigation.inertial.BodyKinematics;
25 import com.irurueta.navigation.inertial.calibration.AccelerationTriad;
26 import com.irurueta.navigation.inertial.calibration.CalibrationException;
27 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
28 import com.irurueta.numerical.EvaluationException;
29 import com.irurueta.numerical.GradientEstimator;
30 import com.irurueta.numerical.fitting.FittingException;
31 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFitter;
32 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFunctionEvaluator;
33 import com.irurueta.statistics.MaxIterationsExceededException;
34 import com.irurueta.units.Acceleration;
35 import com.irurueta.units.AccelerationConverter;
36 import com.irurueta.units.AccelerationUnit;
37
38 import java.util.Collection;
39
40 /**
41 * Abstract class to estimate accelerometer cross couplings and scaling factors when
42 * accelerometer biases and gravity norm are known.
43 * Gravity norm can be known because it has been directly provided or because position
44 * respect Earth is known, and thus gravity norm can be computed.
45 * This calibrator uses Levenberg-Marquardt to find a minimum least squared error
46 * solution.
47 * <p>
48 * To use this calibrator at least 7 measurements taken at a single known position must
49 * be taken at 7 different unknown orientations and zero velocity when common z-axis
50 * is assumed, otherwise at least 10 measurements are required.
51 * <p>
52 * Measured specific force is assumed to follow the model shown below:
53 * <pre>
54 * fmeas = ba + (I + Ma) * ftrue + w
55 * </pre>
56 * Where:
57 * - fmeas is the measured specific force. This is a 3x1 vector.
58 * - ba is accelerometer bias, which is known. This is a 3x1 vector.
59 * - I is the 3x3 identity matrix.
60 * - Ma is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
61 * a perfect accelerometer, this should be a 3x3 zero matrix.
62 * - ftrue is ground-truth specific force.
63 * - w is measurement noise.
64 *
65 * @param <C> a calibrator type.
66 * @param <L> a listener type.
67 */
68 public abstract class BaseBiasGravityNormAccelerometerCalibrator<
69 C extends BaseBiasGravityNormAccelerometerCalibrator<?, ?>,
70 L extends BaseBiasGravityNormAccelerometerCalibratorListener<C>> implements AccelerometerNonLinearCalibrator,
71 KnownBiasAccelerometerCalibrator, UnorderedStandardDeviationBodyKinematicsAccelerometerCalibrator {
72
73 /**
74 * Indicates whether by default a common z-axis is assumed for both the accelerometer
75 * and gyroscope.
76 */
77 public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;
78
79 /**
80 * Number of unknowns when common z-axis is assumed for both the accelerometer
81 * and gyroscope.
82 */
83 public static final int COMMON_Z_AXIS_UNKNOWNS = 6;
84
85 /**
86 * Number of unknowns for the general case.
87 */
88 public static final int GENERAL_UNKNOWNS = 9;
89
90 /**
91 * Required minimum number of measurements when common z-axis is assumed.
92 */
93 public static final int MINIMUM_MEASUREMENTS_COMMON_Z_AXIS = COMMON_Z_AXIS_UNKNOWNS + 1;
94
95 /**
96 * Required minimum number of measurements for the general case.
97 */
98 public static final int MINIMUM_MEASUREMENTS_GENERAL = GENERAL_UNKNOWNS + 1;
99
100 /**
101 * Ground truth gravity norm to be expected at location where measurements have been made,
102 * expressed in meters per squared second (m/s^2).
103 */
104 protected Double groundTruthGravityNorm;
105
106 /**
107 * Levenberg-Marquardt fitter to find a non-linear solution.
108 */
109 private final LevenbergMarquardtMultiDimensionFitter fitter = new LevenbergMarquardtMultiDimensionFitter();
110
111 /**
112 * X-coordinate of known accelerometer bias.
113 * This is expressed in meters per squared second (m/s^2).
114 */
115 private double biasX;
116
117 /**
118 * Y-coordinate of known accelerometer bias.
119 * This is expressed in meters per squared second (m/s^2).
120 */
121 private double biasY;
122
123 /**
124 * Z-coordinate of known accelerometer bias.
125 * This is expressed in meters per squared second (m/s^2).
126 */
127 private double biasZ;
128
129 /**
130 * Initial x scaling factor.
131 */
132 private double initialSx;
133
134 /**
135 * Initial y scaling factor.
136 */
137 private double initialSy;
138
139 /**
140 * Initial z scaling factor.
141 */
142 private double initialSz;
143
144 /**
145 * Initial x-y cross coupling error.
146 */
147 private double initialMxy;
148
149 /**
150 * Initial x-z cross coupling error.
151 */
152 private double initialMxz;
153
154 /**
155 * Initial y-x cross coupling error.
156 */
157 private double initialMyx;
158
159 /**
160 * Initial y-z cross coupling error.
161 */
162 private double initialMyz;
163
164 /**
165 * Initial z-x cross coupling error.
166 */
167 private double initialMzx;
168
169 /**
170 * Initial z-y cross coupling error.
171 */
172 private double initialMzy;
173
174 /**
175 * Contains a collection of body kinematics measurements taken at
176 * a given position with different unknown orientations and containing
177 * the standard deviations of accelerometer and gyroscope measurements.
178 */
179 private Collection<StandardDeviationBodyKinematics> measurements;
180
181 /**
182 * This flag indicates whether z-axis is assumed to be common for accelerometer
183 * and gyroscope.
184 * When enabled, this eliminates 3 variables from Ma matrix.
185 */
186 private boolean commonAxisUsed = DEFAULT_USE_COMMON_Z_AXIS;
187
188 /**
189 * Listener to handle events raised by this calibrator.
190 */
191 private L listener;
192
193 /**
194 * Estimated accelerometer scale factors and cross coupling errors.
195 * This is the product of matrix Ta containing cross coupling errors and Ka
196 * containing scaling factors.
197 * So tat:
198 * <pre>
199 * Ma = [sx mxy mxz] = Ta*Ka
200 * [myx sy myz]
201 * [mzx mzy sz ]
202 * </pre>
203 * Where:
204 * <pre>
205 * Ka = [sx 0 0 ]
206 * [0 sy 0 ]
207 * [0 0 sz]
208 * </pre>
209 * and
210 * <pre>
211 * Ta = [1 -alphaXy alphaXz ]
212 * [alphaYx 1 -alphaYz]
213 * [-alphaZx alphaZy 1 ]
214 * </pre>
215 * Hence:
216 * <pre>
217 * Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
218 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
219 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
220 * </pre>
221 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
222 * are considered to be zero if the accelerometer z-axis is assumed to be the same
223 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
224 * becomes upper diagonal:
225 * <pre>
226 * Ma = [sx mxy mxz]
227 * [0 sy myz]
228 * [0 0 sz ]
229 * </pre>
230 * Values of this matrix are unit-less.
231 */
232 private Matrix estimatedMa;
233
234 /**
235 * Estimated covariance matrix for estimated position.
236 */
237 private Matrix estimatedCovariance;
238
239 /**
240 * Estimated chi square value.
241 */
242 private double estimatedChiSq;
243
244 /**
245 * Estimated degrees of freedom of chi square value. Degrees of freedom is equal to the number of sampled data
246 * minus the number of estimated parameters.
247 */
248 private int estimatedChiSqDegreesOfFreedom;
249
250 /**
251 * Estimated reduced chi square value. This is equal to estimated chi square value divided by its degrees of
252 * freedom. Ideally this value should be close to 1.0.
253 */
254 private double estimatedReducedChiSq;
255
256 /**
257 * Estimated mean square error respect to provided measurements.
258 */
259 private double estimatedMse;
260
261 /**
262 * Estimated probability of finding a smaller chi square value expressed as a value between 0.0 and 1.0. The smaller
263 * the found chi square value is, the better the fit of the estimated parameters to the actual parameter. Thus, the
264 * smaller the chance of finding a smaller chi square value, then the better the estimated fit is.
265 */
266 private double estimatedP;
267
268 /**
269 * Estimated measure of quality of estimated fit as a value between 0.0 and 1.0. The larger the quality value is,
270 * the better the fit that has been estimated.
271 */
272 private double estimatedQ;
273
274 /**
275 * Indicates whether estimator is running.
276 */
277 private boolean running;
278
279 /**
280 * Internally holds x-coordinate of measured specific force during calibration.
281 */
282 private double fmeasX;
283
284 /**
285 * Internally holds y-coordinate of measured specific force during calibration.
286 */
287 private double fmeasY;
288
289 /**
290 * Internally holds z-coordinate of measured specific force during calibration.
291 */
292 private double fmeasZ;
293
294 /**
295 * Internally holds measured specific force during calibration expressed as
296 * a column matrix.
297 */
298 private Matrix fmeas;
299
300 /**
301 * Internally holds cross-coupling errors during calibration.
302 */
303 private Matrix m;
304
305 /**
306 * Internally holds inverse of cross-coupling errors during calibration.
307 */
308 private Matrix invM;
309
310 /**
311 * Internally holds biases during calibration in internal format.
312 */
313 private Matrix b;
314
315 /**
316 * Internally holds computed true specific force during calibration.
317 */
318 private Matrix ftrue;
319
320 /**
321 * Internally hold biases during calibration in external format.
322 */
323 private Matrix ba;
324
325 /**
326 * Constructor.
327 */
328 protected BaseBiasGravityNormAccelerometerCalibrator() {
329 }
330
331 /**
332 * Constructor.
333 *
334 * @param listener listener to handle events raised by this calibrator.
335 */
336 protected BaseBiasGravityNormAccelerometerCalibrator(final L listener) {
337 this.listener = listener;
338 }
339
340 /**
341 * Constructor.
342 *
343 * @param measurements collection of body kinematics measurements with standard
344 * deviations taken at the same position with zero velocity
345 * and unknown different orientations.
346 */
347 protected BaseBiasGravityNormAccelerometerCalibrator(
348 final Collection<StandardDeviationBodyKinematics> measurements) {
349 this.measurements = measurements;
350 }
351
352 /**
353 * Constructor.
354 *
355 * @param measurements collection of body kinematics measurements with standard
356 * deviations taken at the same position with zero velocity
357 * and unknown different orientations.
358 * @param listener listener to handle events raised by this calibrator.
359 */
360 protected BaseBiasGravityNormAccelerometerCalibrator(
361 final Collection<StandardDeviationBodyKinematics> measurements, final L listener) {
362 this(measurements);
363 this.listener = listener;
364 }
365
366 /**
367 * Constructor.
368 *
369 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
370 * accelerometer and gyroscope.
371 */
372 protected BaseBiasGravityNormAccelerometerCalibrator(final boolean commonAxisUsed) {
373 this.commonAxisUsed = commonAxisUsed;
374 }
375
376 /**
377 * Constructor.
378 *
379 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
380 * accelerometer and gyroscope.
381 * @param listener listener to handle events raised by this calibrator.
382 */
383 protected BaseBiasGravityNormAccelerometerCalibrator(final boolean commonAxisUsed, final L listener) {
384 this(commonAxisUsed);
385 this.listener = listener;
386 }
387
388 /**
389 * Constructor.
390 *
391 * @param measurements collection of body kinematics measurements with standard
392 * deviations taken at the same position with zero velocity
393 * and unknown different orientations.
394 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
395 * accelerometer and gyroscope.
396 */
397 protected BaseBiasGravityNormAccelerometerCalibrator(
398 final Collection<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed) {
399 this(measurements);
400 this.commonAxisUsed = commonAxisUsed;
401 }
402
403 /**
404 * Constructor.
405 *
406 * @param measurements collection of body kinematics measurements with standard
407 * deviations taken at the same position with zero velocity
408 * and unknown different orientations.
409 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
410 * accelerometer and gyroscope.
411 * @param listener listener to handle events raised by this calibrator.
412 */
413 protected BaseBiasGravityNormAccelerometerCalibrator(
414 final Collection<StandardDeviationBodyKinematics> measurements,
415 final boolean commonAxisUsed,
416 final L listener) {
417 this(measurements, commonAxisUsed);
418 this.listener = listener;
419 }
420
421 /**
422 * Constructor.
423 *
424 * @param biasX x-coordinate of accelerometer bias.
425 * This is expressed in meters per squared second (m/s^2).
426 * @param biasY y-coordinate of accelerometer bias.
427 * This is expressed in meters per squared second (m/s^2).
428 * @param biasZ z-coordinate of accelerometer bias.
429 * This is expressed in meters per squared second (m/s^2).
430 */
431 protected BaseBiasGravityNormAccelerometerCalibrator(final double biasX, final double biasY, final double biasZ) {
432 try {
433 setBiasCoordinates(biasX, biasY, biasZ);
434 } catch (final LockedException ignore) {
435 // never happens
436 }
437 }
438
439 /**
440 * Constructor.
441 *
442 * @param biasX x-coordinate of accelerometer bias.
443 * This is expressed in meters per squared second (m/s^2).
444 * @param biasY y-coordinate of accelerometer bias.
445 * This is expressed in meters per squared second (m/s^2).
446 * @param biasZ z-coordinate of accelerometer bias.
447 * This is expressed in meters per squared second (m/s^2).
448 * @param listener listener to handle events raised by this calibrator.
449 */
450 protected BaseBiasGravityNormAccelerometerCalibrator(final double biasX, final double biasY, final double biasZ,
451 final L listener) {
452 this(biasX, biasY, biasZ);
453 this.listener = listener;
454 }
455
456 /**
457 * Constructor.
458 *
459 * @param measurements collection of body kinematics measurements with standard
460 * deviations taken at the same position with zero velocity
461 * and unknown different orientations.
462 * @param biasX x-coordinate of accelerometer bias.
463 * This is expressed in meters per squared second (m/s^2).
464 * @param biasY y-coordinate of accelerometer bias.
465 * This is expressed in meters per squared second (m/s^2).
466 * @param biasZ z-coordinate of accelerometer bias.
467 * This is expressed in meters per squared second (m/s^2).
468 */
469 protected BaseBiasGravityNormAccelerometerCalibrator(
470 final Collection<StandardDeviationBodyKinematics> measurements,
471 final double biasX, final double biasY, final double biasZ) {
472 this(biasX, biasY, biasZ);
473 this.measurements = measurements;
474 }
475
476 /**
477 * Constructor.
478 *
479 * @param measurements collection of body kinematics measurements with standard
480 * deviations taken at the same position with zero velocity
481 * and unknown different orientations.
482 * @param biasX x-coordinate of accelerometer bias.
483 * This is expressed in meters per squared second (m/s^2).
484 * @param biasY y-coordinate of accelerometer bias.
485 * This is expressed in meters per squared second (m/s^2).
486 * @param biasZ z-coordinate of accelerometer bias.
487 * This is expressed in meters per squared second (m/s^2).
488 * @param listener listener to handle events raised by this calibrator.
489 */
490 protected BaseBiasGravityNormAccelerometerCalibrator(
491 final Collection<StandardDeviationBodyKinematics> measurements,
492 final double biasX, final double biasY, final double biasZ, final L listener) {
493 this(measurements, biasX, biasY, biasZ);
494 this.listener = listener;
495 }
496
497 /**
498 * Constructor.
499 *
500 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
501 * accelerometer and gyroscope.
502 * @param biasX x-coordinate of accelerometer bias.
503 * This is expressed in meters per squared second (m/s^2).
504 * @param biasY y-coordinate of accelerometer bias.
505 * This is expressed in meters per squared second (m/s^2).
506 * @param biasZ z-coordinate of accelerometer bias.
507 * This is expressed in meters per squared second (m/s^2).
508 */
509 protected BaseBiasGravityNormAccelerometerCalibrator(
510 final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ) {
511 this(biasX, biasY, biasZ);
512 this.commonAxisUsed = commonAxisUsed;
513 }
514
515 /**
516 * Constructor.
517 *
518 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
519 * accelerometer and gyroscope.
520 * @param biasX x-coordinate of accelerometer bias.
521 * This is expressed in meters per squared second (m/s^2).
522 * @param biasY y-coordinate of accelerometer bias.
523 * This is expressed in meters per squared second (m/s^2).
524 * @param biasZ z-coordinate of accelerometer bias.
525 * This is expressed in meters per squared second (m/s^2).
526 * @param listener listener to handle events raised by this calibrator.
527 */
528 protected BaseBiasGravityNormAccelerometerCalibrator(final boolean commonAxisUsed,
529 final double biasX, final double biasY, final double biasZ,
530 final L listener) {
531 this(commonAxisUsed, biasX, biasY, biasZ);
532 this.listener = listener;
533 }
534
535 /**
536 * Constructor.
537 *
538 * @param measurements collection of body kinematics measurements with standard
539 * deviations taken at the same position with zero velocity
540 * and unknown different orientations.
541 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
542 * accelerometer and gyroscope.
543 * @param biasX x-coordinate of accelerometer bias.
544 * This is expressed in meters per squared second (m/s^2).
545 * @param biasY y-coordinate of accelerometer bias.
546 * This is expressed in meters per squared second (m/s^2).
547 * @param biasZ z-coordinate of accelerometer bias.
548 * This is expressed in meters per squared second (m/s^2).
549 */
550 protected BaseBiasGravityNormAccelerometerCalibrator(
551 final Collection<StandardDeviationBodyKinematics> measurements,
552 final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ) {
553 this(commonAxisUsed, biasX, biasY, biasZ);
554 this.measurements = measurements;
555 }
556
557 /**
558 * Constructor.
559 *
560 * @param measurements collection of body kinematics measurements with standard
561 * deviations taken at the same position with zero velocity
562 * and unknown different orientations.
563 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
564 * accelerometer and gyroscope.
565 * @param biasX x-coordinate of accelerometer bias.
566 * This is expressed in meters per squared second (m/s^2).
567 * @param biasY y-coordinate of accelerometer bias.
568 * This is expressed in meters per squared second (m/s^2).
569 * @param biasZ z-coordinate of accelerometer bias.
570 * This is expressed in meters per squared second (m/s^2).
571 * @param listener listener to handle events raised by this calibrator.
572 */
573 protected BaseBiasGravityNormAccelerometerCalibrator(
574 final Collection<StandardDeviationBodyKinematics> measurements,
575 final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
576 final L listener) {
577 this(measurements, commonAxisUsed, biasX, biasY, biasZ);
578 this.listener = listener;
579 }
580
581 /**
582 * Constructor.
583 *
584 * @param biasX x-coordinate of accelerometer bias.
585 * @param biasY y-coordinate of accelerometer bias.
586 * @param biasZ z-coordinate of accelerometer bias.
587 */
588 protected BaseBiasGravityNormAccelerometerCalibrator(
589 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) {
590 try {
591 setBiasCoordinates(biasX, biasY, biasZ);
592 } catch (final LockedException ignore) {
593 // never happens
594 }
595 }
596
597 /**
598 * Constructor.
599 *
600 * @param biasX x-coordinate of accelerometer bias.
601 * @param biasY y-coordinate of accelerometer bias.
602 * @param biasZ z-coordinate of accelerometer bias.
603 * @param listener listener to handle events raised by this calibrator.
604 */
605 protected BaseBiasGravityNormAccelerometerCalibrator(
606 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final L listener) {
607 this(biasX, biasY, biasZ);
608 this.listener = listener;
609 }
610
611 /**
612 * Constructor.
613 *
614 * @param measurements collection of body kinematics measurements with standard
615 * deviations taken at the same position with zero velocity
616 * and unknown different orientations.
617 * @param biasX x-coordinate of accelerometer bias.
618 * @param biasY y-coordinate of accelerometer bias.
619 * @param biasZ z-coordinate of accelerometer bias.
620 */
621 protected BaseBiasGravityNormAccelerometerCalibrator(
622 final Collection<StandardDeviationBodyKinematics> measurements,
623 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) {
624 this(biasX, biasY, biasZ);
625 this.measurements = measurements;
626 }
627
628 /**
629 * Constructor.
630 *
631 * @param measurements collection of body kinematics measurements with standard
632 * deviations taken at the same position with zero velocity
633 * and unknown different orientations.
634 * @param biasX x-coordinate of accelerometer bias.
635 * @param biasY y-coordinate of accelerometer bias.
636 * @param biasZ z-coordinate of accelerometer bias.
637 * @param listener listener to handle events raised by this calibrator.
638 */
639 protected BaseBiasGravityNormAccelerometerCalibrator(
640 final Collection<StandardDeviationBodyKinematics> measurements,
641 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final L listener) {
642 this(measurements, biasX, biasY, biasZ);
643 this.listener = listener;
644 }
645
646 /**
647 * Constructor.
648 *
649 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
650 * accelerometer and gyroscope.
651 * @param biasX x-coordinate of accelerometer bias.
652 * @param biasY y-coordinate of accelerometer bias.
653 * @param biasZ z-coordinate of accelerometer bias.
654 */
655 protected BaseBiasGravityNormAccelerometerCalibrator(
656 final boolean commonAxisUsed, final Acceleration biasX,
657 final Acceleration biasY, final Acceleration biasZ) {
658 this(biasX, biasY, biasZ);
659 this.commonAxisUsed = commonAxisUsed;
660 }
661
662 /**
663 * Constructor.
664 *
665 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
666 * accelerometer and gyroscope.
667 * @param biasX x-coordinate of accelerometer bias.
668 * @param biasY y-coordinate of accelerometer bias.
669 * @param biasZ z-coordinate of accelerometer bias.
670 * @param listener listener to handle events raised by this calibrator.
671 */
672 protected BaseBiasGravityNormAccelerometerCalibrator(
673 final boolean commonAxisUsed, final Acceleration biasX,
674 final Acceleration biasY, final Acceleration biasZ, final L listener) {
675 this(commonAxisUsed, biasX, biasY, biasZ);
676 this.listener = listener;
677 }
678
679 /**
680 * Constructor.
681 *
682 * @param measurements collection of body kinematics measurements with standard
683 * deviations taken at the same position with zero velocity
684 * and unknown different orientations.
685 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
686 * accelerometer and gyroscope.
687 * @param biasX x-coordinate of accelerometer bias.
688 * @param biasY y-coordinate of accelerometer bias.
689 * @param biasZ z-coordinate of accelerometer bias.
690 */
691 protected BaseBiasGravityNormAccelerometerCalibrator(
692 final Collection<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
693 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) {
694 this(commonAxisUsed, biasX, biasY, biasZ);
695 this.measurements = measurements;
696 }
697
698 /**
699 * Constructor.
700 *
701 * @param measurements collection of body kinematics measurements with standard
702 * deviations taken at the same position with zero velocity
703 * and unknown different orientations.
704 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
705 * accelerometer and gyroscope.
706 * @param biasX x-coordinate of accelerometer bias.
707 * @param biasY y-coordinate of accelerometer bias.
708 * @param biasZ z-coordinate of accelerometer bias.
709 * @param listener listener to handle events raised by this calibrator.
710 */
711 protected BaseBiasGravityNormAccelerometerCalibrator(
712 final Collection<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
713 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final L listener) {
714 this(measurements, commonAxisUsed, biasX, biasY, biasZ);
715 this.listener = listener;
716 }
717
718 /**
719 * Constructor.
720 *
721 * @param biasX x-coordinate of accelerometer bias.
722 * This is expressed in meters per squared second (m/s^2).
723 * @param biasY y-coordinate of accelerometer bias.
724 * This is expressed in meters per squared second (m/s^2).
725 * @param biasZ z-coordinate of accelerometer bias.
726 * This is expressed in meters per squared second (m/s^2).
727 * @param initialSx initial x scaling factor.
728 * @param initialSy initial y scaling factor.
729 * @param initialSz initial z scaling factor.
730 */
731 protected BaseBiasGravityNormAccelerometerCalibrator(
732 final double biasX, final double biasY, final double biasZ,
733 final double initialSx, final double initialSy, final double initialSz) {
734 this(biasX, biasY, biasZ);
735 try {
736 setInitialScalingFactors(initialSx, initialSy, initialSz);
737 } catch (final LockedException ignore) {
738 // never happens
739 }
740 }
741
742 /**
743 * Constructor.
744 *
745 * @param measurements collection of body kinematics measurements with standard
746 * deviations taken at the same position with zero velocity
747 * and unknown different orientations.
748 * @param biasX x-coordinate of accelerometer bias.
749 * This is expressed in meters per squared second (m/s^2).
750 * @param biasY y-coordinate of accelerometer bias.
751 * This is expressed in meters per squared second (m/s^2).
752 * @param biasZ z-coordinate of accelerometer bias.
753 * This is expressed in meters per squared second (m/s^2).
754 * @param initialSx initial x scaling factor.
755 * @param initialSy initial y scaling factor.
756 * @param initialSz initial z scaling factor.
757 */
758 protected BaseBiasGravityNormAccelerometerCalibrator(
759 final Collection<StandardDeviationBodyKinematics> measurements,
760 final double biasX, final double biasY, final double biasZ,
761 final double initialSx, final double initialSy, final double initialSz) {
762 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz);
763 this.measurements = measurements;
764 }
765
766 /**
767 * Constructor.
768 *
769 * @param measurements collection of body kinematics measurements with standard
770 * deviations taken at the same position with zero velocity
771 * and unknown different orientations.
772 * @param biasX x-coordinate of accelerometer bias.
773 * This is expressed in meters per squared second (m/s^2).
774 * @param biasY y-coordinate of accelerometer bias.
775 * This is expressed in meters per squared second (m/s^2).
776 * @param biasZ z-coordinate of accelerometer bias.
777 * This is expressed in meters per squared second (m/s^2).
778 * @param initialSx initial x scaling factor.
779 * @param initialSy initial y scaling factor.
780 * @param initialSz initial z scaling factor.
781 * @param listener listener to handle events raised by this calibrator.
782 */
783 protected BaseBiasGravityNormAccelerometerCalibrator(
784 final Collection<StandardDeviationBodyKinematics> measurements,
785 final double biasX, final double biasY, final double biasZ,
786 final double initialSx, final double initialSy, final double initialSz, final L listener) {
787 this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
788 this.listener = listener;
789 }
790
791 /**
792 * Constructor.
793 *
794 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
795 * accelerometer and gyroscope.
796 * @param biasX x-coordinate of accelerometer bias.
797 * This is expressed in meters per squared second (m/s^2).
798 * @param biasY y-coordinate of accelerometer bias.
799 * This is expressed in meters per squared second (m/s^2).
800 * @param biasZ z-coordinate of accelerometer bias.
801 * This is expressed in meters per squared second (m/s^2).
802 * @param initialSx initial x scaling factor.
803 * @param initialSy initial y scaling factor.
804 * @param initialSz initial z scaling factor.
805 */
806 protected BaseBiasGravityNormAccelerometerCalibrator(
807 final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
808 final double initialSx, final double initialSy, final double initialSz) {
809 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz);
810 this.commonAxisUsed = commonAxisUsed;
811 }
812
813 /**
814 * Constructor.
815 *
816 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
817 * accelerometer and gyroscope.
818 * @param biasX x-coordinate of accelerometer bias.
819 * This is expressed in meters per squared second (m/s^2).
820 * @param biasY y-coordinate of accelerometer bias.
821 * This is expressed in meters per squared second (m/s^2).
822 * @param biasZ z-coordinate of accelerometer bias.
823 * This is expressed in meters per squared second (m/s^2).
824 * @param initialSx initial x scaling factor.
825 * @param initialSy initial y scaling factor.
826 * @param initialSz initial z scaling factor.
827 * @param listener listener to handle events raised by this calibrator.
828 */
829 protected BaseBiasGravityNormAccelerometerCalibrator(
830 final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
831 final double initialSx, final double initialSy, final double initialSz, final L listener) {
832 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
833 this.listener = listener;
834 }
835
836 /**
837 * Constructor.
838 *
839 * @param measurements collection of body kinematics measurements with standard
840 * deviations taken at the same position with zero velocity
841 * and unknown different orientations.
842 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
843 * accelerometer and gyroscope.
844 * @param biasX x-coordinate of accelerometer bias.
845 * This is expressed in meters per squared second (m/s^2).
846 * @param biasY y-coordinate of accelerometer bias.
847 * This is expressed in meters per squared second (m/s^2).
848 * @param biasZ z-coordinate of accelerometer bias.
849 * This is expressed in meters per squared second (m/s^2).
850 * @param initialSx initial x scaling factor.
851 * @param initialSy initial y scaling factor.
852 * @param initialSz initial z scaling factor.
853 */
854 protected BaseBiasGravityNormAccelerometerCalibrator(
855 final Collection<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
856 final double biasX, final double biasY, final double biasZ,
857 final double initialSx, final double initialSy, final double initialSz) {
858 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
859 this.measurements = measurements;
860 }
861
862 /**
863 * Constructor.
864 *
865 * @param measurements collection of body kinematics measurements with standard
866 * deviations taken at the same position with zero velocity
867 * and unknown different orientations.
868 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
869 * accelerometer and gyroscope.
870 * @param biasX x-coordinate of accelerometer bias.
871 * This is expressed in meters per squared second (m/s^2).
872 * @param biasY y-coordinate of accelerometer bias.
873 * This is expressed in meters per squared second (m/s^2).
874 * @param biasZ z-coordinate of accelerometer bias.
875 * This is expressed in meters per squared second (m/s^2).
876 * @param initialSx initial x scaling factor.
877 * @param initialSy initial y scaling factor.
878 * @param initialSz initial z scaling factor.
879 * @param listener listener to handle events raised by this calibrator.
880 */
881 protected BaseBiasGravityNormAccelerometerCalibrator(
882 final Collection<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
883 final double biasX, final double biasY, final double biasZ,
884 final double initialSx, final double initialSy, final double initialSz, final L listener) {
885 this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
886 this.listener = listener;
887 }
888
889 /**
890 * Constructor.
891 *
892 * @param biasX x-coordinate of accelerometer bias.
893 * @param biasY y-coordinate of accelerometer bias.
894 * @param biasZ z-coordinate of accelerometer bias.
895 * @param initialSx initial x scaling factor.
896 * @param initialSy initial y scaling factor.
897 * @param initialSz initial z scaling factor.
898 */
899 protected BaseBiasGravityNormAccelerometerCalibrator(
900 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
901 final double initialSx, final double initialSy, final double initialSz) {
902 this(biasX, biasY, biasZ);
903 try {
904 setInitialScalingFactors(initialSx, initialSy, initialSz);
905 } catch (final LockedException ignore) {
906 // never happens
907 }
908 }
909
910 /**
911 * Constructor.
912 *
913 * @param biasX x-coordinate of accelerometer bias.
914 * @param biasY y-coordinate of accelerometer bias.
915 * @param biasZ z-coordinate of accelerometer bias.
916 * @param initialSx initial x scaling factor.
917 * @param initialSy initial y scaling factor.
918 * @param initialSz initial z scaling factor.
919 * @param listener listener to handle events raised by this calibrator.
920 */
921 protected BaseBiasGravityNormAccelerometerCalibrator(
922 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
923 final double initialSx, final double initialSy, final double initialSz, final L listener) {
924 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz);
925 this.listener = listener;
926 }
927
928 /**
929 * Constructor.
930 *
931 * @param measurements collection of body kinematics measurements with standard
932 * deviations taken at the same position with zero velocity
933 * and unknown different orientations.
934 * @param biasX x-coordinate of accelerometer bias.
935 * @param biasY y-coordinate of accelerometer bias.
936 * @param biasZ z-coordinate of accelerometer bias.
937 * @param initialSx initial x scaling factor.
938 * @param initialSy initial y scaling factor.
939 * @param initialSz initial z scaling factor.
940 */
941 protected BaseBiasGravityNormAccelerometerCalibrator(
942 final Collection<StandardDeviationBodyKinematics> measurements,
943 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
944 final double initialSx, final double initialSy, final double initialSz) {
945 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz);
946 this.measurements = measurements;
947 }
948
949 /**
950 * Constructor.
951 *
952 * @param measurements collection of body kinematics measurements with standard
953 * deviations taken at the same position with zero velocity
954 * and unknown different orientations.
955 * @param biasX x-coordinate of accelerometer bias.
956 * @param biasY y-coordinate of accelerometer bias.
957 * @param biasZ z-coordinate of accelerometer bias.
958 * @param initialSx initial x scaling factor.
959 * @param initialSy initial y scaling factor.
960 * @param initialSz initial z scaling factor.
961 * @param listener listener to handle events raised by this calibrator.
962 */
963 protected BaseBiasGravityNormAccelerometerCalibrator(
964 final Collection<StandardDeviationBodyKinematics> measurements,
965 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
966 final double initialSx, final double initialSy, final double initialSz, final L listener) {
967 this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
968 this.listener = listener;
969 }
970
971 /**
972 * Constructor.
973 *
974 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
975 * accelerometer and gyroscope.
976 * @param biasX x-coordinate of accelerometer bias.
977 * @param biasY y-coordinate of accelerometer bias.
978 * @param biasZ z-coordinate of accelerometer bias.
979 * @param initialSx initial x scaling factor.
980 * @param initialSy initial y scaling factor.
981 * @param initialSz initial z scaling factor.
982 */
983 protected BaseBiasGravityNormAccelerometerCalibrator(
984 final boolean commonAxisUsed, final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
985 final double initialSx, final double initialSy, final double initialSz) {
986 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz);
987 this.commonAxisUsed = commonAxisUsed;
988 }
989
990 /**
991 * Constructor.
992 *
993 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
994 * accelerometer and gyroscope.
995 * @param biasX x-coordinate of accelerometer bias.
996 * @param biasY y-coordinate of accelerometer bias.
997 * @param biasZ z-coordinate of accelerometer bias.
998 * @param initialSx initial x scaling factor.
999 * @param initialSy initial y scaling factor.
1000 * @param initialSz initial z scaling factor.
1001 * @param listener listener to handle events raised by this calibrator.
1002 */
1003 protected BaseBiasGravityNormAccelerometerCalibrator(
1004 final boolean commonAxisUsed, final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
1005 final double initialSx, final double initialSy, final double initialSz, final L listener) {
1006 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
1007 this.listener = listener;
1008 }
1009
1010 /**
1011 * Constructor.
1012 *
1013 * @param measurements collection of body kinematics measurements with standard
1014 * deviations taken at the same position with zero velocity
1015 * and unknown different orientations.
1016 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1017 * accelerometer and gyroscope.
1018 * @param biasX x-coordinate of accelerometer bias.
1019 * @param biasY y-coordinate of accelerometer bias.
1020 * @param biasZ z-coordinate of accelerometer bias.
1021 * @param initialSx initial x scaling factor.
1022 * @param initialSy initial y scaling factor.
1023 * @param initialSz initial z scaling factor.
1024 */
1025 protected BaseBiasGravityNormAccelerometerCalibrator(
1026 final Collection<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1027 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
1028 final double initialSx, final double initialSy, final double initialSz) {
1029 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
1030 this.measurements = measurements;
1031 }
1032
1033 /**
1034 * Constructor.
1035 *
1036 * @param measurements collection of body kinematics measurements with standard
1037 * deviations taken at the same position with zero velocity
1038 * and unknown different orientations.
1039 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1040 * accelerometer and gyroscope.
1041 * @param biasX x-coordinate of accelerometer bias.
1042 * @param biasY y-coordinate of accelerometer bias.
1043 * @param biasZ z-coordinate of accelerometer bias.
1044 * @param initialSx initial x scaling factor.
1045 * @param initialSy initial y scaling factor.
1046 * @param initialSz initial z scaling factor.
1047 * @param listener listener to handle events raised by this calibrator.
1048 */
1049 protected BaseBiasGravityNormAccelerometerCalibrator(
1050 final Collection<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1051 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
1052 final double initialSx, final double initialSy, final double initialSz, final L listener) {
1053 this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
1054 this.listener = listener;
1055 }
1056
1057 /**
1058 * Constructor.
1059 *
1060 * @param biasX x-coordinate of accelerometer bias.
1061 * This is expressed in meters per squared second (m/s^2).
1062 * @param biasY y-coordinate of accelerometer bias.
1063 * This is expressed in meters per squared second (m/s^2).
1064 * @param biasZ z-coordinate of accelerometer bias.
1065 * This is expressed in meters per squared second (m/s^2).
1066 * @param initialSx initial x scaling factor.
1067 * @param initialSy initial y scaling factor.
1068 * @param initialSz initial z scaling factor.
1069 * @param initialMxy initial x-y cross coupling error.
1070 * @param initialMxz initial x-z cross coupling error.
1071 * @param initialMyx initial y-x cross coupling error.
1072 * @param initialMyz initial y-z cross coupling error.
1073 * @param initialMzx initial z-x cross coupling error.
1074 * @param initialMzy initial z-y cross coupling error.
1075 */
1076 protected BaseBiasGravityNormAccelerometerCalibrator(
1077 final double biasX, final double biasY, final double biasZ, final double initialSx, final double initialSy,
1078 final double initialSz, final double initialMxy, final double initialMxz, final double initialMyx,
1079 final double initialMyz, final double initialMzx, final double initialMzy) {
1080 this(biasX, biasY, biasZ);
1081 try {
1082 setInitialScalingFactorsAndCrossCouplingErrors(initialSx, initialSy, initialSz, initialMxy, initialMxz,
1083 initialMyx, initialMyz, initialMzx, initialMzy);
1084 } catch (final LockedException ignore) {
1085 // never happens
1086 }
1087 }
1088
1089 /**
1090 * Constructor.
1091 *
1092 * @param measurements collection of body kinematics measurements with standard
1093 * deviations taken at the same position with zero velocity
1094 * and unknown different orientations.
1095 * @param biasX x-coordinate of accelerometer bias.
1096 * This is expressed in meters per squared second (m/s^2).
1097 * @param biasY y-coordinate of accelerometer bias.
1098 * This is expressed in meters per squared second (m/s^2).
1099 * @param biasZ z-coordinate of accelerometer bias.
1100 * This is expressed in meters per squared second (m/s^2).
1101 * @param initialSx initial x scaling factor.
1102 * @param initialSy initial y scaling factor.
1103 * @param initialSz initial z scaling factor.
1104 * @param initialMxy initial x-y cross coupling error.
1105 * @param initialMxz initial x-z cross coupling error.
1106 * @param initialMyx initial y-x cross coupling error.
1107 * @param initialMyz initial y-z cross coupling error.
1108 * @param initialMzx initial z-x cross coupling error.
1109 * @param initialMzy initial z-y cross coupling error.
1110 */
1111 protected BaseBiasGravityNormAccelerometerCalibrator(
1112 final Collection<StandardDeviationBodyKinematics> measurements,
1113 final double biasX, final double biasY, final double biasZ, final double initialSx,
1114 final double initialSy, final double initialSz, final double initialMxy, final double initialMxz,
1115 final double initialMyx, final double initialMyz, final double initialMzx, final double initialMzy) {
1116 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx, initialMyz,
1117 initialMzx, initialMzy);
1118 this.measurements = measurements;
1119 }
1120
1121 /**
1122 * Constructor.
1123 *
1124 * @param measurements collection of body kinematics measurements with standard
1125 * deviations taken at the same position with zero velocity
1126 * and unknown different orientations.
1127 * @param biasX x-coordinate of accelerometer bias.
1128 * This is expressed in meters per squared second (m/s^2).
1129 * @param biasY y-coordinate of accelerometer bias.
1130 * This is expressed in meters per squared second (m/s^2).
1131 * @param biasZ z-coordinate of accelerometer bias.
1132 * This is expressed in meters per squared second (m/s^2).
1133 * @param initialSx initial x scaling factor.
1134 * @param initialSy initial y scaling factor.
1135 * @param initialSz initial z scaling factor.
1136 * @param initialMxy initial x-y cross coupling error.
1137 * @param initialMxz initial x-z cross coupling error.
1138 * @param initialMyx initial y-x cross coupling error.
1139 * @param initialMyz initial y-z cross coupling error.
1140 * @param initialMzx initial z-x cross coupling error.
1141 * @param initialMzy initial z-y cross coupling error.
1142 * @param listener listener to handle events raised by this calibrator.
1143 */
1144 protected BaseBiasGravityNormAccelerometerCalibrator(
1145 final Collection<StandardDeviationBodyKinematics> measurements, final double biasX, final double biasY,
1146 final double biasZ, final double initialSx, final double initialSy, final double initialSz,
1147 final double initialMxy, final double initialMxz, final double initialMyx, final double initialMyz,
1148 final double initialMzx, final double initialMzy, final L listener) {
1149 this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
1150 initialMyz, initialMzx, initialMzy);
1151 this.listener = listener;
1152 }
1153
1154 /**
1155 * Constructor.
1156 *
1157 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1158 * accelerometer and gyroscope.
1159 * @param biasX x-coordinate of accelerometer bias.
1160 * This is expressed in meters per squared second (m/s^2).
1161 * @param biasY y-coordinate of accelerometer bias.
1162 * This is expressed in meters per squared second (m/s^2).
1163 * @param biasZ z-coordinate of accelerometer bias.
1164 * This is expressed in meters per squared second (m/s^2).
1165 * @param initialSx initial x scaling factor.
1166 * @param initialSy initial y scaling factor.
1167 * @param initialSz initial z scaling factor.
1168 * @param initialMxy initial x-y cross coupling error.
1169 * @param initialMxz initial x-z cross coupling error.
1170 * @param initialMyx initial y-x cross coupling error.
1171 * @param initialMyz initial y-z cross coupling error.
1172 * @param initialMzx initial z-x cross coupling error.
1173 * @param initialMzy initial z-y cross coupling error.
1174 */
1175 protected BaseBiasGravityNormAccelerometerCalibrator(
1176 final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
1177 final double initialSx, final double initialSy, final double initialSz, final double initialMxy,
1178 final double initialMxz, final double initialMyx, final double initialMyz, final double initialMzx,
1179 final double initialMzy) {
1180 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx, initialMyz,
1181 initialMzx, initialMzy);
1182 this.commonAxisUsed = commonAxisUsed;
1183 }
1184
1185 /**
1186 * Constructor.
1187 *
1188 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1189 * accelerometer and gyroscope.
1190 * @param biasX x-coordinate of accelerometer bias.
1191 * This is expressed in meters per squared second (m/s^2).
1192 * @param biasY y-coordinate of accelerometer bias.
1193 * This is expressed in meters per squared second (m/s^2).
1194 * @param biasZ z-coordinate of accelerometer bias.
1195 * This is expressed in meters per squared second (m/s^2).
1196 * @param initialSx initial x scaling factor.
1197 * @param initialSy initial y scaling factor.
1198 * @param initialSz initial z scaling factor.
1199 * @param initialMxy initial x-y cross coupling error.
1200 * @param initialMxz initial x-z cross coupling error.
1201 * @param initialMyx initial y-x cross coupling error.
1202 * @param initialMyz initial y-z cross coupling error.
1203 * @param initialMzx initial z-x cross coupling error.
1204 * @param initialMzy initial z-y cross coupling error.
1205 * @param listener listener to handle events raised by this calibrator.
1206 */
1207 protected BaseBiasGravityNormAccelerometerCalibrator(
1208 final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
1209 final double initialSx, final double initialSy, final double initialSz, final double initialMxy,
1210 final double initialMxz, final double initialMyx, final double initialMyz, final double initialMzx,
1211 final double initialMzy, final L listener) {
1212 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
1213 initialMyz, initialMzx, initialMzy);
1214 this.listener = listener;
1215 }
1216
1217 /**
1218 * Constructor.
1219 *
1220 * @param measurements collection of body kinematics measurements with standard
1221 * deviations taken at the same position with zero velocity
1222 * and unknown different orientations.
1223 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1224 * accelerometer and gyroscope.
1225 * @param biasX x-coordinate of accelerometer bias.
1226 * This is expressed in meters per squared second (m/s^2).
1227 * @param biasY y-coordinate of accelerometer bias.
1228 * This is expressed in meters per squared second (m/s^2).
1229 * @param biasZ z-coordinate of accelerometer bias.
1230 * This is expressed in meters per squared second (m/s^2).
1231 * @param initialSx initial x scaling factor.
1232 * @param initialSy initial y scaling factor.
1233 * @param initialSz initial z scaling factor.
1234 * @param initialMxy initial x-y cross coupling error.
1235 * @param initialMxz initial x-z cross coupling error.
1236 * @param initialMyx initial y-x cross coupling error.
1237 * @param initialMyz initial y-z cross coupling error.
1238 * @param initialMzx initial z-x cross coupling error.
1239 * @param initialMzy initial z-y cross coupling error.
1240 */
1241 protected BaseBiasGravityNormAccelerometerCalibrator(
1242 final Collection<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1243 final double biasX, final double biasY, final double biasZ, final double initialSx, final double initialSy,
1244 final double initialSz, final double initialMxy, final double initialMxz, final double initialMyx,
1245 final double initialMyz, final double initialMzx, final double initialMzy) {
1246 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
1247 initialMyz, initialMzx, initialMzy);
1248 this.measurements = measurements;
1249 }
1250
1251 /**
1252 * Constructor.
1253 *
1254 * @param measurements collection of body kinematics measurements with standard
1255 * deviations taken at the same position with zero velocity
1256 * and unknown different orientations.
1257 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1258 * accelerometer and gyroscope.
1259 * @param biasX x-coordinate of accelerometer bias.
1260 * This is expressed in meters per squared second (m/s^2).
1261 * @param biasY y-coordinate of accelerometer bias.
1262 * This is expressed in meters per squared second (m/s^2).
1263 * @param biasZ z-coordinate of accelerometer bias.
1264 * This is expressed in meters per squared second (m/s^2).
1265 * @param initialSx initial x scaling factor.
1266 * @param initialSy initial y scaling factor.
1267 * @param initialSz initial z scaling factor.
1268 * @param initialMxy initial x-y cross coupling error.
1269 * @param initialMxz initial x-z cross coupling error.
1270 * @param initialMyx initial y-x cross coupling error.
1271 * @param initialMyz initial y-z cross coupling error.
1272 * @param initialMzx initial z-x cross coupling error.
1273 * @param initialMzy initial z-y cross coupling error.
1274 * @param listener listener to handle events raised by this calibrator.
1275 */
1276 protected BaseBiasGravityNormAccelerometerCalibrator(
1277 final Collection<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1278 final double biasX, final double biasY, final double biasZ, final double initialSx, final double initialSy,
1279 final double initialSz, final double initialMxy, final double initialMxz, final double initialMyx,
1280 final double initialMyz, final double initialMzx, final double initialMzy, final L listener) {
1281 this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy,
1282 initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
1283 this.listener = listener;
1284 }
1285
1286 /**
1287 * Constructor.
1288 *
1289 * @param biasX x-coordinate of accelerometer bias.
1290 * @param biasY y-coordinate of accelerometer bias.
1291 * @param biasZ z-coordinate of accelerometer bias.
1292 * @param initialSx initial x scaling factor.
1293 * @param initialSy initial y scaling factor.
1294 * @param initialSz initial z scaling factor.
1295 * @param initialMxy initial x-y cross coupling error.
1296 * @param initialMxz initial x-z cross coupling error.
1297 * @param initialMyx initial y-x cross coupling error.
1298 * @param initialMyz initial y-z cross coupling error.
1299 * @param initialMzx initial z-x cross coupling error.
1300 * @param initialMzy initial z-y cross coupling error.
1301 */
1302 protected BaseBiasGravityNormAccelerometerCalibrator(
1303 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final double initialSx,
1304 final double initialSy, final double initialSz, final double initialMxy, final double initialMxz,
1305 final double initialMyx, final double initialMyz, final double initialMzx, final double initialMzy) {
1306 this(biasX, biasY, biasZ);
1307 try {
1308 setInitialScalingFactorsAndCrossCouplingErrors(initialSx, initialSy, initialSz, initialMxy, initialMxz,
1309 initialMyx, initialMyz, initialMzx, initialMzy);
1310 } catch (final LockedException ignore) {
1311 // never happens
1312 }
1313 }
1314
1315 /**
1316 * Constructor.
1317 *
1318 * @param biasX x-coordinate of accelerometer bias.
1319 * @param biasY y-coordinate of accelerometer bias.
1320 * @param biasZ z-coordinate of accelerometer bias.
1321 * @param initialSx initial x scaling factor.
1322 * @param initialSy initial y scaling factor.
1323 * @param initialSz initial z scaling factor.
1324 * @param initialMxy initial x-y cross coupling error.
1325 * @param initialMxz initial x-z cross coupling error.
1326 * @param initialMyx initial y-x cross coupling error.
1327 * @param initialMyz initial y-z cross coupling error.
1328 * @param initialMzx initial z-x cross coupling error.
1329 * @param initialMzy initial z-y cross coupling error.
1330 * @param listener listener to handle events raised by this calibrator.
1331 */
1332 protected BaseBiasGravityNormAccelerometerCalibrator(
1333 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final double initialSx,
1334 final double initialSy, final double initialSz, final double initialMxy,
1335 final double initialMxz, final double initialMyx, final double initialMyz,
1336 final double initialMzx, final double initialMzy, final L listener) {
1337 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx, initialMyz,
1338 initialMzx, initialMzy);
1339 this.listener = listener;
1340 }
1341
1342 /**
1343 * Constructor.
1344 *
1345 * @param measurements collection of body kinematics measurements with standard
1346 * deviations taken at the same position with zero velocity
1347 * and unknown different orientations.
1348 * @param biasX x-coordinate of accelerometer bias.
1349 * @param biasY y-coordinate of accelerometer bias.
1350 * @param biasZ z-coordinate of accelerometer bias.
1351 * @param initialSx initial x scaling factor.
1352 * @param initialSy initial y scaling factor.
1353 * @param initialSz initial z scaling factor.
1354 * @param initialMxy initial x-y cross coupling error.
1355 * @param initialMxz initial x-z cross coupling error.
1356 * @param initialMyx initial y-x cross coupling error.
1357 * @param initialMyz initial y-z cross coupling error.
1358 * @param initialMzx initial z-x cross coupling error.
1359 * @param initialMzy initial z-y cross coupling error.
1360 */
1361 protected BaseBiasGravityNormAccelerometerCalibrator(
1362 final Collection<StandardDeviationBodyKinematics> measurements, final Acceleration biasX,
1363 final Acceleration biasY, final Acceleration biasZ, final double initialSx, final double initialSy,
1364 final double initialSz, final double initialMxy, final double initialMxz, final double initialMyx,
1365 final double initialMyz, final double initialMzx, final double initialMzy) {
1366 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx, initialMyz,
1367 initialMzx, initialMzy);
1368 this.measurements = measurements;
1369 }
1370
1371 /**
1372 * Constructor.
1373 *
1374 * @param measurements collection of body kinematics measurements with standard
1375 * deviations taken at the same position with zero velocity
1376 * and unknown different orientations.
1377 * @param biasX x-coordinate of accelerometer bias.
1378 * @param biasY y-coordinate of accelerometer bias.
1379 * @param biasZ z-coordinate of accelerometer bias.
1380 * @param initialSx initial x scaling factor.
1381 * @param initialSy initial y scaling factor.
1382 * @param initialSz initial z scaling factor.
1383 * @param initialMxy initial x-y cross coupling error.
1384 * @param initialMxz initial x-z cross coupling error.
1385 * @param initialMyx initial y-x cross coupling error.
1386 * @param initialMyz initial y-z cross coupling error.
1387 * @param initialMzx initial z-x cross coupling error.
1388 * @param initialMzy initial z-y cross coupling error.
1389 * @param listener listener to handle events raised by this calibrator.
1390 */
1391 protected BaseBiasGravityNormAccelerometerCalibrator(
1392 final Collection<StandardDeviationBodyKinematics> measurements,
1393 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final double initialSx,
1394 final double initialSy, final double initialSz, final double initialMxy,
1395 final double initialMxz, final double initialMyx, final double initialMyz,
1396 final double initialMzx, final double initialMzy, final L listener) {
1397 this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
1398 initialMyz, initialMzx, initialMzy);
1399 this.listener = listener;
1400 }
1401
1402 /**
1403 * Constructor.
1404 *
1405 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1406 * accelerometer and gyroscope.
1407 * @param biasX x-coordinate of accelerometer bias.
1408 * @param biasY y-coordinate of accelerometer bias.
1409 * @param biasZ z-coordinate of accelerometer bias.
1410 * @param initialSx initial x scaling factor.
1411 * @param initialSy initial y scaling factor.
1412 * @param initialSz initial z scaling factor.
1413 * @param initialMxy initial x-y cross coupling error.
1414 * @param initialMxz initial x-z cross coupling error.
1415 * @param initialMyx initial y-x cross coupling error.
1416 * @param initialMyz initial y-z cross coupling error.
1417 * @param initialMzx initial z-x cross coupling error.
1418 * @param initialMzy initial z-y cross coupling error.
1419 */
1420 protected BaseBiasGravityNormAccelerometerCalibrator(
1421 final boolean commonAxisUsed, final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
1422 final double initialSx, final double initialSy, final double initialSz, final double initialMxy,
1423 final double initialMxz, final double initialMyx, final double initialMyz, final double initialMzx,
1424 final double initialMzy) {
1425 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx, initialMyz,
1426 initialMzx, initialMzy);
1427 this.commonAxisUsed = commonAxisUsed;
1428 }
1429
1430 /**
1431 * Constructor.
1432 *
1433 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1434 * accelerometer and gyroscope.
1435 * @param biasX x-coordinate of accelerometer bias.
1436 * @param biasY y-coordinate of accelerometer bias.
1437 * @param biasZ z-coordinate of accelerometer bias.
1438 * @param initialSx initial x scaling factor.
1439 * @param initialSy initial y scaling factor.
1440 * @param initialSz initial z scaling factor.
1441 * @param initialMxy initial x-y cross coupling error.
1442 * @param initialMxz initial x-z cross coupling error.
1443 * @param initialMyx initial y-x cross coupling error.
1444 * @param initialMyz initial y-z cross coupling error.
1445 * @param initialMzx initial z-x cross coupling error.
1446 * @param initialMzy initial z-y cross coupling error.
1447 * @param listener listener to handle events raised by this calibrator.
1448 */
1449 protected BaseBiasGravityNormAccelerometerCalibrator(
1450 final boolean commonAxisUsed, final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
1451 final double initialSx, final double initialSy, final double initialSz,
1452 final double initialMxy, final double initialMxz, final double initialMyx,
1453 final double initialMyz, final double initialMzx, final double initialMzy,
1454 final L listener) {
1455 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
1456 initialMyz, initialMzx, initialMzy);
1457 this.listener = listener;
1458 }
1459
1460 /**
1461 * Constructor.
1462 *
1463 * @param measurements collection of body kinematics measurements with standard
1464 * deviations taken at the same position with zero velocity
1465 * and unknown different orientations.
1466 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1467 * accelerometer and gyroscope.
1468 * @param biasX x-coordinate of accelerometer bias.
1469 * @param biasY y-coordinate of accelerometer bias.
1470 * @param biasZ z-coordinate of accelerometer bias.
1471 * @param initialSx initial x scaling factor.
1472 * @param initialSy initial y scaling factor.
1473 * @param initialSz initial z scaling factor.
1474 * @param initialMxy initial x-y cross coupling error.
1475 * @param initialMxz initial x-z cross coupling error.
1476 * @param initialMyx initial y-x cross coupling error.
1477 * @param initialMyz initial y-z cross coupling error.
1478 * @param initialMzx initial z-x cross coupling error.
1479 * @param initialMzy initial z-y cross coupling error.
1480 */
1481 protected BaseBiasGravityNormAccelerometerCalibrator(
1482 final Collection<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1483 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
1484 final double initialSx, final double initialSy, final double initialSz,
1485 final double initialMxy, final double initialMxz, final double initialMyx,
1486 final double initialMyz, final double initialMzx, final double initialMzy) {
1487 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
1488 initialMyz, initialMzx, initialMzy);
1489 this.measurements = measurements;
1490 }
1491
1492 /**
1493 * Constructor.
1494 *
1495 * @param measurements collection of body kinematics measurements with standard
1496 * deviations taken at the same position with zero velocity
1497 * and unknown different orientations.
1498 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1499 * accelerometer and gyroscope.
1500 * @param biasX x-coordinate of accelerometer bias.
1501 * @param biasY y-coordinate of accelerometer bias.
1502 * @param biasZ z-coordinate of accelerometer bias.
1503 * @param initialSx initial x scaling factor.
1504 * @param initialSy initial y scaling factor.
1505 * @param initialSz initial z scaling factor.
1506 * @param initialMxy initial x-y cross coupling error.
1507 * @param initialMxz initial x-z cross coupling error.
1508 * @param initialMyx initial y-x cross coupling error.
1509 * @param initialMyz initial y-z cross coupling error.
1510 * @param initialMzx initial z-x cross coupling error.
1511 * @param initialMzy initial z-y cross coupling error.
1512 * @param listener listener to handle events raised by this calibrator.
1513 */
1514 protected BaseBiasGravityNormAccelerometerCalibrator(
1515 final Collection<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1516 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
1517 final double initialSx, final double initialSy, final double initialSz,
1518 final double initialMxy, final double initialMxz, final double initialMyx,
1519 final double initialMyz, final double initialMzx, final double initialMzy,
1520 final L listener) {
1521 this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy,
1522 initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
1523 this.listener = listener;
1524 }
1525
1526 /**
1527 * Constructor.
1528 *
1529 * @param bias known accelerometer bias. This must have length 3 and is expressed
1530 * in meters per squared second (m/s^2).
1531 * @throws IllegalArgumentException if provided bias array does not have length 3.
1532 */
1533 protected BaseBiasGravityNormAccelerometerCalibrator(final double[] bias) {
1534 try {
1535 setBias(bias);
1536 } catch (final LockedException ignore) {
1537 // never happens
1538 }
1539 }
1540
1541 /**
1542 * Constructor.
1543 *
1544 * @param bias known accelerometer bias. This must have length 3 and is expressed
1545 * in meters per squared second (m/s^2).
1546 * @param listener listener to handle events raised by this calibrator.
1547 * @throws IllegalArgumentException if provided bias array does not have length 3.
1548 */
1549 protected BaseBiasGravityNormAccelerometerCalibrator(final double[] bias, final L listener) {
1550 this(bias);
1551 this.listener = listener;
1552 }
1553
1554 /**
1555 * Constructor.
1556 *
1557 * @param measurements collection of body kinematics measurements with standard
1558 * deviations taken at the same position with zero velocity
1559 * and unknown different orientations.
1560 * @param bias known accelerometer bias. This must have length 3 and is expressed
1561 * in meters per squared second (m/s^2).
1562 * @throws IllegalArgumentException if provided bias array does not have length 3.
1563 */
1564 protected BaseBiasGravityNormAccelerometerCalibrator(
1565 final Collection<StandardDeviationBodyKinematics> measurements, final double[] bias) {
1566 this(bias);
1567 this.measurements = measurements;
1568 }
1569
1570 /**
1571 * Constructor.
1572 *
1573 * @param measurements collection of body kinematics measurements with standard
1574 * deviations taken at the same position with zero velocity
1575 * and unknown different orientations.
1576 * @param bias known accelerometer bias. This must have length 3 and is expressed
1577 * in meters per squared second (m/s^2).
1578 * @param listener listener to handle events raised by this calibrator.
1579 * @throws IllegalArgumentException if provided bias array does not have length 3.
1580 */
1581 protected BaseBiasGravityNormAccelerometerCalibrator(
1582 final Collection<StandardDeviationBodyKinematics> measurements, final double[] bias, final L listener) {
1583 this(measurements, bias);
1584 this.listener = listener;
1585 }
1586
1587 /**
1588 * Constructor.
1589 *
1590 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1591 * accelerometer and gyroscope.
1592 * @param bias known accelerometer bias. This must have length 3 and is expressed
1593 * in meters per squared second (m/s^2).
1594 * @throws IllegalArgumentException if provided bias array does not have length 3.
1595 */
1596 protected BaseBiasGravityNormAccelerometerCalibrator(final boolean commonAxisUsed, final double[] bias) {
1597 this(bias);
1598 this.commonAxisUsed = commonAxisUsed;
1599 }
1600
1601 /**
1602 * Constructor.
1603 *
1604 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1605 * accelerometer and gyroscope.
1606 * @param bias known accelerometer bias. This must have length 3 and is expressed
1607 * in meters per squared second (m/s^2).
1608 * @param listener listener to handle events raised by this calibrator.
1609 * @throws IllegalArgumentException if provided bias array does not have length 3.
1610 */
1611 protected BaseBiasGravityNormAccelerometerCalibrator(
1612 final boolean commonAxisUsed, final double[] bias, final L listener) {
1613 this(commonAxisUsed, bias);
1614 this.listener = listener;
1615 }
1616
1617 /**
1618 * Constructor.
1619 *
1620 * @param measurements collection of body kinematics measurements with standard
1621 * deviations taken at the same position with zero velocity
1622 * and unknown different orientations.
1623 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1624 * accelerometer and gyroscope.
1625 * @param bias known accelerometer bias. This must have length 3 and is expressed
1626 * in meters per squared second (m/s^2).
1627 * @throws IllegalArgumentException if provided bias array does not have length 3.
1628 */
1629 protected BaseBiasGravityNormAccelerometerCalibrator(
1630 final Collection<StandardDeviationBodyKinematics> measurements,
1631 final boolean commonAxisUsed, final double[] bias) {
1632 this(commonAxisUsed, bias);
1633 this.measurements = measurements;
1634 }
1635
1636 /**
1637 * Constructor.
1638 *
1639 * @param measurements collection of body kinematics measurements with standard
1640 * deviations taken at the same position with zero velocity
1641 * and unknown different orientations.
1642 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1643 * accelerometer and gyroscope.
1644 * @param bias known accelerometer bias. This must have length 3 and is expressed
1645 * in meters per squared second (m/s^2).
1646 * @param listener listener to handle events raised by this calibrator.
1647 * @throws IllegalArgumentException if provided bias array does not have length 3.
1648 */
1649 protected BaseBiasGravityNormAccelerometerCalibrator(
1650 final Collection<StandardDeviationBodyKinematics> measurements,
1651 final boolean commonAxisUsed, final double[] bias, final L listener) {
1652 this(measurements, commonAxisUsed, bias);
1653 this.listener = listener;
1654 }
1655
1656 /**
1657 * Constructor.
1658 *
1659 * @param bias known accelerometer bias.
1660 * @throws IllegalArgumentException if provided bias matrix is not 3x1.
1661 */
1662 protected BaseBiasGravityNormAccelerometerCalibrator(final Matrix bias) {
1663 try {
1664 setBias(bias);
1665 } catch (final LockedException ignore) {
1666 // never happens
1667 }
1668 }
1669
1670 /**
1671 * Constructor.
1672 *
1673 * @param bias known accelerometer bias.
1674 * @param listener listener to handle events raised by this calibrator.
1675 * @throws IllegalArgumentException if provided bias matrix is not 3x1.
1676 */
1677 protected BaseBiasGravityNormAccelerometerCalibrator(final Matrix bias, final L listener) {
1678 this(bias);
1679 this.listener = listener;
1680 }
1681
1682 /**
1683 * Constructor.
1684 *
1685 * @param measurements collection of body kinematics measurements with standard
1686 * deviations taken at the same position with zero velocity
1687 * and unknown different orientations.
1688 * @param bias known accelerometer bias.
1689 * @throws IllegalArgumentException if provided bias matrix is not 3x1.
1690 */
1691 protected BaseBiasGravityNormAccelerometerCalibrator(
1692 final Collection<StandardDeviationBodyKinematics> measurements, final Matrix bias) {
1693 this(bias);
1694 this.measurements = measurements;
1695 }
1696
1697 /**
1698 * Constructor.
1699 *
1700 * @param measurements collection of body kinematics measurements with standard
1701 * deviations taken at the same position with zero velocity
1702 * and unknown different orientations.
1703 * @param bias known accelerometer bias.
1704 * @param listener listener to handle events raised by this calibrator.
1705 */
1706 protected BaseBiasGravityNormAccelerometerCalibrator(
1707 final Collection<StandardDeviationBodyKinematics> measurements, final Matrix bias, final L listener) {
1708 this(measurements, bias);
1709 this.listener = listener;
1710 }
1711
1712 /**
1713 * Constructor.
1714 *
1715 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1716 * accelerometer and gyroscope.
1717 * @param bias known accelerometer bias.
1718 * @throws IllegalArgumentException if provided bias matrix is not 3x1.
1719 */
1720 protected BaseBiasGravityNormAccelerometerCalibrator(final boolean commonAxisUsed, final Matrix bias) {
1721 this(bias);
1722 this.commonAxisUsed = commonAxisUsed;
1723 }
1724
1725 /**
1726 * Constructor.
1727 *
1728 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1729 * accelerometer and gyroscope.
1730 * @param bias known accelerometer bias.
1731 * @param listener listener to handle events raised by this calibrator.
1732 * @throws IllegalArgumentException if provided bias matrix is not 3x1.
1733 */
1734 protected BaseBiasGravityNormAccelerometerCalibrator(
1735 final boolean commonAxisUsed, final Matrix bias, final L listener) {
1736 this(commonAxisUsed, bias);
1737 this.listener = listener;
1738 }
1739
1740 /**
1741 * Constructor.
1742 *
1743 * @param measurements collection of body kinematics measurements with standard
1744 * deviations taken at the same position with zero velocity
1745 * and unknown different orientations.
1746 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1747 * accelerometer and gyroscope.
1748 * @param bias known accelerometer bias.
1749 * @throws IllegalArgumentException if provided bias matrix is not 3x1.
1750 */
1751 protected BaseBiasGravityNormAccelerometerCalibrator(
1752 final Collection<StandardDeviationBodyKinematics> measurements,
1753 final boolean commonAxisUsed, final Matrix bias) {
1754 this(commonAxisUsed, bias);
1755 this.measurements = measurements;
1756 }
1757
1758 /**
1759 * Constructor.
1760 *
1761 * @param measurements collection of body kinematics measurements with standard
1762 * deviations taken at the same position with zero velocity
1763 * and unknown different orientations.
1764 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1765 * accelerometer and gyroscope.
1766 * @param bias known accelerometer bias.
1767 * @param listener listener to handle events raised by this calibrator.
1768 * @throws IllegalArgumentException if provided bias matrix is not 3x1.
1769 */
1770 protected BaseBiasGravityNormAccelerometerCalibrator(
1771 final Collection<StandardDeviationBodyKinematics> measurements,
1772 final boolean commonAxisUsed, final Matrix bias, final L listener) {
1773 this(measurements, commonAxisUsed, bias);
1774 this.listener = listener;
1775 }
1776
1777 /**
1778 * Constructor.
1779 *
1780 * @param bias known accelerometer bias.
1781 * @param initialMa initial scale factors and cross coupling errors matrix.
1782 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1783 * scaling and coupling error matrix is not 3x3.
1784 */
1785 protected BaseBiasGravityNormAccelerometerCalibrator(final Matrix bias, final Matrix initialMa) {
1786 this(bias);
1787 try {
1788 setInitialMa(initialMa);
1789 } catch (final LockedException ignore) {
1790 // never happens
1791 }
1792 }
1793
1794 /**
1795 * Constructor.
1796 *
1797 * @param bias known accelerometer bias.
1798 * @param initialMa initial scale factors and cross coupling errors matrix.
1799 * @param listener listener to handle events raised by this calibrator.
1800 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1801 * scaling and coupling error matrix is not 3x3.
1802 */
1803 protected BaseBiasGravityNormAccelerometerCalibrator(final Matrix bias, final Matrix initialMa, final L listener) {
1804 this(bias, initialMa);
1805 this.listener = listener;
1806 }
1807
1808 /**
1809 * Constructor.
1810 *
1811 * @param measurements collection of body kinematics measurements with standard
1812 * deviations taken at the same position with zero velocity
1813 * and unknown different orientations.
1814 * @param bias known accelerometer bias.
1815 * @param initialMa initial scale factors and cross coupling errors matrix.
1816 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1817 * scaling and coupling error matrix is not 3x3.
1818 */
1819 protected BaseBiasGravityNormAccelerometerCalibrator(
1820 final Collection<StandardDeviationBodyKinematics> measurements,
1821 final Matrix bias, final Matrix initialMa) {
1822 this(bias, initialMa);
1823 this.measurements = measurements;
1824 }
1825
1826 /**
1827 * Constructor.
1828 *
1829 * @param measurements collection of body kinematics measurements with standard
1830 * deviations taken at the same position with zero velocity
1831 * and unknown different orientations.
1832 * @param bias known accelerometer bias.
1833 * @param initialMa initial scale factors and cross coupling errors matrix.
1834 * @param listener listener to handle events raised by this calibrator.
1835 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1836 * scaling and coupling error matrix is not 3x3.
1837 */
1838 protected BaseBiasGravityNormAccelerometerCalibrator(
1839 final Collection<StandardDeviationBodyKinematics> measurements,
1840 final Matrix bias, final Matrix initialMa, final L listener) {
1841 this(measurements, bias, initialMa);
1842 this.listener = listener;
1843 }
1844
1845 /**
1846 * Constructor.
1847 *
1848 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1849 * accelerometer and gyroscope.
1850 * @param bias known accelerometer bias.
1851 * @param initialMa initial scale factors and cross coupling errors matrix.
1852 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1853 * scaling and coupling error matrix is not 3x3.
1854 */
1855 protected BaseBiasGravityNormAccelerometerCalibrator(final boolean commonAxisUsed, final Matrix bias,
1856 final Matrix initialMa) {
1857 this(bias, initialMa);
1858 this.commonAxisUsed = commonAxisUsed;
1859 }
1860
1861 /**
1862 * Constructor.
1863 *
1864 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1865 * accelerometer and gyroscope.
1866 * @param bias known accelerometer bias.
1867 * @param initialMa initial scale factors and cross coupling errors matrix.
1868 * @param listener listener to handle events raised by this calibrator.
1869 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1870 * scaling and coupling error matrix is not 3x3.
1871 */
1872 protected BaseBiasGravityNormAccelerometerCalibrator(
1873 final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa, final L listener) {
1874 this(commonAxisUsed, bias, initialMa);
1875 this.listener = listener;
1876 }
1877
1878 /**
1879 * Constructor.
1880 *
1881 * @param measurements collection of body kinematics measurements with standard
1882 * deviations taken at the same position with zero velocity
1883 * and unknown different orientations.
1884 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1885 * accelerometer and gyroscope.
1886 * @param bias known accelerometer bias.
1887 * @param initialMa initial scale factors and cross coupling errors matrix.
1888 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1889 * scaling and coupling error matrix is not 3x3.
1890 */
1891 protected BaseBiasGravityNormAccelerometerCalibrator(
1892 final Collection<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1893 final Matrix bias, final Matrix initialMa) {
1894 this(commonAxisUsed, bias, initialMa);
1895 this.measurements = measurements;
1896 }
1897
1898 /**
1899 * Constructor.
1900 *
1901 * @param measurements collection of body kinematics measurements with standard
1902 * deviations taken at the same position with zero velocity
1903 * and unknown different orientations.
1904 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1905 * accelerometer and gyroscope.
1906 * @param bias known accelerometer bias.
1907 * @param initialMa initial scale factors and cross coupling errors matrix.
1908 * @param listener listener to handle events raised by this calibrator.
1909 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
1910 * scaling and coupling error matrix is not 3x3.
1911 */
1912 protected BaseBiasGravityNormAccelerometerCalibrator(
1913 final Collection<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
1914 final Matrix bias, final Matrix initialMa, final L listener) {
1915 this(measurements, commonAxisUsed, bias, initialMa);
1916 this.listener = listener;
1917 }
1918
1919 /**
1920 * Constructor.
1921 *
1922 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1923 * squared second (m/s^2).
1924 * @throws IllegalArgumentException if provided gravity norm value is negative.
1925 */
1926 protected BaseBiasGravityNormAccelerometerCalibrator(final Double groundTruthGravityNorm) {
1927 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
1928 }
1929
1930 /**
1931 * Constructor.
1932 *
1933 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1934 * squared second (m/s^2).
1935 * @param listener listener to handle events raised by this calibrator.
1936 * @throws IllegalArgumentException if provided gravity norm value is negative.
1937 */
1938 protected BaseBiasGravityNormAccelerometerCalibrator(final Double groundTruthGravityNorm, final L listener) {
1939 this(groundTruthGravityNorm);
1940 this.listener = listener;
1941 }
1942
1943 /**
1944 * Constructor.
1945 *
1946 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1947 * squared second (m/s^2).
1948 * @param measurements collection of body kinematics measurements with standard
1949 * deviations taken at the same position with zero velocity
1950 * and unknown different orientations.
1951 * @throws IllegalArgumentException if provided gravity norm value is negative.
1952 */
1953 protected BaseBiasGravityNormAccelerometerCalibrator(
1954 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements) {
1955 this(groundTruthGravityNorm);
1956 this.measurements = measurements;
1957 }
1958
1959 /**
1960 * Constructor.
1961 *
1962 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1963 * squared second (m/s^2).
1964 * @param measurements collection of body kinematics measurements with standard
1965 * deviations taken at the same position with zero velocity
1966 * and unknown different orientations.
1967 * @param listener listener to handle events raised by this calibrator.
1968 * @throws IllegalArgumentException if provided gravity norm value is negative.
1969 */
1970 protected BaseBiasGravityNormAccelerometerCalibrator(
1971 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
1972 final L listener) {
1973 this(measurements, listener);
1974 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
1975 }
1976
1977 /**
1978 * Constructor.
1979 *
1980 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1981 * squared second (m/s^2).
1982 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1983 * accelerometer and gyroscope.
1984 * @throws IllegalArgumentException if provided gravity norm value is negative.
1985 */
1986 protected BaseBiasGravityNormAccelerometerCalibrator(
1987 final Double groundTruthGravityNorm, final boolean commonAxisUsed) {
1988 this(commonAxisUsed);
1989 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
1990 }
1991
1992 /**
1993 * Constructor.
1994 *
1995 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
1996 * squared second (m/s^2).
1997 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
1998 * accelerometer and gyroscope.
1999 * @param listener listener to handle events raised by this calibrator.
2000 * @throws IllegalArgumentException if provided gravity norm value is negative. *
2001 */
2002 protected BaseBiasGravityNormAccelerometerCalibrator(
2003 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final L listener) {
2004 this(commonAxisUsed, listener);
2005 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2006 }
2007
2008 /**
2009 * Constructor.
2010 *
2011 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2012 * squared second (m/s^2).
2013 * @param measurements collection of body kinematics measurements with standard
2014 * deviations taken at the same position with zero velocity
2015 * and unknown different orientations.
2016 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2017 * accelerometer and gyroscope.
2018 * @throws IllegalArgumentException if provided gravity norm value is negative.
2019 */
2020 protected BaseBiasGravityNormAccelerometerCalibrator(
2021 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2022 final boolean commonAxisUsed) {
2023 this(measurements, commonAxisUsed);
2024 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2025 }
2026
2027 /**
2028 * Constructor.
2029 *
2030 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2031 * squared second (m/s^2).
2032 * @param measurements collection of body kinematics measurements with standard
2033 * deviations taken at the same position with zero velocity
2034 * and unknown different orientations.
2035 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2036 * accelerometer and gyroscope.
2037 * @param listener listener to handle events raised by this calibrator.
2038 * @throws IllegalArgumentException if provided gravity norm value is negative.
2039 */
2040 protected BaseBiasGravityNormAccelerometerCalibrator(
2041 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2042 final boolean commonAxisUsed, final L listener) {
2043 this(measurements, commonAxisUsed, listener);
2044 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2045 }
2046
2047 /**
2048 * Constructor.
2049 *
2050 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2051 * squared second (m/s^2).
2052 * @param biasX x-coordinate of accelerometer bias.
2053 * This is expressed in meters per squared second (m/s^2).
2054 * @param biasY y-coordinate of accelerometer bias.
2055 * This is expressed in meters per squared second (m/s^2).
2056 * @param biasZ z-coordinate of accelerometer bias.
2057 * This is expressed in meters per squared second (m/s^2).
2058 * @throws IllegalArgumentException if provided gravity norm value is negative.
2059 */
2060 protected BaseBiasGravityNormAccelerometerCalibrator(
2061 final Double groundTruthGravityNorm, final double biasX, final double biasY, final double biasZ) {
2062 this(biasX, biasY, biasZ);
2063 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2064 }
2065
2066 /**
2067 * Constructor.
2068 *
2069 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2070 * squared second (m/s^2).
2071 * @param biasX x-coordinate of accelerometer bias.
2072 * This is expressed in meters per squared second (m/s^2).
2073 * @param biasY y-coordinate of accelerometer bias.
2074 * This is expressed in meters per squared second (m/s^2).
2075 * @param biasZ z-coordinate of accelerometer bias.
2076 * This is expressed in meters per squared second (m/s^2).
2077 * @param listener listener to handle events raised by this calibrator.
2078 * @throws IllegalArgumentException if provided gravity norm value is negative.
2079 */
2080 protected BaseBiasGravityNormAccelerometerCalibrator(
2081 final Double groundTruthGravityNorm, final double biasX, final double biasY, final double biasZ,
2082 final L listener) {
2083 this(biasX, biasY, biasZ, listener);
2084 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2085 }
2086
2087 /**
2088 * Constructor.
2089 *
2090 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2091 * squared second (m/s^2).
2092 * @param measurements collection of body kinematics measurements with standard
2093 * deviations taken at the same position with zero velocity
2094 * and unknown different orientations.
2095 * @param biasX x-coordinate of accelerometer bias.
2096 * This is expressed in meters per squared second (m/s^2).
2097 * @param biasY y-coordinate of accelerometer bias.
2098 * This is expressed in meters per squared second (m/s^2).
2099 * @param biasZ z-coordinate of accelerometer bias.
2100 * This is expressed in meters per squared second (m/s^2).
2101 * @throws IllegalArgumentException if provided gravity norm value is negative.
2102 */
2103 protected BaseBiasGravityNormAccelerometerCalibrator(
2104 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2105 final double biasX, final double biasY, final double biasZ) {
2106 this(measurements, biasX, biasY, biasZ);
2107 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2108 }
2109
2110 /**
2111 * Constructor.
2112 *
2113 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2114 * squared second (m/s^2).
2115 * @param measurements collection of body kinematics measurements with standard
2116 * deviations taken at the same position with zero velocity
2117 * and unknown different orientations.
2118 * @param biasX x-coordinate of accelerometer bias.
2119 * This is expressed in meters per squared second (m/s^2).
2120 * @param biasY y-coordinate of accelerometer bias.
2121 * This is expressed in meters per squared second (m/s^2).
2122 * @param biasZ z-coordinate of accelerometer bias.
2123 * This is expressed in meters per squared second (m/s^2).
2124 * @param listener listener to handle events raised by this calibrator.
2125 * @throws IllegalArgumentException if provided gravity norm value is negative.
2126 */
2127 protected BaseBiasGravityNormAccelerometerCalibrator(
2128 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2129 final double biasX, final double biasY, final double biasZ, final L listener) {
2130 this(measurements, biasX, biasY, biasZ, listener);
2131 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2132 }
2133
2134 /**
2135 * Constructor.
2136 *
2137 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2138 * squared second (m/s^2).
2139 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2140 * accelerometer and gyroscope.
2141 * @param biasX x-coordinate of accelerometer bias.
2142 * This is expressed in meters per squared second (m/s^2).
2143 * @param biasY y-coordinate of accelerometer bias.
2144 * This is expressed in meters per squared second (m/s^2).
2145 * @param biasZ z-coordinate of accelerometer bias.
2146 * This is expressed in meters per squared second (m/s^2).
2147 * @throws IllegalArgumentException if provided gravity norm value is negative.
2148 */
2149 protected BaseBiasGravityNormAccelerometerCalibrator(
2150 final Double groundTruthGravityNorm, final boolean commonAxisUsed,
2151 final double biasX, final double biasY, final double biasZ) {
2152 this(commonAxisUsed, biasX, biasY, biasZ);
2153 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2154 }
2155
2156 /**
2157 * Constructor.
2158 *
2159 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2160 * squared second (m/s^2).
2161 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2162 * accelerometer and gyroscope.
2163 * @param biasX x-coordinate of accelerometer bias.
2164 * This is expressed in meters per squared second (m/s^2).
2165 * @param biasY y-coordinate of accelerometer bias.
2166 * This is expressed in meters per squared second (m/s^2).
2167 * @param biasZ z-coordinate of accelerometer bias.
2168 * This is expressed in meters per squared second (m/s^2).
2169 * @param listener listener to handle events raised by this calibrator.
2170 * @throws IllegalArgumentException if provided gravity norm value is negative.
2171 */
2172 protected BaseBiasGravityNormAccelerometerCalibrator(
2173 final Double groundTruthGravityNorm, final boolean commonAxisUsed,
2174 final double biasX, final double biasY, final double biasZ, final L listener) {
2175 this(commonAxisUsed, biasX, biasY, biasZ, listener);
2176 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2177 }
2178
2179 /**
2180 * Constructor.
2181 *
2182 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2183 * squared second (m/s^2).
2184 * @param measurements collection of body kinematics measurements with standard
2185 * deviations taken at the same position with zero velocity
2186 * and unknown different orientations.
2187 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2188 * accelerometer and gyroscope.
2189 * @param biasX x-coordinate of accelerometer bias.
2190 * This is expressed in meters per squared second (m/s^2).
2191 * @param biasY y-coordinate of accelerometer bias.
2192 * This is expressed in meters per squared second (m/s^2).
2193 * @param biasZ z-coordinate of accelerometer bias.
2194 * This is expressed in meters per squared second (m/s^2).
2195 * @throws IllegalArgumentException if provided gravity norm value is negative.
2196 */
2197 protected BaseBiasGravityNormAccelerometerCalibrator(
2198 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2199 final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ) {
2200 this(measurements, commonAxisUsed, biasX, biasY, biasZ);
2201 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2202 }
2203
2204 /**
2205 * Constructor.
2206 *
2207 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2208 * squared second (m/s^2).
2209 * @param measurements collection of body kinematics measurements with standard
2210 * deviations taken at the same position with zero velocity
2211 * and unknown different orientations.
2212 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2213 * accelerometer and gyroscope.
2214 * @param biasX x-coordinate of accelerometer bias.
2215 * This is expressed in meters per squared second (m/s^2).
2216 * @param biasY y-coordinate of accelerometer bias.
2217 * This is expressed in meters per squared second (m/s^2).
2218 * @param biasZ z-coordinate of accelerometer bias.
2219 * This is expressed in meters per squared second (m/s^2).
2220 * @param listener listener to handle events raised by this calibrator.
2221 * @throws IllegalArgumentException if provided gravity norm value is negative.
2222 */
2223 protected BaseBiasGravityNormAccelerometerCalibrator(
2224 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2225 final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
2226 final L listener) {
2227 this(measurements, commonAxisUsed, biasX, biasY, biasZ, listener);
2228 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2229 }
2230
2231 /**
2232 * Constructor.
2233 *
2234 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2235 * squared second (m/s^2).
2236 * @param biasX x-coordinate of accelerometer bias.
2237 * @param biasY y-coordinate of accelerometer bias.
2238 * @param biasZ z-coordinate of accelerometer bias.
2239 * @throws IllegalArgumentException if provided gravity norm value is negative.
2240 */
2241 protected BaseBiasGravityNormAccelerometerCalibrator(
2242 final Double groundTruthGravityNorm,
2243 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) {
2244 this(biasX, biasY, biasZ);
2245 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2246 }
2247
2248 /**
2249 * Constructor.
2250 *
2251 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2252 * squared second (m/s^2).
2253 * @param biasX x-coordinate of accelerometer bias.
2254 * @param biasY y-coordinate of accelerometer bias.
2255 * @param biasZ z-coordinate of accelerometer bias.
2256 * @param listener listener to handle events raised by this calibrator.
2257 * @throws IllegalArgumentException if provided gravity norm value is negative.
2258 */
2259 protected BaseBiasGravityNormAccelerometerCalibrator(
2260 final Double groundTruthGravityNorm,
2261 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final L listener) {
2262 this(biasX, biasY, biasZ, listener);
2263 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2264 }
2265
2266 /**
2267 * Constructor.
2268 *
2269 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2270 * squared second (m/s^2).
2271 * @param measurements collection of body kinematics measurements with standard
2272 * deviations taken at the same position with zero velocity
2273 * and unknown different orientations.
2274 * @param biasX x-coordinate of accelerometer bias.
2275 * @param biasY y-coordinate of accelerometer bias.
2276 * @param biasZ z-coordinate of accelerometer bias.
2277 * @throws IllegalArgumentException if provided gravity norm value is negative.
2278 */
2279 protected BaseBiasGravityNormAccelerometerCalibrator(
2280 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2281 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) {
2282 this(measurements, biasX, biasY, biasZ);
2283 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2284 }
2285
2286 /**
2287 * Constructor.
2288 *
2289 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2290 * squared second (m/s^2).
2291 * @param measurements collection of body kinematics measurements with standard
2292 * deviations taken at the same position with zero velocity
2293 * and unknown different orientations.
2294 * @param biasX x-coordinate of accelerometer bias.
2295 * @param biasY y-coordinate of accelerometer bias.
2296 * @param biasZ z-coordinate of accelerometer bias.
2297 * @param listener listener to handle events raised by this calibrator.
2298 * @throws IllegalArgumentException if provided gravity norm value is negative.
2299 */
2300 protected BaseBiasGravityNormAccelerometerCalibrator(
2301 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2302 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final L listener) {
2303 this(measurements, biasX, biasY, biasZ, listener);
2304 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2305 }
2306
2307 /**
2308 * Constructor.
2309 *
2310 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2311 * squared second (m/s^2).
2312 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2313 * accelerometer and gyroscope.
2314 * @param biasX x-coordinate of accelerometer bias.
2315 * @param biasY y-coordinate of accelerometer bias.
2316 * @param biasZ z-coordinate of accelerometer bias.
2317 * @throws IllegalArgumentException if provided gravity norm value is negative.
2318 */
2319 protected BaseBiasGravityNormAccelerometerCalibrator(
2320 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final Acceleration biasX,
2321 final Acceleration biasY, final Acceleration biasZ) {
2322 this(commonAxisUsed, biasX, biasY, biasZ);
2323 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2324 }
2325
2326 /**
2327 * Constructor.
2328 *
2329 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2330 * squared second (m/s^2).
2331 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2332 * accelerometer and gyroscope.
2333 * @param biasX x-coordinate of accelerometer bias.
2334 * @param biasY y-coordinate of accelerometer bias.
2335 * @param biasZ z-coordinate of accelerometer bias.
2336 * @param listener listener to handle events raised by this calibrator.
2337 * @throws IllegalArgumentException if provided gravity norm value is negative.
2338 */
2339 protected BaseBiasGravityNormAccelerometerCalibrator(
2340 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final Acceleration biasX,
2341 final Acceleration biasY, final Acceleration biasZ, final L listener) {
2342 this(commonAxisUsed, biasX, biasY, biasZ, listener);
2343 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2344 }
2345
2346 /**
2347 * Constructor.
2348 *
2349 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2350 * squared second (m/s^2).
2351 * @param measurements collection of body kinematics measurements with standard
2352 * deviations taken at the same position with zero velocity
2353 * and unknown different orientations.
2354 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2355 * accelerometer and gyroscope.
2356 * @param biasX x-coordinate of accelerometer bias.
2357 * @param biasY y-coordinate of accelerometer bias.
2358 * @param biasZ z-coordinate of accelerometer bias.
2359 * @throws IllegalArgumentException if provided gravity norm value is negative.
2360 */
2361 protected BaseBiasGravityNormAccelerometerCalibrator(
2362 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2363 final boolean commonAxisUsed,
2364 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) {
2365 this(measurements, commonAxisUsed, biasX, biasY, biasZ);
2366 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2367 }
2368
2369 /**
2370 * Constructor.
2371 *
2372 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2373 * squared second (m/s^2).
2374 * @param measurements collection of body kinematics measurements with standard
2375 * deviations taken at the same position with zero velocity
2376 * and unknown different orientations.
2377 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2378 * accelerometer and gyroscope.
2379 * @param biasX x-coordinate of accelerometer bias.
2380 * @param biasY y-coordinate of accelerometer bias.
2381 * @param biasZ z-coordinate of accelerometer bias.
2382 * @param listener listener to handle events raised by this calibrator.
2383 * @throws IllegalArgumentException if provided gravity norm value is negative.
2384 */
2385 protected BaseBiasGravityNormAccelerometerCalibrator(
2386 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2387 final boolean commonAxisUsed, final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
2388 final L listener) {
2389 this(measurements, commonAxisUsed, biasX, biasY, biasZ, listener);
2390 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2391 }
2392
2393 /**
2394 * Constructor.
2395 *
2396 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2397 * squared second (m/s^2).
2398 * @param biasX x-coordinate of accelerometer bias.
2399 * This is expressed in meters per squared second (m/s^2).
2400 * @param biasY y-coordinate of accelerometer bias.
2401 * This is expressed in meters per squared second (m/s^2).
2402 * @param biasZ z-coordinate of accelerometer bias.
2403 * This is expressed in meters per squared second (m/s^2).
2404 * @param initialSx initial x scaling factor.
2405 * @param initialSy initial y scaling factor.
2406 * @param initialSz initial z scaling factor.
2407 * @throws IllegalArgumentException if provided gravity norm value is negative.
2408 */
2409 protected BaseBiasGravityNormAccelerometerCalibrator(
2410 final Double groundTruthGravityNorm, final double biasX, final double biasY, final double biasZ,
2411 final double initialSx, final double initialSy, final double initialSz) {
2412 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz);
2413 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2414 }
2415
2416 /**
2417 * Constructor.
2418 *
2419 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2420 * squared second (m/s^2).
2421 * @param measurements collection of body kinematics measurements with standard
2422 * deviations taken at the same position with zero velocity
2423 * and unknown different orientations.
2424 * @param biasX x-coordinate of accelerometer bias.
2425 * This is expressed in meters per squared second (m/s^2).
2426 * @param biasY y-coordinate of accelerometer bias.
2427 * This is expressed in meters per squared second (m/s^2).
2428 * @param biasZ z-coordinate of accelerometer bias.
2429 * This is expressed in meters per squared second (m/s^2).
2430 * @param initialSx initial x scaling factor.
2431 * @param initialSy initial y scaling factor.
2432 * @param initialSz initial z scaling factor.
2433 * @throws IllegalArgumentException if provided gravity norm value is negative.
2434 */
2435 protected BaseBiasGravityNormAccelerometerCalibrator(
2436 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2437 final double biasX, final double biasY, final double biasZ,
2438 final double initialSx, final double initialSy, final double initialSz) {
2439 this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
2440 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2441 }
2442
2443 /**
2444 * Constructor.
2445 *
2446 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2447 * squared second (m/s^2).
2448 * @param measurements collection of body kinematics measurements with standard
2449 * deviations taken at the same position with zero velocity
2450 * and unknown different orientations.
2451 * @param biasX x-coordinate of accelerometer bias.
2452 * This is expressed in meters per squared second (m/s^2).
2453 * @param biasY y-coordinate of accelerometer bias.
2454 * This is expressed in meters per squared second (m/s^2).
2455 * @param biasZ z-coordinate of accelerometer bias.
2456 * This is expressed in meters per squared second (m/s^2).
2457 * @param initialSx initial x scaling factor.
2458 * @param initialSy initial y scaling factor.
2459 * @param initialSz initial z scaling factor.
2460 * @param listener listener to handle events raised by this calibrator.
2461 * @throws IllegalArgumentException if provided gravity norm value is negative.
2462 */
2463 protected BaseBiasGravityNormAccelerometerCalibrator(
2464 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2465 final double biasX, final double biasY, final double biasZ,
2466 final double initialSx, final double initialSy, final double initialSz, final L listener) {
2467 this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz, listener);
2468 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2469 }
2470
2471 /**
2472 * Constructor.
2473 *
2474 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2475 * squared second (m/s^2).
2476 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2477 * accelerometer and gyroscope.
2478 * @param biasX x-coordinate of accelerometer bias.
2479 * This is expressed in meters per squared second (m/s^2).
2480 * @param biasY y-coordinate of accelerometer bias.
2481 * This is expressed in meters per squared second (m/s^2).
2482 * @param biasZ z-coordinate of accelerometer bias.
2483 * This is expressed in meters per squared second (m/s^2).
2484 * @param initialSx initial x scaling factor.
2485 * @param initialSy initial y scaling factor.
2486 * @param initialSz initial z scaling factor.
2487 * @throws IllegalArgumentException if provided gravity norm value is negative.
2488 */
2489 protected BaseBiasGravityNormAccelerometerCalibrator(
2490 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final double biasX, final double biasY,
2491 final double biasZ, final double initialSx, final double initialSy, final double initialSz) {
2492 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
2493 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2494 }
2495
2496 /**
2497 * Constructor.
2498 *
2499 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2500 * squared second (m/s^2).
2501 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2502 * accelerometer and gyroscope.
2503 * @param biasX x-coordinate of accelerometer bias.
2504 * This is expressed in meters per squared second (m/s^2).
2505 * @param biasY y-coordinate of accelerometer bias.
2506 * This is expressed in meters per squared second (m/s^2).
2507 * @param biasZ z-coordinate of accelerometer bias.
2508 * This is expressed in meters per squared second (m/s^2).
2509 * @param initialSx initial x scaling factor.
2510 * @param initialSy initial y scaling factor.
2511 * @param initialSz initial z scaling factor.
2512 * @param listener listener to handle events raised by this calibrator.
2513 * @throws IllegalArgumentException if provided gravity norm value is negative.
2514 */
2515 protected BaseBiasGravityNormAccelerometerCalibrator(
2516 final Double groundTruthGravityNorm, final boolean commonAxisUsed,
2517 final double biasX, final double biasY, final double biasZ, final double initialSx, final double initialSy,
2518 final double initialSz, final L listener) {
2519 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, listener);
2520 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2521 }
2522
2523 /**
2524 * Constructor.
2525 *
2526 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2527 * squared second (m/s^2).
2528 * @param measurements collection of body kinematics measurements with standard
2529 * deviations taken at the same position with zero velocity
2530 * and unknown different orientations.
2531 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2532 * accelerometer and gyroscope.
2533 * @param biasX x-coordinate of accelerometer bias.
2534 * This is expressed in meters per squared second (m/s^2).
2535 * @param biasY y-coordinate of accelerometer bias.
2536 * This is expressed in meters per squared second (m/s^2).
2537 * @param biasZ z-coordinate of accelerometer bias.
2538 * This is expressed in meters per squared second (m/s^2).
2539 * @param initialSx initial x scaling factor.
2540 * @param initialSy initial y scaling factor.
2541 * @param initialSz initial z scaling factor.
2542 * @throws IllegalArgumentException if provided gravity norm value is negative.
2543 */
2544 protected BaseBiasGravityNormAccelerometerCalibrator(
2545 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2546 final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
2547 final double initialSx, final double initialSy, final double initialSz) {
2548 this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
2549 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2550 }
2551
2552 /**
2553 * Constructor.
2554 *
2555 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2556 * squared second (m/s^2).
2557 * @param measurements collection of body kinematics measurements with standard
2558 * deviations taken at the same position with zero velocity
2559 * and unknown different orientations.
2560 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2561 * accelerometer and gyroscope.
2562 * @param biasX x-coordinate of accelerometer bias.
2563 * This is expressed in meters per squared second (m/s^2).
2564 * @param biasY y-coordinate of accelerometer bias.
2565 * This is expressed in meters per squared second (m/s^2).
2566 * @param biasZ z-coordinate of accelerometer bias.
2567 * This is expressed in meters per squared second (m/s^2).
2568 * @param initialSx initial x scaling factor.
2569 * @param initialSy initial y scaling factor.
2570 * @param initialSz initial z scaling factor.
2571 * @param listener listener to handle events raised by this calibrator.
2572 * @throws IllegalArgumentException if provided gravity norm value is negative.
2573 */
2574 protected BaseBiasGravityNormAccelerometerCalibrator(
2575 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2576 final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
2577 final double initialSx, final double initialSy, final double initialSz, final L listener) {
2578 this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, listener);
2579 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2580 }
2581
2582 /**
2583 * Constructor.
2584 *
2585 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2586 * squared second (m/s^2).
2587 * @param biasX x-coordinate of accelerometer bias.
2588 * @param biasY y-coordinate of accelerometer bias.
2589 * @param biasZ z-coordinate of accelerometer bias.
2590 * @param initialSx initial x scaling factor.
2591 * @param initialSy initial y scaling factor.
2592 * @param initialSz initial z scaling factor.
2593 * @throws IllegalArgumentException if provided gravity norm value is negative.
2594 */
2595 protected BaseBiasGravityNormAccelerometerCalibrator(
2596 final Double groundTruthGravityNorm, final Acceleration biasX, final Acceleration biasY,
2597 final Acceleration biasZ, final double initialSx, final double initialSy, final double initialSz) {
2598 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz);
2599 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2600 }
2601
2602 /**
2603 * Constructor.
2604 *
2605 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2606 * squared second (m/s^2).
2607 * @param biasX x-coordinate of accelerometer bias.
2608 * @param biasY y-coordinate of accelerometer bias.
2609 * @param biasZ z-coordinate of accelerometer bias.
2610 * @param initialSx initial x scaling factor.
2611 * @param initialSy initial y scaling factor.
2612 * @param initialSz initial z scaling factor.
2613 * @param listener listener to handle events raised by this calibrator.
2614 * @throws IllegalArgumentException if provided gravity norm value is negative.
2615 */
2616 protected BaseBiasGravityNormAccelerometerCalibrator(
2617 final Double groundTruthGravityNorm, final Acceleration biasX, final Acceleration biasY,
2618 final Acceleration biasZ, final double initialSx, final double initialSy, final double initialSz,
2619 final L listener) {
2620 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz, listener);
2621 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2622 }
2623
2624 /**
2625 * Constructor.
2626 *
2627 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2628 * squared second (m/s^2).
2629 * @param measurements collection of body kinematics measurements with standard
2630 * deviations taken at the same position with zero velocity
2631 * and unknown different orientations.
2632 * @param biasX x-coordinate of accelerometer bias.
2633 * @param biasY y-coordinate of accelerometer bias.
2634 * @param biasZ z-coordinate of accelerometer bias.
2635 * @param initialSx initial x scaling factor.
2636 * @param initialSy initial y scaling factor.
2637 * @param initialSz initial z scaling factor.
2638 * @throws IllegalArgumentException if provided gravity norm value is negative.
2639 */
2640 protected BaseBiasGravityNormAccelerometerCalibrator(
2641 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2642 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final double initialSx,
2643 final double initialSy, final double initialSz) {
2644 this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
2645 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2646 }
2647
2648 /**
2649 * Constructor.
2650 *
2651 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2652 * squared second (m/s^2).
2653 * @param measurements collection of body kinematics measurements with standard
2654 * deviations taken at the same position with zero velocity
2655 * and unknown different orientations.
2656 * @param biasX x-coordinate of accelerometer bias.
2657 * @param biasY y-coordinate of accelerometer bias.
2658 * @param biasZ z-coordinate of accelerometer bias.
2659 * @param initialSx initial x scaling factor.
2660 * @param initialSy initial y scaling factor.
2661 * @param initialSz initial z scaling factor.
2662 * @param listener listener to handle events raised by this calibrator.
2663 * @throws IllegalArgumentException if provided gravity norm value is negative.
2664 */
2665 protected BaseBiasGravityNormAccelerometerCalibrator(
2666 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2667 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final double initialSx,
2668 final double initialSy, final double initialSz, final L listener) {
2669 this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz, listener);
2670 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2671 }
2672
2673 /**
2674 * Constructor.
2675 *
2676 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2677 * squared second (m/s^2).
2678 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2679 * accelerometer and gyroscope.
2680 * @param biasX x-coordinate of accelerometer bias.
2681 * @param biasY y-coordinate of accelerometer bias.
2682 * @param biasZ z-coordinate of accelerometer bias.
2683 * @param initialSx initial x scaling factor.
2684 * @param initialSy initial y scaling factor.
2685 * @param initialSz initial z scaling factor.
2686 * @throws IllegalArgumentException if provided gravity norm value is negative.
2687 */
2688 protected BaseBiasGravityNormAccelerometerCalibrator(
2689 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final Acceleration biasX,
2690 final Acceleration biasY, final Acceleration biasZ,
2691 final double initialSx, final double initialSy, final double initialSz) {
2692 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
2693 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2694 }
2695
2696 /**
2697 * Constructor.
2698 *
2699 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2700 * squared second (m/s^2).
2701 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2702 * accelerometer and gyroscope.
2703 * @param biasX x-coordinate of accelerometer bias.
2704 * @param biasY y-coordinate of accelerometer bias.
2705 * @param biasZ z-coordinate of accelerometer bias.
2706 * @param initialSx initial x scaling factor.
2707 * @param initialSy initial y scaling factor.
2708 * @param initialSz initial z scaling factor.
2709 * @param listener listener to handle events raised by this calibrator.
2710 * @throws IllegalArgumentException if provided gravity norm value is negative.
2711 */
2712 protected BaseBiasGravityNormAccelerometerCalibrator(
2713 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final Acceleration biasX,
2714 final Acceleration biasY, final Acceleration biasZ, final double initialSx, final double initialSy,
2715 final double initialSz, final L listener) {
2716 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, listener);
2717 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2718 }
2719
2720 /**
2721 * Constructor.
2722 *
2723 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2724 * squared second (m/s^2).
2725 * @param measurements collection of body kinematics measurements with standard
2726 * deviations taken at the same position with zero velocity
2727 * and unknown different orientations.
2728 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2729 * accelerometer and gyroscope.
2730 * @param biasX x-coordinate of accelerometer bias.
2731 * @param biasY y-coordinate of accelerometer bias.
2732 * @param biasZ z-coordinate of accelerometer bias.
2733 * @param initialSx initial x scaling factor.
2734 * @param initialSy initial y scaling factor.
2735 * @param initialSz initial z scaling factor.
2736 * @throws IllegalArgumentException if provided gravity norm value is negative.
2737 */
2738 protected BaseBiasGravityNormAccelerometerCalibrator(
2739 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2740 final boolean commonAxisUsed, final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
2741 final double initialSx, final double initialSy, final double initialSz) {
2742 this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz);
2743 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2744 }
2745
2746 /**
2747 * Constructor.
2748 *
2749 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2750 * squared second (m/s^2).
2751 * @param measurements collection of body kinematics measurements with standard
2752 * deviations taken at the same position with zero velocity
2753 * and unknown different orientations.
2754 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2755 * accelerometer and gyroscope.
2756 * @param biasX x-coordinate of accelerometer bias.
2757 * @param biasY y-coordinate of accelerometer bias.
2758 * @param biasZ z-coordinate of accelerometer bias.
2759 * @param initialSx initial x scaling factor.
2760 * @param initialSy initial y scaling factor.
2761 * @param initialSz initial z scaling factor.
2762 * @param listener listener to handle events raised by this calibrator.
2763 * @throws IllegalArgumentException if provided gravity norm value is negative.
2764 */
2765 protected BaseBiasGravityNormAccelerometerCalibrator(
2766 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2767 final boolean commonAxisUsed, final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
2768 final double initialSx, final double initialSy, final double initialSz, final L listener) {
2769 this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, listener);
2770 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2771 }
2772
2773 /**
2774 * Constructor.
2775 *
2776 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2777 * squared second (m/s^2).
2778 * @param biasX x-coordinate of accelerometer bias.
2779 * This is expressed in meters per squared second (m/s^2).
2780 * @param biasY y-coordinate of accelerometer bias.
2781 * This is expressed in meters per squared second (m/s^2).
2782 * @param biasZ z-coordinate of accelerometer bias.
2783 * This is expressed in meters per squared second (m/s^2).
2784 * @param initialSx initial x scaling factor.
2785 * @param initialSy initial y scaling factor.
2786 * @param initialSz initial z scaling factor.
2787 * @param initialMxy initial x-y cross coupling error.
2788 * @param initialMxz initial x-z cross coupling error.
2789 * @param initialMyx initial y-x cross coupling error.
2790 * @param initialMyz initial y-z cross coupling error.
2791 * @param initialMzx initial z-x cross coupling error.
2792 * @param initialMzy initial z-y cross coupling error.
2793 * @throws IllegalArgumentException if provided gravity norm value is negative.
2794 */
2795 protected BaseBiasGravityNormAccelerometerCalibrator(
2796 final Double groundTruthGravityNorm, final double biasX, final double biasY, final double biasZ,
2797 final double initialSx, final double initialSy, final double initialSz, final double initialMxy,
2798 final double initialMxz, final double initialMyx, final double initialMyz, final double initialMzx,
2799 final double initialMzy) {
2800 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx, initialMyz,
2801 initialMzx, initialMzy);
2802 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2803 }
2804
2805 /**
2806 * Constructor.
2807 *
2808 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2809 * squared second (m/s^2).
2810 * @param measurements collection of body kinematics measurements with standard
2811 * deviations taken at the same position with zero velocity
2812 * and unknown different orientations.
2813 * @param biasX x-coordinate of accelerometer bias.
2814 * This is expressed in meters per squared second (m/s^2).
2815 * @param biasY y-coordinate of accelerometer bias.
2816 * This is expressed in meters per squared second (m/s^2).
2817 * @param biasZ z-coordinate of accelerometer bias.
2818 * This is expressed in meters per squared second (m/s^2).
2819 * @param initialSx initial x scaling factor.
2820 * @param initialSy initial y scaling factor.
2821 * @param initialSz initial z scaling factor.
2822 * @param initialMxy initial x-y cross coupling error.
2823 * @param initialMxz initial x-z cross coupling error.
2824 * @param initialMyx initial y-x cross coupling error.
2825 * @param initialMyz initial y-z cross coupling error.
2826 * @param initialMzx initial z-x cross coupling error.
2827 * @param initialMzy initial z-y cross coupling error.
2828 * @throws IllegalArgumentException if provided gravity norm value is negative.
2829 */
2830 protected BaseBiasGravityNormAccelerometerCalibrator(
2831 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2832 final double biasX, final double biasY, final double biasZ, final double initialSx, final double initialSy,
2833 final double initialSz, final double initialMxy, final double initialMxz, final double initialMyx,
2834 final double initialMyz, final double initialMzx, final double initialMzy) {
2835 this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
2836 initialMyz, initialMzx, initialMzy);
2837 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2838 }
2839
2840 /**
2841 * Constructor.
2842 *
2843 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2844 * squared second (m/s^2).
2845 * @param measurements collection of body kinematics measurements with standard
2846 * deviations taken at the same position with zero velocity
2847 * and unknown different orientations.
2848 * @param biasX x-coordinate of accelerometer bias.
2849 * This is expressed in meters per squared second (m/s^2).
2850 * @param biasY y-coordinate of accelerometer bias.
2851 * This is expressed in meters per squared second (m/s^2).
2852 * @param biasZ z-coordinate of accelerometer bias.
2853 * This is expressed in meters per squared second (m/s^2).
2854 * @param initialSx initial x scaling factor.
2855 * @param initialSy initial y scaling factor.
2856 * @param initialSz initial z scaling factor.
2857 * @param initialMxy initial x-y cross coupling error.
2858 * @param initialMxz initial x-z cross coupling error.
2859 * @param initialMyx initial y-x cross coupling error.
2860 * @param initialMyz initial y-z cross coupling error.
2861 * @param initialMzx initial z-x cross coupling error.
2862 * @param initialMzy initial z-y cross coupling error.
2863 * @param listener listener to handle events raised by this calibrator.
2864 * @throws IllegalArgumentException if provided gravity norm value is negative.
2865 */
2866 protected BaseBiasGravityNormAccelerometerCalibrator(
2867 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2868 final double biasX, final double biasY, final double biasZ, final double initialSx, final double initialSy,
2869 final double initialSz, final double initialMxy, final double initialMxz, final double initialMyx,
2870 final double initialMyz, final double initialMzx, final double initialMzy,
2871 final L listener) {
2872 this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
2873 initialMyz, initialMzx, initialMzy, listener);
2874 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2875 }
2876
2877 /**
2878 * Constructor.
2879 *
2880 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2881 * squared second (m/s^2).
2882 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2883 * accelerometer and gyroscope.
2884 * @param biasX x-coordinate of accelerometer bias.
2885 * This is expressed in meters per squared second (m/s^2).
2886 * @param biasY y-coordinate of accelerometer bias.
2887 * This is expressed in meters per squared second (m/s^2).
2888 * @param biasZ z-coordinate of accelerometer bias.
2889 * This is expressed in meters per squared second (m/s^2).
2890 * @param initialSx initial x scaling factor.
2891 * @param initialSy initial y scaling factor.
2892 * @param initialSz initial z scaling factor.
2893 * @param initialMxy initial x-y cross coupling error.
2894 * @param initialMxz initial x-z cross coupling error.
2895 * @param initialMyx initial y-x cross coupling error.
2896 * @param initialMyz initial y-z cross coupling error.
2897 * @param initialMzx initial z-x cross coupling error.
2898 * @param initialMzy initial z-y cross coupling error.
2899 * @throws IllegalArgumentException if provided gravity norm value is negative.
2900 */
2901 protected BaseBiasGravityNormAccelerometerCalibrator(
2902 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final double biasX, final double biasY,
2903 final double biasZ, final double initialSx, final double initialSy, final double initialSz,
2904 final double initialMxy, final double initialMxz, final double initialMyx, final double initialMyz,
2905 final double initialMzx, final double initialMzy) {
2906 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
2907 initialMyz, initialMzx, initialMzy);
2908 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2909 }
2910
2911 /**
2912 * Constructor.
2913 *
2914 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2915 * squared second (m/s^2).
2916 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2917 * accelerometer and gyroscope.
2918 * @param biasX x-coordinate of accelerometer bias.
2919 * This is expressed in meters per squared second (m/s^2).
2920 * @param biasY y-coordinate of accelerometer bias.
2921 * This is expressed in meters per squared second (m/s^2).
2922 * @param biasZ z-coordinate of accelerometer bias.
2923 * This is expressed in meters per squared second (m/s^2).
2924 * @param initialSx initial x scaling factor.
2925 * @param initialSy initial y scaling factor.
2926 * @param initialSz initial z scaling factor.
2927 * @param initialMxy initial x-y cross coupling error.
2928 * @param initialMxz initial x-z cross coupling error.
2929 * @param initialMyx initial y-x cross coupling error.
2930 * @param initialMyz initial y-z cross coupling error.
2931 * @param initialMzx initial z-x cross coupling error.
2932 * @param initialMzy initial z-y cross coupling error.
2933 * @param listener listener to handle events raised by this calibrator.
2934 * @throws IllegalArgumentException if provided gravity norm value is negative.
2935 */
2936 protected BaseBiasGravityNormAccelerometerCalibrator(
2937 final Double groundTruthGravityNorm, final boolean commonAxisUsed,
2938 final double biasX, final double biasY, final double biasZ, final double initialSx, final double initialSy,
2939 final double initialSz, final double initialMxy, final double initialMxz, final double initialMyx,
2940 final double initialMyz, final double initialMzx, final double initialMzy, final L listener) {
2941 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
2942 initialMyz, initialMzx, initialMzy, listener);
2943 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2944 }
2945
2946 /**
2947 * Constructor.
2948 *
2949 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2950 * squared second (m/s^2).
2951 * @param measurements collection of body kinematics measurements with standard
2952 * deviations taken at the same position with zero velocity
2953 * and unknown different orientations.
2954 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2955 * accelerometer and gyroscope.
2956 * @param biasX x-coordinate of accelerometer bias.
2957 * This is expressed in meters per squared second (m/s^2).
2958 * @param biasY y-coordinate of accelerometer bias.
2959 * This is expressed in meters per squared second (m/s^2).
2960 * @param biasZ z-coordinate of accelerometer bias.
2961 * This is expressed in meters per squared second (m/s^2).
2962 * @param initialSx initial x scaling factor.
2963 * @param initialSy initial y scaling factor.
2964 * @param initialSz initial z scaling factor.
2965 * @param initialMxy initial x-y cross coupling error.
2966 * @param initialMxz initial x-z cross coupling error.
2967 * @param initialMyx initial y-x cross coupling error.
2968 * @param initialMyz initial y-z cross coupling error.
2969 * @param initialMzx initial z-x cross coupling error.
2970 * @param initialMzy initial z-y cross coupling error.
2971 * @throws IllegalArgumentException if provided gravity norm value is negative.
2972 */
2973 protected BaseBiasGravityNormAccelerometerCalibrator(
2974 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
2975 final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
2976 final double initialSx, final double initialSy, final double initialSz, final double initialMxy,
2977 final double initialMxz, final double initialMyx, final double initialMyz, final double initialMzx,
2978 final double initialMzy) {
2979 this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz,
2980 initialMyx, initialMyz, initialMzx, initialMzy);
2981 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
2982 }
2983
2984 /**
2985 * Constructor.
2986 *
2987 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
2988 * squared second (m/s^2).
2989 * @param measurements collection of body kinematics measurements with standard
2990 * deviations taken at the same position with zero velocity
2991 * and unknown different orientations.
2992 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
2993 * accelerometer and gyroscope.
2994 * @param biasX x-coordinate of accelerometer bias.
2995 * This is expressed in meters per squared second (m/s^2).
2996 * @param biasY y-coordinate of accelerometer bias.
2997 * This is expressed in meters per squared second (m/s^2).
2998 * @param biasZ z-coordinate of accelerometer bias.
2999 * This is expressed in meters per squared second (m/s^2).
3000 * @param initialSx initial x scaling factor.
3001 * @param initialSy initial y scaling factor.
3002 * @param initialSz initial z scaling factor.
3003 * @param initialMxy initial x-y cross coupling error.
3004 * @param initialMxz initial x-z cross coupling error.
3005 * @param initialMyx initial y-x cross coupling error.
3006 * @param initialMyz initial y-z cross coupling error.
3007 * @param initialMzx initial z-x cross coupling error.
3008 * @param initialMzy initial z-y cross coupling error.
3009 * @param listener listener to handle events raised by this calibrator.
3010 * @throws IllegalArgumentException if provided gravity norm value is negative.
3011 */
3012 protected BaseBiasGravityNormAccelerometerCalibrator(
3013 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3014 final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
3015 final double initialSx, final double initialSy, final double initialSz, final double initialMxy,
3016 final double initialMxz, final double initialMyx, final double initialMyz, final double initialMzx,
3017 final double initialMzy, final L listener) {
3018 this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy,
3019 initialMxz, initialMyx, initialMyz, initialMzx, initialMzy, listener);
3020 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3021 }
3022
3023 /**
3024 * Constructor.
3025 *
3026 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3027 * squared second (m/s^2).
3028 * @param biasX x-coordinate of accelerometer bias.
3029 * @param biasY y-coordinate of accelerometer bias.
3030 * @param biasZ z-coordinate of accelerometer bias.
3031 * @param initialSx initial x scaling factor.
3032 * @param initialSy initial y scaling factor.
3033 * @param initialSz initial z scaling factor.
3034 * @param initialMxy initial x-y cross coupling error.
3035 * @param initialMxz initial x-z cross coupling error.
3036 * @param initialMyx initial y-x cross coupling error.
3037 * @param initialMyz initial y-z cross coupling error.
3038 * @param initialMzx initial z-x cross coupling error.
3039 * @param initialMzy initial z-y cross coupling error.
3040 * @throws IllegalArgumentException if provided gravity norm value is negative.
3041 */
3042 protected BaseBiasGravityNormAccelerometerCalibrator(
3043 final Double groundTruthGravityNorm, final Acceleration biasX, final Acceleration biasY,
3044 final Acceleration biasZ, final double initialSx, final double initialSy, final double initialSz,
3045 final double initialMxy, final double initialMxz, final double initialMyx, final double initialMyz,
3046 final double initialMzx, final double initialMzy) {
3047 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
3048 initialMyz, initialMzx, initialMzy);
3049 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3050 }
3051
3052 /**
3053 * Constructor.
3054 *
3055 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3056 * squared second (m/s^2).
3057 * @param biasX x-coordinate of accelerometer bias.
3058 * @param biasY y-coordinate of accelerometer bias.
3059 * @param biasZ z-coordinate of accelerometer bias.
3060 * @param initialSx initial x scaling factor.
3061 * @param initialSy initial y scaling factor.
3062 * @param initialSz initial z scaling factor.
3063 * @param initialMxy initial x-y cross coupling error.
3064 * @param initialMxz initial x-z cross coupling error.
3065 * @param initialMyx initial y-x cross coupling error.
3066 * @param initialMyz initial y-z cross coupling error.
3067 * @param initialMzx initial z-x cross coupling error.
3068 * @param initialMzy initial z-y cross coupling error.
3069 * @param listener listener to handle events raised by this calibrator.
3070 * @throws IllegalArgumentException if provided gravity norm value is negative.
3071 */
3072 protected BaseBiasGravityNormAccelerometerCalibrator(
3073 final Double groundTruthGravityNorm, final Acceleration biasX, final Acceleration biasY,
3074 final Acceleration biasZ, final double initialSx, final double initialSy, final double initialSz,
3075 final double initialMxy, final double initialMxz, final double initialMyx, final double initialMyz,
3076 final double initialMzx, final double initialMzy, final L listener) {
3077 this(biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx, initialMyz,
3078 initialMzx, initialMzy, listener);
3079 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3080 }
3081
3082 /**
3083 * Constructor.
3084 *
3085 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3086 * squared second (m/s^2).
3087 * @param measurements collection of body kinematics measurements with standard
3088 * deviations taken at the same position with zero velocity
3089 * and unknown different orientations.
3090 * @param biasX x-coordinate of accelerometer bias.
3091 * @param biasY y-coordinate of accelerometer bias.
3092 * @param biasZ z-coordinate of accelerometer bias.
3093 * @param initialSx initial x scaling factor.
3094 * @param initialSy initial y scaling factor.
3095 * @param initialSz initial z scaling factor.
3096 * @param initialMxy initial x-y cross coupling error.
3097 * @param initialMxz initial x-z cross coupling error.
3098 * @param initialMyx initial y-x cross coupling error.
3099 * @param initialMyz initial y-z cross coupling error.
3100 * @param initialMzx initial z-x cross coupling error.
3101 * @param initialMzy initial z-y cross coupling error.
3102 * @throws IllegalArgumentException if provided gravity norm value is negative.
3103 */
3104 protected BaseBiasGravityNormAccelerometerCalibrator(
3105 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3106 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final double initialSx,
3107 final double initialSy, final double initialSz, final double initialMxy, final double initialMxz,
3108 final double initialMyx, final double initialMyz, final double initialMzx, final double initialMzy) {
3109 this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
3110 initialMyz, initialMzx, initialMzy);
3111 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3112 }
3113
3114 /**
3115 * Constructor.
3116 *
3117 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3118 * squared second (m/s^2).
3119 * @param measurements collection of body kinematics measurements with standard
3120 * deviations taken at the same position with zero velocity
3121 * and unknown different orientations.
3122 * @param biasX x-coordinate of accelerometer bias.
3123 * @param biasY y-coordinate of accelerometer bias.
3124 * @param biasZ z-coordinate of accelerometer bias.
3125 * @param initialSx initial x scaling factor.
3126 * @param initialSy initial y scaling factor.
3127 * @param initialSz initial z scaling factor.
3128 * @param initialMxy initial x-y cross coupling error.
3129 * @param initialMxz initial x-z cross coupling error.
3130 * @param initialMyx initial y-x cross coupling error.
3131 * @param initialMyz initial y-z cross coupling error.
3132 * @param initialMzx initial z-x cross coupling error.
3133 * @param initialMzy initial z-y cross coupling error.
3134 * @param listener listener to handle events raised by this calibrator.
3135 * @throws IllegalArgumentException if provided gravity norm value is negative.
3136 */
3137 protected BaseBiasGravityNormAccelerometerCalibrator(
3138 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3139 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final double initialSx,
3140 final double initialSy, final double initialSz, final double initialMxy,
3141 final double initialMxz, final double initialMyx, final double initialMyz,
3142 final double initialMzx, final double initialMzy, final L listener) {
3143 this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
3144 initialMyz, initialMzx, initialMzy, listener);
3145 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3146 }
3147
3148 /**
3149 * Constructor.
3150 *
3151 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3152 * squared second (m/s^2).
3153 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3154 * accelerometer and gyroscope.
3155 * @param biasX x-coordinate of accelerometer bias.
3156 * @param biasY y-coordinate of accelerometer bias.
3157 * @param biasZ z-coordinate of accelerometer bias.
3158 * @param initialSx initial x scaling factor.
3159 * @param initialSy initial y scaling factor.
3160 * @param initialSz initial z scaling factor.
3161 * @param initialMxy initial x-y cross coupling error.
3162 * @param initialMxz initial x-z cross coupling error.
3163 * @param initialMyx initial y-x cross coupling error.
3164 * @param initialMyz initial y-z cross coupling error.
3165 * @param initialMzx initial z-x cross coupling error.
3166 * @param initialMzy initial z-y cross coupling error.
3167 * @throws IllegalArgumentException if provided gravity norm value is negative.
3168 */
3169 protected BaseBiasGravityNormAccelerometerCalibrator(
3170 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final Acceleration biasX,
3171 final Acceleration biasY, final Acceleration biasZ, final double initialSx, final double initialSy,
3172 final double initialSz, final double initialMxy, final double initialMxz, final double initialMyx,
3173 final double initialMyz, final double initialMzx, final double initialMzy) {
3174 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
3175 initialMyz, initialMzx, initialMzy);
3176 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3177 }
3178
3179 /**
3180 * Constructor.
3181 *
3182 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3183 * squared second (m/s^2).
3184 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3185 * accelerometer and gyroscope.
3186 * @param biasX x-coordinate of accelerometer bias.
3187 * @param biasY y-coordinate of accelerometer bias.
3188 * @param biasZ z-coordinate of accelerometer bias.
3189 * @param initialSx initial x scaling factor.
3190 * @param initialSy initial y scaling factor.
3191 * @param initialSz initial z scaling factor.
3192 * @param initialMxy initial x-y cross coupling error.
3193 * @param initialMxz initial x-z cross coupling error.
3194 * @param initialMyx initial y-x cross coupling error.
3195 * @param initialMyz initial y-z cross coupling error.
3196 * @param initialMzx initial z-x cross coupling error.
3197 * @param initialMzy initial z-y cross coupling error.
3198 * @param listener listener to handle events raised by this calibrator.
3199 * @throws IllegalArgumentException if provided gravity norm value is negative.
3200 */
3201 protected BaseBiasGravityNormAccelerometerCalibrator(
3202 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final Acceleration biasX,
3203 final Acceleration biasY, final Acceleration biasZ,
3204 final double initialSx, final double initialSy, final double initialSz,
3205 final double initialMxy, final double initialMxz, final double initialMyx,
3206 final double initialMyz, final double initialMzx, final double initialMzy, final L listener) {
3207 this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
3208 initialMyz, initialMzx, initialMzy, listener);
3209 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3210 }
3211
3212 /**
3213 * Constructor.
3214 *
3215 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3216 * squared second (m/s^2).
3217 * @param measurements collection of body kinematics measurements with standard
3218 * deviations taken at the same position with zero velocity
3219 * and unknown different orientations.
3220 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3221 * accelerometer and gyroscope.
3222 * @param biasX x-coordinate of accelerometer bias.
3223 * @param biasY y-coordinate of accelerometer bias.
3224 * @param biasZ z-coordinate of accelerometer bias.
3225 * @param initialSx initial x scaling factor.
3226 * @param initialSy initial y scaling factor.
3227 * @param initialSz initial z scaling factor.
3228 * @param initialMxy initial x-y cross coupling error.
3229 * @param initialMxz initial x-z cross coupling error.
3230 * @param initialMyx initial y-x cross coupling error.
3231 * @param initialMyz initial y-z cross coupling error.
3232 * @param initialMzx initial z-x cross coupling error.
3233 * @param initialMzy initial z-y cross coupling error.
3234 * @throws IllegalArgumentException if provided gravity norm value is negative.
3235 */
3236 protected BaseBiasGravityNormAccelerometerCalibrator(
3237 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3238 final boolean commonAxisUsed, final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
3239 final double initialSx, final double initialSy, final double initialSz,
3240 final double initialMxy, final double initialMxz, final double initialMyx,
3241 final double initialMyz, final double initialMzx, final double initialMzy) {
3242 this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy,
3243 initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
3244 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3245 }
3246
3247 /**
3248 * Constructor.
3249 *
3250 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3251 * squared second (m/s^2).
3252 * @param measurements collection of body kinematics measurements with standard
3253 * deviations taken at the same position with zero velocity
3254 * and unknown different orientations.
3255 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3256 * accelerometer and gyroscope.
3257 * @param biasX x-coordinate of accelerometer bias.
3258 * @param biasY y-coordinate of accelerometer bias.
3259 * @param biasZ z-coordinate of accelerometer bias.
3260 * @param initialSx initial x scaling factor.
3261 * @param initialSy initial y scaling factor.
3262 * @param initialSz initial z scaling factor.
3263 * @param initialMxy initial x-y cross coupling error.
3264 * @param initialMxz initial x-z cross coupling error.
3265 * @param initialMyx initial y-x cross coupling error.
3266 * @param initialMyz initial y-z cross coupling error.
3267 * @param initialMzx initial z-x cross coupling error.
3268 * @param initialMzy initial z-y cross coupling error.
3269 * @param listener listener to handle events raised by this calibrator.
3270 * @throws IllegalArgumentException if provided gravity norm value is negative.
3271 */
3272 protected BaseBiasGravityNormAccelerometerCalibrator(
3273 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3274 final boolean commonAxisUsed, final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
3275 final double initialSx, final double initialSy, final double initialSz,
3276 final double initialMxy, final double initialMxz, final double initialMyx,
3277 final double initialMyz, final double initialMzx, final double initialMzy, final L listener) {
3278 this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy,
3279 initialMxz, initialMyx, initialMyz, initialMzx, initialMzy, listener);
3280 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3281 }
3282
3283 /**
3284 * Constructor.
3285 *
3286 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3287 * squared second (m/s^2).
3288 * @param bias known accelerometer bias. This must have length 3 and is expressed
3289 * in meters per squared second (m/s^2).
3290 * @throws IllegalArgumentException if provided bias array does not have length 3 or
3291 * if provided gravity norm value is negative.
3292 */
3293 protected BaseBiasGravityNormAccelerometerCalibrator(final Double groundTruthGravityNorm, final double[] bias) {
3294 this(bias);
3295 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3296 }
3297
3298 /**
3299 * Constructor.
3300 *
3301 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3302 * squared second (m/s^2).
3303 * @param bias known accelerometer bias. This must have length 3 and is expressed
3304 * in meters per squared second (m/s^2).
3305 * @param listener listener to handle events raised by this calibrator.
3306 * @throws IllegalArgumentException if provided bias array does not have length 3 or
3307 * if provided gravity norm value is negative.
3308 */
3309 protected BaseBiasGravityNormAccelerometerCalibrator(
3310 final Double groundTruthGravityNorm, final double[] bias, final L listener) {
3311 this(bias, listener);
3312 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3313 }
3314
3315 /**
3316 * Constructor.
3317 *
3318 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3319 * squared second (m/s^2).
3320 * @param measurements collection of body kinematics measurements with standard
3321 * deviations taken at the same position with zero velocity
3322 * and unknown different orientations.
3323 * @param bias known accelerometer bias. This must have length 3 and is expressed
3324 * in meters per squared second (m/s^2).
3325 * @throws IllegalArgumentException if provided bias array does not have length 3 or
3326 * if provided gravity norm value is negative.
3327 */
3328 protected BaseBiasGravityNormAccelerometerCalibrator(
3329 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3330 final double[] bias) {
3331 this(measurements, bias);
3332 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3333 }
3334
3335 /**
3336 * Constructor.
3337 *
3338 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3339 * squared second (m/s^2).
3340 * @param measurements collection of body kinematics measurements with standard
3341 * deviations taken at the same position with zero velocity
3342 * and unknown different orientations.
3343 * @param bias known accelerometer bias. This must have length 3 and is expressed
3344 * in meters per squared second (m/s^2).
3345 * @param listener listener to handle events raised by this calibrator.
3346 * @throws IllegalArgumentException if provided bias array does not have length 3 or
3347 * if provided gravity norm value is negative.
3348 */
3349 protected BaseBiasGravityNormAccelerometerCalibrator(
3350 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3351 final double[] bias, final L listener) {
3352 this(measurements, bias, listener);
3353 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3354 }
3355
3356 /**
3357 * Constructor.
3358 *
3359 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3360 * squared second (m/s^2).
3361 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3362 * accelerometer and gyroscope.
3363 * @param bias known accelerometer bias. This must have length 3 and is expressed
3364 * in meters per squared second (m/s^2).
3365 * @throws IllegalArgumentException if provided bias array does not have length 3 or
3366 * if provided gravity norm value is negative.
3367 */
3368 protected BaseBiasGravityNormAccelerometerCalibrator(
3369 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final double[] bias) {
3370 this(commonAxisUsed, bias);
3371 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3372 }
3373
3374 /**
3375 * Constructor.
3376 *
3377 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3378 * squared second (m/s^2).
3379 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3380 * accelerometer and gyroscope.
3381 * @param bias known accelerometer bias. This must have length 3 and is expressed
3382 * in meters per squared second (m/s^2).
3383 * @param listener listener to handle events raised by this calibrator.
3384 * @throws IllegalArgumentException if provided bias array does not have length 3 or
3385 * if provided gravity norm value is negative.
3386 */
3387 protected BaseBiasGravityNormAccelerometerCalibrator(
3388 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final double[] bias, final L listener) {
3389 this(commonAxisUsed, bias, listener);
3390 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3391 }
3392
3393 /**
3394 * Constructor.
3395 *
3396 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3397 * squared second (m/s^2).
3398 * @param measurements collection of body kinematics measurements with standard
3399 * deviations taken at the same position with zero velocity
3400 * and unknown different orientations.
3401 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3402 * accelerometer and gyroscope.
3403 * @param bias known accelerometer bias. This must have length 3 and is expressed
3404 * in meters per squared second (m/s^2).
3405 * @throws IllegalArgumentException if provided bias array does not have length 3 or
3406 * if provided gravity norm value is negative.
3407 */
3408 protected BaseBiasGravityNormAccelerometerCalibrator(
3409 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3410 final boolean commonAxisUsed, final double[] bias) {
3411 this(measurements, commonAxisUsed, bias);
3412 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3413 }
3414
3415 /**
3416 * Constructor.
3417 *
3418 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3419 * squared second (m/s^2).
3420 * @param measurements collection of body kinematics measurements with standard
3421 * deviations taken at the same position with zero velocity
3422 * and unknown different orientations.
3423 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3424 * accelerometer and gyroscope.
3425 * @param bias known accelerometer bias. This must have length 3 and is expressed
3426 * in meters per squared second (m/s^2).
3427 * @param listener listener to handle events raised by this calibrator.
3428 * @throws IllegalArgumentException if provided bias array does not have length 3 or
3429 * if provided gravity norm value is negative.
3430 */
3431 protected BaseBiasGravityNormAccelerometerCalibrator(
3432 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3433 final boolean commonAxisUsed, final double[] bias, final L listener) {
3434 this(measurements, commonAxisUsed, bias, listener);
3435 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3436 }
3437
3438 /**
3439 * Constructor.
3440 *
3441 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3442 * squared second (m/s^2).
3443 * @param bias known accelerometer bias.
3444 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
3445 * if provided gravity norm value is negative.
3446 */
3447 protected BaseBiasGravityNormAccelerometerCalibrator(final Double groundTruthGravityNorm, final Matrix bias) {
3448 this(bias);
3449 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3450 }
3451
3452 /**
3453 * Constructor.
3454 *
3455 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3456 * squared second (m/s^2).
3457 * @param bias known accelerometer bias.
3458 * @param listener listener to handle events raised by this calibrator.
3459 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
3460 * if provided gravity norm value is negative.
3461 */
3462 protected BaseBiasGravityNormAccelerometerCalibrator(
3463 final Double groundTruthGravityNorm, final Matrix bias, final L listener) {
3464 this(bias, listener);
3465 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3466 }
3467
3468 /**
3469 * Constructor.
3470 *
3471 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3472 * squared second (m/s^2).
3473 * @param measurements collection of body kinematics measurements with standard
3474 * deviations taken at the same position with zero velocity
3475 * and unknown different orientations.
3476 * @param bias known accelerometer bias.
3477 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
3478 * if provided gravity norm value is negative.
3479 */
3480 protected BaseBiasGravityNormAccelerometerCalibrator(
3481 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3482 final Matrix bias) {
3483 this(measurements, bias);
3484 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3485 }
3486
3487 /**
3488 * Constructor.
3489 *
3490 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3491 * squared second (m/s^2).
3492 * @param measurements collection of body kinematics measurements with standard
3493 * deviations taken at the same position with zero velocity
3494 * and unknown different orientations.
3495 * @param bias known accelerometer bias.
3496 * @param listener listener to handle events raised by this calibrator.
3497 * @throws IllegalArgumentException if provided gravity norm value is negative.
3498 */
3499 protected BaseBiasGravityNormAccelerometerCalibrator(
3500 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3501 final Matrix bias, final L listener) {
3502 this(measurements, bias, listener);
3503 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3504 }
3505
3506 /**
3507 * Constructor.
3508 *
3509 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3510 * squared second (m/s^2).
3511 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3512 * accelerometer and gyroscope.
3513 * @param bias known accelerometer bias.
3514 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
3515 * if provided gravity norm value is negative.
3516 */
3517 protected BaseBiasGravityNormAccelerometerCalibrator(
3518 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final Matrix bias) {
3519 this(commonAxisUsed, bias);
3520 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3521 }
3522
3523 /**
3524 * Constructor.
3525 *
3526 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3527 * squared second (m/s^2).
3528 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3529 * accelerometer and gyroscope.
3530 * @param bias known accelerometer bias.
3531 * @param listener listener to handle events raised by this calibrator.
3532 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
3533 * if provided gravity norm value is negative.
3534 */
3535 protected BaseBiasGravityNormAccelerometerCalibrator(
3536 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final Matrix bias, final L listener) {
3537 this(commonAxisUsed, bias, listener);
3538 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3539 }
3540
3541 /**
3542 * Constructor.
3543 *
3544 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3545 * squared second (m/s^2).
3546 * @param measurements collection of body kinematics measurements with standard
3547 * deviations taken at the same position with zero velocity
3548 * and unknown different orientations.
3549 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3550 * accelerometer and gyroscope.
3551 * @param bias known accelerometer bias.
3552 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
3553 * if provided gravity norm value is negative.
3554 */
3555 protected BaseBiasGravityNormAccelerometerCalibrator(
3556 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3557 final boolean commonAxisUsed, final Matrix bias) {
3558 this(measurements, commonAxisUsed, bias);
3559 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3560 }
3561
3562 /**
3563 * Constructor.
3564 *
3565 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3566 * squared second (m/s^2).
3567 * @param measurements collection of body kinematics measurements with standard
3568 * deviations taken at the same position with zero velocity
3569 * and unknown different orientations.
3570 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3571 * accelerometer and gyroscope.
3572 * @param bias known accelerometer bias.
3573 * @param listener listener to handle events raised by this calibrator.
3574 * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
3575 * if provided gravity norm value is negative.
3576 */
3577 protected BaseBiasGravityNormAccelerometerCalibrator(
3578 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3579 final boolean commonAxisUsed, final Matrix bias, final L listener) {
3580 this(measurements, commonAxisUsed, bias, listener);
3581 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3582 }
3583
3584 /**
3585 * Constructor.
3586 *
3587 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3588 * squared second (m/s^2).
3589 * @param bias known accelerometer bias.
3590 * @param initialMa initial scale factors and cross coupling errors matrix.
3591 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
3592 * scaling and coupling error matrix is not 3x3 or
3593 * if provided gravity norm value is negative.
3594 */
3595 protected BaseBiasGravityNormAccelerometerCalibrator(
3596 final Double groundTruthGravityNorm, final Matrix bias, final Matrix initialMa) {
3597 this(bias, initialMa);
3598 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3599 }
3600
3601 /**
3602 * Constructor.
3603 *
3604 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3605 * squared second (m/s^2).
3606 * @param bias known accelerometer bias.
3607 * @param initialMa initial scale factors and cross coupling errors matrix.
3608 * @param listener listener to handle events raised by this calibrator.
3609 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
3610 * scaling and coupling error matrix is not 3x3 or
3611 * if provided gravity norm value is negative.
3612 */
3613 protected BaseBiasGravityNormAccelerometerCalibrator(
3614 final Double groundTruthGravityNorm, final Matrix bias, final Matrix initialMa, final L listener) {
3615 this(bias, initialMa, listener);
3616 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3617 }
3618
3619 /**
3620 * Constructor.
3621 *
3622 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3623 * squared second (m/s^2).
3624 * @param measurements collection of body kinematics measurements with standard
3625 * deviations taken at the same position with zero velocity
3626 * and unknown different orientations.
3627 * @param bias known accelerometer bias.
3628 * @param initialMa initial scale factors and cross coupling errors matrix.
3629 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
3630 * scaling and coupling error matrix is not 3x3 or
3631 * if provided gravity norm value is negative.
3632 */
3633 protected BaseBiasGravityNormAccelerometerCalibrator(
3634 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3635 final Matrix bias, final Matrix initialMa) {
3636 this(measurements, bias, initialMa);
3637 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3638 }
3639
3640 /**
3641 * Constructor.
3642 *
3643 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3644 * squared second (m/s^2).
3645 * @param measurements collection of body kinematics measurements with standard
3646 * deviations taken at the same position with zero velocity
3647 * and unknown different orientations.
3648 * @param bias known accelerometer bias.
3649 * @param initialMa initial scale factors and cross coupling errors matrix.
3650 * @param listener listener to handle events raised by this calibrator.
3651 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
3652 * scaling and coupling error matrix is not 3x3 or
3653 * if provided gravity norm value is negative.
3654 */
3655 protected BaseBiasGravityNormAccelerometerCalibrator(
3656 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3657 final Matrix bias, final Matrix initialMa, final L listener) {
3658 this(measurements, bias, initialMa, listener);
3659 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3660 }
3661
3662 /**
3663 * Constructor.
3664 *
3665 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3666 * squared second (m/s^2).
3667 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3668 * accelerometer and gyroscope.
3669 * @param bias known accelerometer bias.
3670 * @param initialMa initial scale factors and cross coupling errors matrix.
3671 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
3672 * scaling and coupling error matrix is not 3x3 or
3673 * if provided gravity norm value is negative.
3674 */
3675 protected BaseBiasGravityNormAccelerometerCalibrator(
3676 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final Matrix bias,
3677 final Matrix initialMa) {
3678 this(commonAxisUsed, bias, initialMa);
3679 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3680 }
3681
3682 /**
3683 * Constructor.
3684 *
3685 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3686 * squared second (m/s^2).
3687 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3688 * accelerometer and gyroscope.
3689 * @param bias known accelerometer bias.
3690 * @param initialMa initial scale factors and cross coupling errors matrix.
3691 * @param listener listener to handle events raised by this calibrator.
3692 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
3693 * scaling and coupling error matrix is not 3x3 or
3694 * if provided gravity norm value is negative.
3695 */
3696 protected BaseBiasGravityNormAccelerometerCalibrator(
3697 final Double groundTruthGravityNorm, final boolean commonAxisUsed, final Matrix bias,
3698 final Matrix initialMa, final L listener) {
3699 this(commonAxisUsed, bias, initialMa, listener);
3700 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3701 }
3702
3703 /**
3704 * Constructor.
3705 *
3706 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3707 * squared second (m/s^2).
3708 * @param measurements collection of body kinematics measurements with standard
3709 * deviations taken at the same position with zero velocity
3710 * and unknown different orientations.
3711 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3712 * accelerometer and gyroscope.
3713 * @param bias known accelerometer bias.
3714 * @param initialMa initial scale factors and cross coupling errors matrix.
3715 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
3716 * scaling and coupling error matrix is not 3x3 or
3717 * if provided gravity norm value is negative.
3718 */
3719 protected BaseBiasGravityNormAccelerometerCalibrator(
3720 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3721 final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa) {
3722 this(measurements, commonAxisUsed, bias, initialMa);
3723 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3724 }
3725
3726 /**
3727 * Constructor.
3728 *
3729 * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
3730 * squared second (m/s^2).
3731 * @param measurements collection of body kinematics measurements with standard
3732 * deviations taken at the same position with zero velocity
3733 * and unknown different orientations.
3734 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
3735 * accelerometer and gyroscope.
3736 * @param bias known accelerometer bias.
3737 * @param initialMa initial scale factors and cross coupling errors matrix.
3738 * @param listener listener to handle events raised by this calibrator.
3739 * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
3740 * scaling and coupling error matrix is not 3x3 or
3741 * if provided gravity norm value is negative.
3742 */
3743 protected BaseBiasGravityNormAccelerometerCalibrator(
3744 final Double groundTruthGravityNorm, final Collection<StandardDeviationBodyKinematics> measurements,
3745 final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa, final L listener) {
3746 this(measurements, commonAxisUsed, bias, initialMa, listener);
3747 internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
3748 }
3749
3750 /**
3751 * Gets ground truth gravity norm to be expected at location where measurements have been made,
3752 * expressed in meter per squared second (m/s^2).
3753 *
3754 * @return ground truth gravity norm or null.
3755 */
3756 public Double getGroundTruthGravityNorm() {
3757 return groundTruthGravityNorm;
3758 }
3759
3760 /**
3761 * Gets ground truth gravity norm to be expected at location where measurements have been made.
3762 *
3763 * @return ground truth gravity norm or null.
3764 */
3765 public Acceleration getGroundTruthGravityNormAsAcceleration() {
3766 return groundTruthGravityNorm != null
3767 ? new Acceleration(groundTruthGravityNorm, AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
3768 }
3769
3770 /**
3771 * Gets ground truth gravity norm to be expected at location where measurements have been made.
3772 *
3773 * @param result instance where result will be stored.
3774 * @return true if ground truth gravity norm has been defined, false if it is not available yet.
3775 */
3776 public boolean getGroundTruthGravityNormAsAcceleration(final Acceleration result) {
3777 if (groundTruthGravityNorm != null) {
3778 result.setValue(groundTruthGravityNorm);
3779 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
3780 return true;
3781 } else {
3782 return false;
3783 }
3784 }
3785
3786 /**
3787 * Gets x-coordinate of known accelerometer bias.
3788 * This is expressed in meters per squared second (m/s^2).
3789 *
3790 * @return x-coordinate of known accelerometer bias.
3791 */
3792 @Override
3793 public double getBiasX() {
3794 return biasX;
3795 }
3796
3797 /**
3798 * Sets x-coordinate of known accelerometer bias.
3799 * This is expressed in meters per squared second (m/s^2).
3800 *
3801 * @param biasX x-coordinate of known accelerometer bias.
3802 * @throws LockedException if calibrator is currently running.
3803 */
3804 @Override
3805 public void setBiasX(final double biasX) throws LockedException {
3806 if (running) {
3807 throw new LockedException();
3808 }
3809 this.biasX = biasX;
3810 }
3811
3812 /**
3813 * Gets y-coordinate of known accelerometer bias.
3814 * This is expressed in meters per squared second (m/s^2).
3815 *
3816 * @return y-coordinate of known accelerometer bias.
3817 */
3818 @Override
3819 public double getBiasY() {
3820 return biasY;
3821 }
3822
3823 /**
3824 * Sets y-coordinate of known accelerometer bias.
3825 * This is expressed in meters per squared second (m/s^2).
3826 *
3827 * @param biasY y-coordinate of known accelerometer bias.
3828 * @throws LockedException if calibrator is currently running.
3829 */
3830 @Override
3831 public void setBiasY(final double biasY) throws LockedException {
3832 if (running) {
3833 throw new LockedException();
3834 }
3835 this.biasY = biasY;
3836 }
3837
3838 /**
3839 * Gets z-coordinate of known accelerometer bias.
3840 * This is expressed in meters per squared second (m/s^2).
3841 *
3842 * @return z-coordinate of known accelerometer bias.
3843 */
3844 @Override
3845 public double getBiasZ() {
3846 return biasZ;
3847 }
3848
3849 /**
3850 * Sets z-coordinate of known accelerometer bias.
3851 * This is expressed in meters per squared second (m/s^2).
3852 *
3853 * @param biasZ z-coordinate of known accelerometer bias.
3854 * @throws LockedException if calibrator is currently running.
3855 */
3856 @Override
3857 public void setBiasZ(final double biasZ) throws LockedException {
3858 if (running) {
3859 throw new LockedException();
3860 }
3861 this.biasZ = biasZ;
3862 }
3863
3864 /**
3865 * Gets x-coordinate of known accelerometer bias.
3866 *
3867 * @return x-coordinate of known accelerometer bias.
3868 */
3869 @Override
3870 public Acceleration getBiasXAsAcceleration() {
3871 return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3872 }
3873
3874 /**
3875 * Gets x-coordinate of known accelerometer bias.
3876 *
3877 * @param result instance where result data will be stored.
3878 */
3879 @Override
3880 public void getBiasXAsAcceleration(final Acceleration result) {
3881 result.setValue(biasX);
3882 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
3883 }
3884
3885 /**
3886 * Sets x-coordinate of known accelerometer bias.
3887 *
3888 * @param biasX x-coordinate of known accelerometer bias.
3889 * @throws LockedException if calibrator is currently running.
3890 */
3891 @Override
3892 public void setBiasX(final Acceleration biasX) throws LockedException {
3893 if (running) {
3894 throw new LockedException();
3895 }
3896 this.biasX = convertAcceleration(biasX);
3897 }
3898
3899 /**
3900 * Gets y-coordinate of known accelerometer bias.
3901 *
3902 * @return y-coordinate of known accelerometer bias.
3903 */
3904 @Override
3905 public Acceleration getBiasYAsAcceleration() {
3906 return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3907 }
3908
3909 /**
3910 * Gets y-coordinate of known accelerometer bias.
3911 *
3912 * @param result instance where result data will be stored.
3913 */
3914 @Override
3915 public void getBiasYAsAcceleration(final Acceleration result) {
3916 result.setValue(biasY);
3917 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
3918 }
3919
3920 /**
3921 * Sets y-coordinate of known accelerometer bias.
3922 *
3923 * @param biasY y-coordinate of known accelerometer bias.
3924 * @throws LockedException if calibrator is currently running.
3925 */
3926 @Override
3927 public void setBiasY(final Acceleration biasY) throws LockedException {
3928 if (running) {
3929 throw new LockedException();
3930 }
3931 this.biasY = convertAcceleration(biasY);
3932 }
3933
3934 /**
3935 * Gets z-coordinate of known accelerometer bias.
3936 *
3937 * @return z-coordinate of known accelerometer bias.
3938 */
3939 @Override
3940 public Acceleration getBiasZAsAcceleration() {
3941 return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3942 }
3943
3944 /**
3945 * Gets z-coordinate of known accelerometer bias.
3946 *
3947 * @param result instance where result data will be stored.
3948 */
3949 @Override
3950 public void getBiasZAsAcceleration(final Acceleration result) {
3951 result.setValue(biasZ);
3952 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
3953 }
3954
3955 /**
3956 * Sets z-coordinate of known accelerometer bias to be used to find a solution.
3957 *
3958 * @param biasZ z-coordinate of known accelerometer bias.
3959 * @throws LockedException if calibrator is currently running.
3960 */
3961 @Override
3962 public void setBiasZ(final Acceleration biasZ) throws LockedException {
3963 if (running) {
3964 throw new LockedException();
3965 }
3966 this.biasZ = convertAcceleration(biasZ);
3967 }
3968
3969 /**
3970 * Sets known bias coordinates of accelerometer expressed in meters
3971 * per squared second (m/s^2).
3972 *
3973 * @param biasX x-coordinate of known accelerometer bias.
3974 * @param biasY y-coordinate of known accelerometer bias.
3975 * @param biasZ z-coordinate of known accelerometer bias.
3976 * @throws LockedException if calibrator is currently running.
3977 */
3978 @Override
3979 public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
3980 if (running) {
3981 throw new LockedException();
3982 }
3983 this.biasX = biasX;
3984 this.biasY = biasY;
3985 this.biasZ = biasZ;
3986 }
3987
3988 /**
3989 * Sets known bias coordinates of accelerometer.
3990 *
3991 * @param biasX x-coordinate of known accelerometer bias.
3992 * @param biasY y-coordinate of known accelerometer bias.
3993 * @param biasZ z-coordinate of known accelerometer bias.
3994 * @throws LockedException if calibrator is currently running.
3995 */
3996 @Override
3997 public void setBiasCoordinates(
3998 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) throws LockedException {
3999 if (running) {
4000 throw new LockedException();
4001 }
4002 this.biasX = convertAcceleration(biasX);
4003 this.biasY = convertAcceleration(biasY);
4004 this.biasZ = convertAcceleration(biasZ);
4005 }
4006
4007 /**
4008 * Gets known accelerometer bias.
4009 *
4010 * @return known accelerometer bias.
4011 */
4012 @Override
4013 public AccelerationTriad getBiasAsTriad() {
4014 return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
4015 }
4016
4017 /**
4018 * Gets known accelerometer bias.
4019 *
4020 * @param result instance where result will be stored.
4021 */
4022 @Override
4023 public void getBiasAsTriad(final AccelerationTriad result) {
4024 result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4025 }
4026
4027 /**
4028 * Sets known accelerometer bias.
4029 *
4030 * @param bias accelerometer bias to be set.
4031 * @throws LockedException if calibrator is currently running.
4032 */
4033 @Override
4034 public void setBias(final AccelerationTriad bias) throws LockedException {
4035 if (running) {
4036 throw new LockedException();
4037 }
4038
4039 biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
4040 biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
4041 biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
4042 }
4043
4044 /**
4045 * Gets initial x scaling factor.
4046 *
4047 * @return initial x scaling factor.
4048 */
4049 @Override
4050 public double getInitialSx() {
4051 return initialSx;
4052 }
4053
4054 /**
4055 * Sets initial x scaling factor.
4056 *
4057 * @param initialSx initial x scaling factor.
4058 * @throws LockedException if calibrator is currently running.
4059 */
4060 @Override
4061 public void setInitialSx(final double initialSx) throws LockedException {
4062 if (running) {
4063 throw new LockedException();
4064 }
4065 this.initialSx = initialSx;
4066 }
4067
4068 /**
4069 * Gets initial y scaling factor.
4070 *
4071 * @return initial y scaling factor.
4072 */
4073 @Override
4074 public double getInitialSy() {
4075 return initialSy;
4076 }
4077
4078 /**
4079 * Sets initial y scaling factor.
4080 *
4081 * @param initialSy initial y scaling factor.
4082 * @throws LockedException if calibrator is currently running.
4083 */
4084 @Override
4085 public void setInitialSy(final double initialSy) throws LockedException {
4086 if (running) {
4087 throw new LockedException();
4088 }
4089 this.initialSy = initialSy;
4090 }
4091
4092 /**
4093 * Gets initial z scaling factor.
4094 *
4095 * @return initial z scaling factor.
4096 */
4097 @Override
4098 public double getInitialSz() {
4099 return initialSz;
4100 }
4101
4102 /**
4103 * Sets initial z scaling factor.
4104 *
4105 * @param initialSz initial z scaling factor.
4106 * @throws LockedException if calibrator is currently running.
4107 */
4108 @Override
4109 public void setInitialSz(final double initialSz) throws LockedException {
4110 if (running) {
4111 throw new LockedException();
4112 }
4113 this.initialSz = initialSz;
4114 }
4115
4116 /**
4117 * Gets initial x-y cross coupling error.
4118 *
4119 * @return initial x-y cross coupling error.
4120 */
4121 @Override
4122 public double getInitialMxy() {
4123 return initialMxy;
4124 }
4125
4126 /**
4127 * Sets initial x-y cross coupling error.
4128 *
4129 * @param initialMxy initial x-y cross coupling error.
4130 * @throws LockedException if calibrator is currently running.
4131 */
4132 @Override
4133 public void setInitialMxy(final double initialMxy) throws LockedException {
4134 if (running) {
4135 throw new LockedException();
4136 }
4137 this.initialMxy = initialMxy;
4138 }
4139
4140 /**
4141 * Gets initial x-z cross coupling error.
4142 *
4143 * @return initial x-z cross coupling error.
4144 */
4145 @Override
4146 public double getInitialMxz() {
4147 return initialMxz;
4148 }
4149
4150 /**
4151 * Sets initial x-z cross coupling error.
4152 *
4153 * @param initialMxz initial x-z cross coupling error.
4154 * @throws LockedException if calibrator is currently running.
4155 */
4156 @Override
4157 public void setInitialMxz(final double initialMxz) throws LockedException {
4158 if (running) {
4159 throw new LockedException();
4160 }
4161 this.initialMxz = initialMxz;
4162 }
4163
4164 /**
4165 * Gets initial y-x cross coupling error.
4166 *
4167 * @return initial y-x cross coupling error.
4168 */
4169 @Override
4170 public double getInitialMyx() {
4171 return initialMyx;
4172 }
4173
4174 /**
4175 * Sets initial y-x cross coupling error.
4176 *
4177 * @param initialMyx initial y-x cross coupling error.
4178 * @throws LockedException if calibrator is currently running.
4179 */
4180 @Override
4181 public void setInitialMyx(final double initialMyx) throws LockedException {
4182 if (running) {
4183 throw new LockedException();
4184 }
4185 this.initialMyx = initialMyx;
4186 }
4187
4188 /**
4189 * Gets initial y-z cross coupling error.
4190 *
4191 * @return initial y-z cross coupling error.
4192 */
4193 @Override
4194 public double getInitialMyz() {
4195 return initialMyz;
4196 }
4197
4198 /**
4199 * Sets initial y-z cross coupling error.
4200 *
4201 * @param initialMyz initial y-z cross coupling error.
4202 * @throws LockedException if calibrator is currently running.
4203 */
4204 @Override
4205 public void setInitialMyz(final double initialMyz) throws LockedException {
4206 if (running) {
4207 throw new LockedException();
4208 }
4209 this.initialMyz = initialMyz;
4210 }
4211
4212 /**
4213 * Gets initial z-x cross coupling error.
4214 *
4215 * @return initial z-x cross coupling error.
4216 */
4217 @Override
4218 public double getInitialMzx() {
4219 return initialMzx;
4220 }
4221
4222 /**
4223 * Sets initial z-x cross coupling error.
4224 *
4225 * @param initialMzx initial z-x cross coupling error.
4226 * @throws LockedException if calibrator is currently running.
4227 */
4228 @Override
4229 public void setInitialMzx(final double initialMzx) throws LockedException {
4230 if (running) {
4231 throw new LockedException();
4232 }
4233 this.initialMzx = initialMzx;
4234 }
4235
4236 /**
4237 * Gets initial z-y cross coupling error.
4238 *
4239 * @return initial z-y cross coupling error.
4240 */
4241 @Override
4242 public double getInitialMzy() {
4243 return initialMzy;
4244 }
4245
4246 /**
4247 * Sets initial z-y cross coupling error.
4248 *
4249 * @param initialMzy initial z-y cross coupling error.
4250 * @throws LockedException if calibrator is currently running.
4251 */
4252 @Override
4253 public void setInitialMzy(final double initialMzy) throws LockedException {
4254 if (running) {
4255 throw new LockedException();
4256 }
4257 this.initialMzy = initialMzy;
4258 }
4259
4260 /**
4261 * Sets initial scaling factors.
4262 *
4263 * @param initialSx initial x scaling factor.
4264 * @param initialSy initial y scaling factor.
4265 * @param initialSz initial z scaling factor.
4266 * @throws LockedException if calibrator is currently running.
4267 */
4268 @Override
4269 public void setInitialScalingFactors(
4270 final double initialSx, final double initialSy, final double initialSz) throws LockedException {
4271 if (running) {
4272 throw new LockedException();
4273 }
4274 this.initialSx = initialSx;
4275 this.initialSy = initialSy;
4276 this.initialSz = initialSz;
4277 }
4278
4279 /**
4280 * Sets initial cross coupling errors.
4281 *
4282 * @param initialMxy initial x-y cross coupling error.
4283 * @param initialMxz initial x-z cross coupling error.
4284 * @param initialMyx initial y-x cross coupling error.
4285 * @param initialMyz initial y-z cross coupling error.
4286 * @param initialMzx initial z-x cross coupling error.
4287 * @param initialMzy initial z-y cross coupling error.
4288 * @throws LockedException if calibrator is currently running.
4289 */
4290 @Override
4291 public void setInitialCrossCouplingErrors(
4292 final double initialMxy, final double initialMxz, final double initialMyx,
4293 final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
4294 if (running) {
4295 throw new LockedException();
4296 }
4297 this.initialMxy = initialMxy;
4298 this.initialMxz = initialMxz;
4299 this.initialMyx = initialMyx;
4300 this.initialMyz = initialMyz;
4301 this.initialMzx = initialMzx;
4302 this.initialMzy = initialMzy;
4303 }
4304
4305 /**
4306 * Sets initial scaling factors and cross coupling errors.
4307 *
4308 * @param initialSx initial x scaling factor.
4309 * @param initialSy initial y scaling factor.
4310 * @param initialSz initial z scaling factor.
4311 * @param initialMxy initial x-y cross coupling error.
4312 * @param initialMxz initial x-z cross coupling error.
4313 * @param initialMyx initial y-x cross coupling error.
4314 * @param initialMyz initial y-z cross coupling error.
4315 * @param initialMzx initial z-x cross coupling error.
4316 * @param initialMzy initial z-y cross coupling error.
4317 * @throws LockedException if calibrator is currently running.
4318 */
4319 @Override
4320 public void setInitialScalingFactorsAndCrossCouplingErrors(
4321 final double initialSx, final double initialSy, final double initialSz,
4322 final double initialMxy, final double initialMxz, final double initialMyx,
4323 final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
4324 if (running) {
4325 throw new LockedException();
4326 }
4327 setInitialScalingFactors(initialSx, initialSy, initialSz);
4328 setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
4329 }
4330
4331 /**
4332 * Gets known bias as an array.
4333 * Array values are expressed in meters per squared second (m/s^2).
4334 *
4335 * @return array containing coordinates of known bias.
4336 */
4337 @Override
4338 public double[] getBias() {
4339 final var result = new double[BodyKinematics.COMPONENTS];
4340 getBias(result);
4341 return result;
4342 }
4343
4344 /**
4345 * Gets known bias as an array.
4346 * Array values are expressed in meters per squared second (m/s^2).
4347 *
4348 * @param result instance where result data will be copied to.
4349 * @throws IllegalArgumentException if provided array does not have length 3.
4350 */
4351 @Override
4352 public void getBias(final double[] result) {
4353 if (result.length != BodyKinematics.COMPONENTS) {
4354 throw new IllegalArgumentException();
4355 }
4356 result[0] = biasX;
4357 result[1] = biasY;
4358 result[2] = biasZ;
4359 }
4360
4361 /**
4362 * Sets known bias as an array.
4363 * Array values are expressed in meters per squared second (m/s^2).
4364 *
4365 * @param bias known bias to be set
4366 * @throws LockedException if calibrator is currently running.
4367 * @throws IllegalArgumentException if provided array does not have length 3.
4368 */
4369 @Override
4370 public void setBias(final double[] bias) throws LockedException {
4371 if (running) {
4372 throw new LockedException();
4373 }
4374
4375 if (bias.length != BodyKinematics.COMPONENTS) {
4376 throw new IllegalArgumentException();
4377 }
4378 biasX = bias[0];
4379 biasY = bias[1];
4380 biasZ = bias[2];
4381 }
4382
4383 /**
4384 * Gets known bias as a column matrix.
4385 *
4386 * @return known bias as a column matrix.
4387 */
4388 @Override
4389 public Matrix getBiasAsMatrix() {
4390 Matrix result;
4391 try {
4392 result = new Matrix(BodyKinematics.COMPONENTS, 1);
4393 getBiasAsMatrix(result);
4394 } catch (final WrongSizeException ignore) {
4395 // never happens
4396 result = null;
4397 }
4398 return result;
4399 }
4400
4401 /**
4402 * Gets known bias as a column matrix.
4403 *
4404 * @param result instance where result data will be copied to.
4405 * @throws IllegalArgumentException if provided matrix is not 3x1.
4406 */
4407 @Override
4408 public void getBiasAsMatrix(final Matrix result) {
4409 if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
4410 throw new IllegalArgumentException();
4411 }
4412 result.setElementAtIndex(0, biasX);
4413 result.setElementAtIndex(1, biasY);
4414 result.setElementAtIndex(2, biasZ);
4415 }
4416
4417 /**
4418 * Sets known bias as an array.
4419 *
4420 * @param bias bias to be set.
4421 * @throws LockedException if calibrator is currently running.
4422 * @throws IllegalArgumentException if provided matrix is not 3x1.
4423 */
4424 @Override
4425 public void setBias(final Matrix bias) throws LockedException {
4426 if (running) {
4427 throw new LockedException();
4428 }
4429 if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
4430 throw new IllegalArgumentException();
4431 }
4432
4433 biasX = bias.getElementAtIndex(0);
4434 biasY = bias.getElementAtIndex(1);
4435 biasZ = bias.getElementAtIndex(2);
4436 }
4437
4438 /**
4439 * Gets initial scale factors and cross coupling errors matrix.
4440 *
4441 * @return initial scale factors and cross coupling errors matrix.
4442 */
4443 @Override
4444 public Matrix getInitialMa() {
4445 Matrix result;
4446 try {
4447 result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
4448 getInitialMa(result);
4449 } catch (final WrongSizeException ignore) {
4450 // never happens
4451 result = null;
4452 }
4453 return result;
4454 }
4455
4456 /**
4457 * Gets initial scale factors and cross coupling errors matrix.
4458 *
4459 * @param result instance where data will be stored.
4460 * @throws IllegalArgumentException if provided matrix is not 3x3.
4461 */
4462 @Override
4463 public void getInitialMa(final Matrix result) {
4464 if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
4465 throw new IllegalArgumentException();
4466 }
4467 result.setElementAtIndex(0, initialSx);
4468 result.setElementAtIndex(1, initialMyx);
4469 result.setElementAtIndex(2, initialMzx);
4470
4471 result.setElementAtIndex(3, initialMxy);
4472 result.setElementAtIndex(4, initialSy);
4473 result.setElementAtIndex(5, initialMzy);
4474
4475 result.setElementAtIndex(6, initialMxz);
4476 result.setElementAtIndex(7, initialMyz);
4477 result.setElementAtIndex(8, initialSz);
4478 }
4479
4480 /**
4481 * Sets initial scale factors and cross coupling errors matrix.
4482 *
4483 * @param initialMa initial scale factors and cross coupling errors matrix.
4484 * @throws IllegalArgumentException if provided matrix is not 3x3.
4485 * @throws LockedException if calibrator is currently running.
4486 */
4487 @Override
4488 public void setInitialMa(final Matrix initialMa) throws LockedException {
4489 if (running) {
4490 throw new LockedException();
4491 }
4492 if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
4493 throw new IllegalArgumentException();
4494 }
4495
4496 initialSx = initialMa.getElementAtIndex(0);
4497 initialMyx = initialMa.getElementAtIndex(1);
4498 initialMzx = initialMa.getElementAtIndex(2);
4499
4500 initialMxy = initialMa.getElementAtIndex(3);
4501 initialSy = initialMa.getElementAtIndex(4);
4502 initialMzy = initialMa.getElementAtIndex(5);
4503
4504 initialMxz = initialMa.getElementAtIndex(6);
4505 initialMyz = initialMa.getElementAtIndex(7);
4506 initialSz = initialMa.getElementAtIndex(8);
4507 }
4508
4509 /**
4510 * Gets a collection of body kinematics measurements taken at
4511 * a given position with different unknown orientations and containing
4512 * the standard deviations of accelerometer and gyroscope measurements.
4513 *
4514 * @return collection of body kinematics measurements at a known position
4515 * with unknown orientations.
4516 */
4517 @Override
4518 public Collection<StandardDeviationBodyKinematics> getMeasurements() {
4519 return measurements;
4520 }
4521
4522 /**
4523 * Sets a collection of body kinematics measurements taken at
4524 * a given position with different unknown orientations and containing
4525 * the standard deviations of accelerometer and gyroscope measurements.
4526 *
4527 * @param measurements collection of body kinematics measurements at a
4528 * known position with unknown orientations.
4529 * @throws LockedException if calibrator is currently running.
4530 */
4531 @Override
4532 public void setMeasurements(final Collection<StandardDeviationBodyKinematics> measurements) throws LockedException {
4533 if (running) {
4534 throw new LockedException();
4535 }
4536 this.measurements = measurements;
4537 }
4538
4539 /**
4540 * Indicates the type of measurement used by this calibrator.
4541 *
4542 * @return type of measurement used by this calibrator.
4543 */
4544 @Override
4545 public AccelerometerCalibratorMeasurementType getMeasurementType() {
4546 return AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS;
4547 }
4548
4549 /**
4550 * Indicates whether this calibrator requires ordered measurements in a
4551 * list or not.
4552 *
4553 * @return true if measurements must be ordered, false otherwise.
4554 */
4555 @Override
4556 public boolean isOrderedMeasurementsRequired() {
4557 return false;
4558 }
4559
4560 /**
4561 * Indicates whether this calibrator requires quality scores for each
4562 * measurement or not.
4563 *
4564 * @return true if quality scores are required, false otherwise.
4565 */
4566 @Override
4567 public boolean isQualityScoresRequired() {
4568 return false;
4569 }
4570
4571 /**
4572 * Indicates whether z-axis is assumed to be common for accelerometer and
4573 * gyroscope.
4574 * When enabled, this eliminates 3 variables from Ma matrix.
4575 *
4576 * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
4577 * false otherwise.
4578 */
4579 @Override
4580 public boolean isCommonAxisUsed() {
4581 return commonAxisUsed;
4582 }
4583
4584 /**
4585 * Specifies whether z-axis is assumed to be common for accelerometer and
4586 * gyroscope.
4587 * When enabled, this eliminates 3 variables from Ma matrix.
4588 *
4589 * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
4590 * and gyroscope, false otherwise.
4591 * @throws LockedException if calibrator is currently running.
4592 */
4593 @Override
4594 public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
4595 if (running) {
4596 throw new LockedException();
4597 }
4598
4599 this.commonAxisUsed = commonAxisUsed;
4600 }
4601
4602 /**
4603 * Gets listener to handle events raised by this estimator.
4604 *
4605 * @return listener to handle events raised by this estimator.
4606 */
4607 public L getListener() {
4608 return listener;
4609 }
4610
4611 /**
4612 * Sets listener to handle events raised by this estimator.
4613 *
4614 * @param listener listener to handle events raised by this estimator.
4615 * @throws LockedException if calibrator is currently running.
4616 */
4617 public void setListener(final L listener) throws LockedException {
4618 if (running) {
4619 throw new LockedException();
4620 }
4621
4622 this.listener = listener;
4623 }
4624
4625 /**
4626 * Gets minimum number of required measurements.
4627 *
4628 * @return minimum number of required measurements.
4629 */
4630 @Override
4631 public int getMinimumRequiredMeasurements() {
4632 return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
4633 }
4634
4635 /**
4636 * Indicates whether calibrator is ready to start.
4637 *
4638 * @return true if calibrator is ready, false otherwise.
4639 */
4640 @Override
4641 public boolean isReady() {
4642 return measurements != null && measurements.size() >= getMinimumRequiredMeasurements()
4643 && groundTruthGravityNorm != null;
4644 }
4645
4646 /**
4647 * Indicates whether calibrator is currently running or not.
4648 *
4649 * @return true if calibrator is running, false otherwise.
4650 */
4651 @Override
4652 public boolean isRunning() {
4653 return running;
4654 }
4655
4656 /**
4657 * Estimates accelerometer calibration parameters containing scale factors
4658 * and cross-coupling errors.
4659 *
4660 * @throws LockedException if calibrator is currently running.
4661 * @throws NotReadyException if calibrator is not ready.
4662 * @throws CalibrationException if estimation fails for numerical reasons.
4663 */
4664 @Override
4665 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
4666 if (running) {
4667 throw new LockedException();
4668 }
4669
4670 if (!isReady()) {
4671 throw new NotReadyException();
4672 }
4673
4674 try {
4675 running = true;
4676
4677 if (listener != null) {
4678 //noinspection unchecked
4679 listener.onCalibrateStart((C) this);
4680 }
4681
4682 if (commonAxisUsed) {
4683 calibrateCommonAxis();
4684 } else {
4685 calibrateGeneral();
4686 }
4687
4688 if (listener != null) {
4689 //noinspection unchecked
4690 listener.onCalibrateEnd((C) this);
4691 }
4692
4693 } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
4694 throw new CalibrationException(e);
4695 } finally {
4696 running = false;
4697 }
4698 }
4699
4700 /**
4701 * Gets estimated accelerometer scale factors and ross coupling errors.
4702 * This is the product of matrix Ta containing cross coupling errors and Ka
4703 * containing scaling factors.
4704 * So tat:
4705 * <pre>
4706 * Ma = [sx mxy mxz] = Ta*Ka
4707 * [myx sy myz]
4708 * [mzx mzy sz ]
4709 * </pre>
4710 * Where:
4711 * <pre>
4712 * Ka = [sx 0 0 ]
4713 * [0 sy 0 ]
4714 * [0 0 sz]
4715 * </pre>
4716 * and
4717 * <pre>
4718 * Ta = [1 -alphaXy alphaXz ]
4719 * [alphaYx 1 -alphaYz]
4720 * [-alphaZx alphaZy 1 ]
4721 * </pre>
4722 * Hence:
4723 * <pre>
4724 * Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
4725 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
4726 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
4727 * </pre>
4728 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
4729 * are considered to be zero if the accelerometer z-axis is assumed to be the same
4730 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
4731 * becomes upper diagonal:
4732 * <pre>
4733 * Ma = [sx mxy mxz]
4734 * [0 sy myz]
4735 * [0 0 sz ]
4736 * </pre>
4737 * Values of this matrix are unit-less.
4738 *
4739 * @return estimated accelerometer scale factors and cross coupling errors, or null
4740 * if not available.
4741 */
4742 @Override
4743 public Matrix getEstimatedMa() {
4744 return estimatedMa;
4745 }
4746
4747 /**
4748 * Gets estimated x-axis scale factor.
4749 *
4750 * @return estimated x-axis scale factor or null if not available.
4751 */
4752 @Override
4753 public Double getEstimatedSx() {
4754 return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
4755 }
4756
4757 /**
4758 * Gets estimated y-axis scale factor.
4759 *
4760 * @return estimated y-axis scale factor or null if not available.
4761 */
4762 @Override
4763 public Double getEstimatedSy() {
4764 return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
4765 }
4766
4767 /**
4768 * Gets estimated z-axis scale factor.
4769 *
4770 * @return estimated z-axis scale factor or null if not available.
4771 */
4772 @Override
4773 public Double getEstimatedSz() {
4774 return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
4775 }
4776
4777 /**
4778 * Gets estimated x-y cross-coupling error.
4779 *
4780 * @return estimated x-y cross-coupling error or null if not available.
4781 */
4782 @Override
4783 public Double getEstimatedMxy() {
4784 return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
4785 }
4786
4787 /**
4788 * Gets estimated x-z cross-coupling error.
4789 *
4790 * @return estimated x-z cross-coupling error or null if not available.
4791 */
4792 @Override
4793 public Double getEstimatedMxz() {
4794 return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
4795 }
4796
4797 /**
4798 * Gets estimated y-x cross-coupling error.
4799 *
4800 * @return estimated y-x cross-coupling error or null if not available.
4801 */
4802 @Override
4803 public Double getEstimatedMyx() {
4804 return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
4805 }
4806
4807 /**
4808 * Gets estimated y-z cross-coupling error.
4809 *
4810 * @return estimated y-z cross-coupling error or null if not available.
4811 */
4812 @Override
4813 public Double getEstimatedMyz() {
4814 return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
4815 }
4816
4817 /**
4818 * Gets estimated z-x cross-coupling error.
4819 *
4820 * @return estimated z-x cross-coupling error or null if not available.
4821 */
4822 @Override
4823 public Double getEstimatedMzx() {
4824 return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
4825 }
4826
4827 /**
4828 * Gets estimated z-y cross-coupling error.
4829 *
4830 * @return estimated z-y cross-coupling error or null if not available.
4831 */
4832 @Override
4833 public Double getEstimatedMzy() {
4834 return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
4835 }
4836
4837 /**
4838 * Gets estimated covariance matrix for estimated calibration parameters.
4839 * Diagonal elements of the matrix contains variance for the following
4840 * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
4841 * myz, mzx, mzy.
4842 *
4843 * @return estimated covariance matrix for estimated calibration parameters.
4844 */
4845 @Override
4846 public Matrix getEstimatedCovariance() {
4847 return estimatedCovariance;
4848 }
4849
4850 /**
4851 * Gets estimated chi square value.
4852 *
4853 * @return estimated chi square value.
4854 */
4855 @Override
4856 public double getEstimatedChiSq() {
4857 return estimatedChiSq;
4858 }
4859
4860 /**
4861 * Gets estimated chi square degrees of freedom. Degrees of freedom is equal to the number of sampled data minus the
4862 * number of estimated parameters.
4863 *
4864 * @return estimated degrees of freedom of chi square value
4865 */
4866 @Override
4867 public int getEstimatedChiSqDegreesOfFreedom() {
4868 return estimatedChiSqDegreesOfFreedom;
4869 }
4870
4871 /**
4872 * Gets estimated reduced chi square value. This is equal to estimated chi square value divided by its degrees of
4873 * freedom. Ideally this value should be close to 1.0, indicating that fit is optimal.
4874 * A value larger than 1.0 indicates that fit is not good or noise has been underestimated, and a value smaller than
4875 * 1.0 indicates that there is overfitting or noise has been overestimated.
4876 *
4877 * @return estimated reduced chi square value
4878 */
4879 @Override
4880 public double getEstimatedReducedChiSq() {
4881 return estimatedReducedChiSq;
4882 }
4883
4884 /**
4885 * Gets estimated mean square error respect to provided measurements.
4886 *
4887 * @return estimated mean square error respect to provided measurements.
4888 */
4889 @Override
4890 public double getEstimatedMse() {
4891 return estimatedMse;
4892 }
4893
4894 /**
4895 * Gets estimated probability of finding a smaller chi square value expressed as a value between 0.0 and 1.0. The
4896 * smaller the found chi square value is, the better the fit of the estimated parameters to the actual parameter.
4897 * Thus, the smaller the chance of finding a smaller chi square value, then the better the estimated fit is.
4898 *
4899 * @return estimated probability of finding a smaller chi square value.
4900 */
4901 @Override
4902 public double getEstimatedP() {
4903 return estimatedP;
4904 }
4905
4906 /**
4907 * Gets estimated measure of quality of estimated fit as a value between 0.0 and 1.0. The larger the quality value
4908 * is, the better the fit that has been estimated.
4909 *
4910 * @return estimated measure of quality of estimated fit.
4911 */
4912 @Override
4913 public double getEstimatedQ() {
4914 return estimatedQ;
4915 }
4916
4917 /**
4918 * Converts acceleration instance to meters per squared second.
4919 *
4920 * @param acceleration acceleration instance to be converted.
4921 * @return converted value.
4922 */
4923 protected static double convertAcceleration(final Acceleration acceleration) {
4924 return convertAcceleration(acceleration.getValue().doubleValue(), acceleration.getUnit());
4925 }
4926
4927 /**
4928 * Converts acceleration value and unit to meters per squared second.
4929 *
4930 * @param value acceleration value.
4931 * @param unit unit of acceleration value.
4932 * @return converted value.
4933 */
4934 private static double convertAcceleration(final double value, final AccelerationUnit unit) {
4935 return AccelerationConverter.convert(value, unit, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4936 }
4937
4938 /**
4939 * Internally sets ground truth gravity norm to be expected at location where
4940 * measurements have been made, expressed in meters per squared second
4941 * (m/s^2).
4942 *
4943 * @param groundTruthGravityNorm ground truth gravity norm or null if
4944 * undefined.
4945 * @throws IllegalArgumentException if provided value is negative.
4946 */
4947 protected void internalSetGroundTruthGravityNorm(final Double groundTruthGravityNorm) {
4948 if (groundTruthGravityNorm != null && groundTruthGravityNorm < 0.0) {
4949 throw new IllegalArgumentException();
4950 }
4951 this.groundTruthGravityNorm = groundTruthGravityNorm;
4952 }
4953
4954 /**
4955 * Sets input data into Levenberg-Marquardt fitter.
4956 *
4957 * @throws WrongSizeException never happens.
4958 */
4959 private void setInputData() throws WrongSizeException {
4960
4961 final var g = groundTruthGravityNorm;
4962 final var g2 = g * g;
4963
4964 final var numMeasurements = measurements.size();
4965 final var x = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
4966 final var y = new double[numMeasurements];
4967 final var specificForceStandardDeviations = new double[numMeasurements];
4968 var i = 0;
4969 for (final var measurement : measurements) {
4970 final var measuredKinematics = measurement.getKinematics();
4971
4972 final var fx = measuredKinematics.getFx();
4973 final var fy = measuredKinematics.getFy();
4974 final var fz = measuredKinematics.getFz();
4975
4976 x.setElementAt(i, 0, fx);
4977 x.setElementAt(i, 1, fy);
4978 x.setElementAt(i, 2, fz);
4979
4980 y[i] = g2;
4981
4982 specificForceStandardDeviations[i] = measurement.getSpecificForceStandardDeviation();
4983
4984 i++;
4985 }
4986
4987 fitter.setInputData(x, y, specificForceStandardDeviations);
4988 }
4989
4990 /**
4991 * Internal method to perform general calibration.
4992 *
4993 * @throws FittingException if Levenberg-Marquardt fails for numerical reasons.
4994 * @throws AlgebraException if there are numerical instabilities that prevent
4995 * matrix inversion.
4996 * @throws com.irurueta.numerical.NotReadyException never happens.
4997 */
4998 private void calibrateGeneral() throws AlgebraException, FittingException,
4999 com.irurueta.numerical.NotReadyException {
5000 // The accelerometer model is:
5001 // fmeas = ba + (I + Ma) * ftrue + w
5002
5003 // Ideally a least squares solution tries to minimize noise component, so:
5004 // fmeas = ba + (I + Ma) * ftrue
5005
5006 // For convergence purposes of the Levenberg-Marquardt algorithm, the
5007 // accelerometer model can be better expressed as:
5008 // fmeas = T*K*(ftrue + b)
5009 // fmeas = M*(ftrue + b)
5010 // fmeas = M*ftrue + M*b
5011
5012 // where:
5013 // M = I + Ma
5014 // ba = M*b = (I + Ma)*b --> b = M^-1*ba
5015
5016 // We know that the norm of the true specific force is equal to the amount
5017 // of gravity at a certain Earth position
5018 // ||ftrue|| = ||g|| ~ 9.81 m/s^2
5019
5020 // Hence:
5021 // fmeas - M*b = M*ftrue
5022
5023 // M^-1 * (fmeas - M*b) = ftrue
5024
5025 // ||g||^2 = ||ftrue||^2 = (M^-1 * (fmeas - M*b))^T * (M^-1 * (fmeas - M*b))
5026 // ||g||^2 = (fmeas - M*b)^T*(M^-1)^T * M^-1 * (fmeas - M*b)
5027 // ||g||^2 = (fmeas - M * b)^T * ||M^-1||^2 * (fmeas - M * b)
5028 // ||g||^2 = ||fmeas - M * b||^2 * ||M^-1||^2
5029
5030 // Where:
5031
5032 // b = [bx]
5033 // [by]
5034 // [bz]
5035
5036 // M = [m11 m12 m13]
5037 // [m21 m22 m23]
5038 // [m31 m32 m33]
5039
5040 // Notice that bias b is known, hence only terms in matrix M need to be estimated
5041
5042 final var gradientEstimator = new GradientEstimator(this::evaluateGeneral);
5043
5044 final var initialM = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
5045 initialM.add(getInitialMa());
5046
5047 fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
5048 @Override
5049 public int getNumberOfDimensions() {
5050 // Input points are measured specific force coordinates
5051 return BodyKinematics.COMPONENTS;
5052 }
5053
5054 @Override
5055 public double[] createInitialParametersArray() {
5056 // cross coupling errors M
5057 return initialM.toArray();
5058 }
5059
5060 @Override
5061 public double evaluate(final int i, final double[] point, final double[] params,
5062 final double[] derivatives) throws EvaluationException {
5063
5064 fmeasX = point[0];
5065 fmeasY = point[1];
5066 fmeasZ = point[2];
5067
5068 gradientEstimator.gradient(params, derivatives);
5069
5070 return evaluateGeneral(params);
5071 }
5072 });
5073
5074 setInputData();
5075
5076 fitter.fit();
5077
5078 final var result = fitter.getA();
5079
5080 final var m11 = result[0];
5081 final var m21 = result[1];
5082 final var m31 = result[2];
5083
5084 final var m12 = result[3];
5085 final var m22 = result[4];
5086 final var m32 = result[5];
5087
5088 final var m13 = result[6];
5089 final var m23 = result[7];
5090 final var m33 = result[8];
5091
5092 final var crossCoupling = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
5093 crossCoupling.setElementAtIndex(0, m11);
5094 crossCoupling.setElementAtIndex(1, m21);
5095 crossCoupling.setElementAtIndex(2, m31);
5096
5097 crossCoupling.setElementAtIndex(3, m12);
5098 crossCoupling.setElementAtIndex(4, m22);
5099 crossCoupling.setElementAtIndex(5, m32);
5100
5101 crossCoupling.setElementAtIndex(6, m13);
5102 crossCoupling.setElementAtIndex(7, m23);
5103 crossCoupling.setElementAtIndex(8, m33);
5104
5105 setResult(crossCoupling);
5106 }
5107
5108 /**
5109 * Internal method to perform calibration when common z-axis is assumed for both
5110 * the accelerometer and gyroscope.
5111 *
5112 * @throws FittingException if Levenberg-Marquardt fails for numerical reasons.
5113 * @throws AlgebraException if there are numerical instabilities that prevent
5114 * matrix inversion.
5115 * @throws com.irurueta.numerical.NotReadyException never happens.
5116 */
5117 private void calibrateCommonAxis() throws AlgebraException, FittingException,
5118 com.irurueta.numerical.NotReadyException {
5119 // The accelerometer model is:
5120 // fmeas = ba + (I + Ma) * ftrue + w
5121
5122 // Ideally a least squares solution tries to minimize noise component, so:
5123 // fmeas = ba + (I + Ma) * ftrue
5124
5125 // For convergence purposes of the Levenberg-Marquardt algorithm, the
5126 // accelerometer model can be better expressed as:
5127 // fmeas = T*K*(ftrue + b)
5128 // fmeas = M*(ftrue + b)
5129 // fmeas = M*ftrue + M*b
5130
5131 // where:
5132 // M = I + Ma
5133 // ba = M*b = (I + Ma)*b --> b = M^-1*ba
5134
5135 // We know that the norm of the true specific force is equal to the amount
5136 // of gravity at a certain Earth position
5137 // ||ftrue|| = ||g|| ~ 9.81 m/s^2
5138
5139 // Hence:
5140 // fmeas - M*b = M*ftrue
5141
5142 // M^-1 * (fmeas - M*b) = ftrue
5143
5144 // ||g||^2 = ||ftrue||^2 = (M^-1 * (fmeas - M*b))^T * (M^-1 * (fmeas - M*b))
5145 // ||g||^2 = (fmeas - M*b)^T*(M^-1)^T * M^-1 * (fmeas - M*b)
5146 // ||g||^2 = (fmeas - M * b)^T * ||M^-1||^2 * (fmeas - M * b)
5147 // ||g||^2 = ||fmeas - M * b||^2 * ||M^-1||^2
5148
5149 // Where:
5150
5151 // b = [bx]
5152 // [by]
5153 // [bz]
5154
5155 // M = [m11 m12 m13]
5156 // [0 m22 m23]
5157 // [0 0 m33]
5158
5159 // Notice that bias b is known, hence only terms in matrix M need to be estimated
5160
5161 final var gradientEstimator = new GradientEstimator(this::evaluateCommonAxis);
5162
5163 final Matrix initialM = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
5164 initialM.add(getInitialMa());
5165
5166 // Force initial M to be upper diagonal
5167 initialM.setElementAt(1, 0, 0.0);
5168 initialM.setElementAt(2, 0, 0.0);
5169 initialM.setElementAt(2, 1, 0.0);
5170
5171 fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
5172 @Override
5173 public int getNumberOfDimensions() {
5174 // Input points are measured specific force coordinates
5175 return BodyKinematics.COMPONENTS;
5176 }
5177
5178 @Override
5179 public double[] createInitialParametersArray() {
5180 final double[] initial = new double[COMMON_Z_AXIS_UNKNOWNS];
5181
5182 // upper diagonal cross coupling errors M
5183 var k = 0;
5184 for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
5185 for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
5186 if (i <= j) {
5187 initial[k] = initialM.getElementAt(i, j);
5188 k++;
5189 }
5190 }
5191 }
5192
5193 return initial;
5194 }
5195
5196 @Override
5197 public double evaluate(final int i, final double[] point, final double[] params,
5198 final double[] derivatives) throws EvaluationException {
5199
5200 fmeasX = point[0];
5201 fmeasY = point[1];
5202 fmeasZ = point[2];
5203
5204 gradientEstimator.gradient(params, derivatives);
5205
5206 return evaluateCommonAxis(params);
5207 }
5208 });
5209
5210 setInputData();
5211
5212 fitter.fit();
5213
5214 final var result = fitter.getA();
5215
5216 final var m11 = result[0];
5217
5218 final var m12 = result[1];
5219 final var m22 = result[2];
5220
5221 final var m13 = result[3];
5222 final var m23 = result[4];
5223 final var m33 = result[5];
5224
5225 final var crossCoupling = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
5226 crossCoupling.setElementAtIndex(0, m11);
5227 crossCoupling.setElementAtIndex(1, 0.0);
5228 crossCoupling.setElementAtIndex(2, 0.0);
5229
5230 crossCoupling.setElementAtIndex(3, m12);
5231 crossCoupling.setElementAtIndex(4, m22);
5232 crossCoupling.setElementAtIndex(5, 0.0);
5233
5234 crossCoupling.setElementAtIndex(6, m13);
5235 crossCoupling.setElementAtIndex(7, m23);
5236 crossCoupling.setElementAtIndex(8, m33);
5237
5238 setResult(crossCoupling);
5239
5240 // taking into account that:
5241 // Ma = [sx mxy mxz] = [m11 m12 m13]
5242 // [myx sy myz] [m21 m22 m23]
5243 // [mzx mzy sz ] [m31 m32 m33]
5244
5245 // propagate covariance so that all parameters are taken into account
5246 // in the order: sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy
5247
5248 // We define a lineal function mapping original parameters for the
5249 // common axis case to the general case
5250 // [sx'] = [1 0 0 0 0 0][sx ]
5251 // [sy'] [0 0 1 0 0 0][mxy]
5252 // [sz'] [0 0 0 0 0 1][sy ]
5253 // [mxy'] [0 1 0 0 0 0][mxz]
5254 // [mxz'] [0 0 0 1 0 0][myz]
5255 // [myx'] [0 0 0 0 0 0][sz ]
5256 // [myz'] [0 0 0 0 1 0]
5257 // [mzx'] [0 0 0 0 0 0]
5258 // [mzy'] [0 0 0 0 0 0]
5259
5260 // As defined in com.irurueta.statistics.MultivariateNormalDist,
5261 // if we consider the jacobian of the lineal application the matrix shown
5262 // above, then covariance can be propagated as follows
5263 final var jacobian = new Matrix(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
5264 jacobian.setElementAt(0, 0, 1.0);
5265 jacobian.setElementAt(1, 2, 1.0);
5266 jacobian.setElementAt(2, 5, 1.0);
5267 jacobian.setElementAt(3, 1, 1.0);
5268 jacobian.setElementAt(4, 3, 1.0);
5269 jacobian.setElementAt(6, 4, 1.0);
5270 // propagated covariance is J * Cov * J'
5271 final var jacobianTrans = jacobian.transposeAndReturnNew();
5272 jacobian.multiply(estimatedCovariance);
5273 jacobian.multiply(jacobianTrans);
5274 estimatedCovariance = jacobian;
5275 }
5276
5277 /**
5278 * Makes proper conversion of internal cross-coupling and bias matrices.
5279 *
5280 * @param m internal cross-coupling matrix.
5281 */
5282 private void setResult(final Matrix m) {
5283 // Because:
5284 // M = I + Ma
5285
5286 // Then:
5287 // Ma = M - I
5288 if (estimatedMa == null) {
5289 estimatedMa = m;
5290 } else {
5291 estimatedMa.copyFrom(m);
5292 }
5293
5294 for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
5295 estimatedMa.setElementAt(i, i, estimatedMa.getElementAt(i, i) - 1.0);
5296 }
5297
5298 // since only a constant term is subtracted, covariance is preserved
5299 estimatedCovariance = fitter.getCovar();
5300 estimatedChiSq = fitter.getChisq();
5301 estimatedChiSqDegreesOfFreedom = fitter.getChisqDegreesOfFreedom();
5302 estimatedReducedChiSq = fitter.getReducedChisq();
5303 estimatedMse = fitter.getMse();
5304 try {
5305 estimatedP = fitter.getP();
5306 estimatedQ = fitter.getQ();
5307 } catch (final MaxIterationsExceededException ignore) {
5308 // if numerical instabilities arise, we assume worst case (no fit at all)
5309 // probability of finding a smaller chi square value is 1.0
5310 // quality of fit is 0.0
5311 estimatedP = 1.0;
5312 estimatedQ = 0.0;
5313 }
5314 }
5315
5316 /**
5317 * Computes estimated true specific force squared norm using current measured
5318 * specific force and provided parameters for the general case.
5319 * This method is internally executed during gradient estimation and
5320 * Levenberg-Marquardt fitting needed for calibration computation.
5321 *
5322 * @param params array containing current parameters for the general purpose case.
5323 * Must have length 9.
5324 * @return estimated true specific force squared norm.
5325 * @throws EvaluationException if there are numerical instabilities.
5326 */
5327 private double evaluateGeneral(final double[] params) throws EvaluationException {
5328 final var m11 = params[0];
5329 final var m21 = params[1];
5330 final var m31 = params[2];
5331
5332 final var m12 = params[3];
5333 final var m22 = params[4];
5334 final var m32 = params[5];
5335
5336 final var m13 = params[6];
5337 final var m23 = params[7];
5338 final var m33 = params[8];
5339
5340 return evaluate(m11, m21, m31, m12, m22, m32, m13, m23, m33);
5341 }
5342
5343 /**
5344 * Computes estimated true specific force squared norm using current measured
5345 * specific force and provided parameters when common z-axis is assumed.
5346 * This method is internally executed during gradient estimation and
5347 * Levenberg-Marquardt fitting needed for calibration computation.
5348 *
5349 * @param params array containing current parameters for the common z-axis case.
5350 * Must have length 6.
5351 * @return estimated true specific force squared norm.
5352 * @throws EvaluationException if there are numerical instabilities.
5353 */
5354 private double evaluateCommonAxis(final double[] params) throws EvaluationException {
5355 final var m11 = params[0];
5356
5357 final var m12 = params[1];
5358 final var m22 = params[2];
5359
5360 final var m13 = params[3];
5361 final var m23 = params[4];
5362 final var m33 = params[5];
5363
5364 return evaluate(m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33);
5365 }
5366
5367 /**
5368 * Computes estimated true specific force squared norm using current measured
5369 * specific force and provided parameters.
5370 * This method is internally executed during gradient estimation and
5371 * Levenberg-Marquardt fitting needed for calibration computation.
5372 *
5373 * @param m11 element 1,1 of cross-coupling error matrix.
5374 * @param m21 element 2,1 of cross-coupling error matrix.
5375 * @param m31 element 3,1 of cross-coupling error matrix.
5376 * @param m12 element 1,2 of cross-coupling error matrix.
5377 * @param m22 element 2,2 of cross-coupling error matrix.
5378 * @param m32 element 3,2 of cross-coupling error matrix.
5379 * @param m13 element 1,3 of cross-coupling error matrix.
5380 * @param m23 element 2,3 of cross-coupling error matrix.
5381 * @param m33 element 3,3 of cross-coupling error matrix.
5382 * @return estimated true specific force squared norm.
5383 * @throws EvaluationException if there are numerical instabilities.
5384 */
5385 private double evaluate(final double m11, final double m21, final double m31,
5386 final double m12, final double m22, final double m32,
5387 final double m13, final double m23, final double m33) throws EvaluationException {
5388
5389 // fmeas = M*(ftrue + b)
5390
5391 // ftrue = M^-1*fmeas - b
5392
5393 try {
5394 if (fmeas == null) {
5395 fmeas = new Matrix(BodyKinematics.COMPONENTS, 1);
5396 }
5397 if (m == null) {
5398 m = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
5399 }
5400 if (invM == null) {
5401 invM = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
5402 }
5403 if (b == null) {
5404 b = new Matrix(BodyKinematics.COMPONENTS, 1);
5405 }
5406 if (ftrue == null) {
5407 ftrue = new Matrix(BodyKinematics.COMPONENTS, 1);
5408 }
5409 if (ba == null) {
5410 ba = new Matrix(BodyKinematics.COMPONENTS, 1);
5411 }
5412
5413 getBiasAsMatrix(ba);
5414
5415 fmeas.setElementAtIndex(0, fmeasX);
5416 fmeas.setElementAtIndex(1, fmeasY);
5417 fmeas.setElementAtIndex(2, fmeasZ);
5418
5419 m.setElementAt(0, 0, m11);
5420 m.setElementAt(1, 0, m21);
5421 m.setElementAt(2, 0, m31);
5422
5423 m.setElementAt(0, 1, m12);
5424 m.setElementAt(1, 1, m22);
5425 m.setElementAt(2, 1, m32);
5426
5427 m.setElementAt(0, 2, m13);
5428 m.setElementAt(1, 2, m23);
5429 m.setElementAt(2, 2, m33);
5430
5431 Utils.inverse(m, invM);
5432
5433 // b = M^-1*ba
5434 invM.multiply(ba, b);
5435
5436 invM.multiply(fmeas, ftrue);
5437 ftrue.subtract(b);
5438
5439 final var norm = Utils.normF(ftrue);
5440 return norm * norm;
5441
5442 } catch (final AlgebraException e) {
5443 throw new EvaluationException(e);
5444 }
5445 }
5446 }