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.FrameBodyKinematics;
28 import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
29 import com.irurueta.units.Acceleration;
30 import com.irurueta.units.AccelerationConverter;
31 import com.irurueta.units.AccelerationUnit;
32
33 import java.util.Collection;
34
35 /**
36 * Estimates accelerometer cross couplings and scaling factors.
37 * This estimator assumes that biases are known.
38 * <p>
39 * This estimator uses a linear approach to find a minimum least squared error
40 * solution.
41 * <p>
42 * To use this estimator at least 4 measurements at different known frames must
43 * be provided. In other words, accelerometer samples must be obtained at 4
44 * different positions, orientations and velocities (although typically velocities are
45 * always zero).
46 * <p>
47 * Measured specific force is assumed to follow the model shown below:
48 * <pre>
49 * fmeas = ba + (I + Ma) * ftrue + w
50 * </pre>
51 * Where:
52 * - fmeas is the measured specific force. This is a 3x1 vector.
53 * - ba is accelerometer bias. This is a known 3x1 vector.
54 * - I is the 3x3 identity matrix.
55 * - Ma is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
56 * a perfect accelerometer, this should be a 3x3 zero matrix.
57 * - ftrue is ground-truth specific force.
58 * - w is measurement noise.
59 */
60 @SuppressWarnings("DuplicatedCode")
61 public class KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator implements
62 KnownBiasAndFrameAccelerometerCalibrator<FrameBodyKinematics,
63 KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener>,
64 UnorderedFrameBodyKinematicsAccelerometerCalibrator {
65
66 /**
67 * Indicates whether by default a common z-axis is assumed for both the accelerometer
68 * and gyroscope.
69 */
70 public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;
71
72 /**
73 * Required minimum number of measurements.
74 */
75 public static final int MINIMUM_MEASUREMENTS = 3;
76
77 /**
78 * Number of equations generated for each measurement.
79 */
80 private static final int EQUATIONS_PER_MEASUREMENT = 3;
81
82 /**
83 * Number of unknowns when common z-axis is assumed for both the accelerometer
84 * and gyroscope.
85 */
86 private static final int COMMON_Z_AXIS_UNKNOWNS = 6;
87
88 /**
89 * Number of unknowns for the general case.
90 */
91 private static final int GENERAL_UNKNOWNS = 9;
92
93 /**
94 * Contains a collection of body kinematics measurements taken at different
95 * frames (positions, orientations and velocities).
96 * If a single device IMU needs to be calibrated, typically all measurements are
97 * taken at the same position, with zero velocity and multiple orientations.
98 * However, if we just want to calibrate a given IMU model (e.g. obtain
99 * an average and less precise calibration for the IMU of a given phone model),
100 * we could take measurements collected throughout the planet at multiple positions
101 * while the phone remains static (e.g. while charging), hence each measurement
102 * position will change, velocity will remain zero and orientation will be
103 * typically constant at horizontal orientation while the phone remains on a
104 * flat surface.
105 */
106 private Collection<FrameBodyKinematics> measurements;
107
108 /**
109 * This flag indicates whether z-axis is assumed to be common for accelerometer
110 * and gyroscope.
111 * When enabled, this eliminates 3 variables from Ma matrix.
112 */
113 private boolean commonAxisUsed = DEFAULT_USE_COMMON_Z_AXIS;
114
115 /**
116 * Listener to handle events raised by this estimator.
117 */
118 private KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener;
119
120 /**
121 * Known x coordinate of accelerometer bias expressed in meters per squared
122 * second (m/s^2).
123 */
124 private double biasX;
125
126 /**
127 * Known y coordinate of accelerometer bias expressed in meters per squared
128 * second (m/s^2).
129 */
130 private double biasY;
131
132 /**
133 * Known z coordinate of accelerometer bias expressed in meters per squared
134 * second (m/s^2).
135 */
136 private double biasZ;
137
138 /**
139 * Estimated accelerometer scale factors and cross coupling errors.
140 * This is the product of matrix Ta containing cross coupling errors and Ka
141 * containing scaling factors.
142 * So tat:
143 * <pre>
144 * Ma = [sx mxy mxz] = Ta*Ka
145 * [myx sy myz]
146 * [mzx mzy sz ]
147 * </pre>
148 * Where:
149 * <pre>
150 * Ka = [sx 0 0 ]
151 * [0 sy 0 ]
152 * [0 0 sz]
153 * </pre>
154 * and
155 * <pre>
156 * Ta = [1 -alphaXy alphaXz ]
157 * [alphaYx 1 -alphaYz]
158 * [-alphaZx alphaZy 1 ]
159 * </pre>
160 * Hence:
161 * <pre>
162 * Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
163 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
164 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
165 * </pre>
166 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
167 * are considered to be zero if the accelerometer z-axis is assumed to be the same
168 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
169 * becomes upper diagonal:
170 * <pre>
171 * Ma = [sx mxy mxz]
172 * [0 sy myz]
173 * [0 0 sz ]
174 * </pre>
175 * Values of this matrix are unit-less.
176 */
177 private Matrix estimatedMa;
178
179 /**
180 * Indicates whether estimator is running.
181 */
182 private boolean running;
183
184 /**
185 * Constructor.
186 */
187 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator() {
188 }
189
190 /**
191 * Constructor.
192 *
193 * @param listener listener to handle events raised by this calibrator.
194 */
195 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
196 final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
197 this.listener = listener;
198 }
199
200 /**
201 * Constructor.
202 *
203 * @param measurements collection of body kinematics measurements taken at
204 * different frames (positions, orientations and velocities).
205 */
206 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
207 final Collection<FrameBodyKinematics> measurements) {
208 this.measurements = measurements;
209 }
210
211 /**
212 * Constructor.
213 *
214 * @param measurements collection of body kinematics measurements taken at
215 * different frames (positions, orientations and velocities).
216 * @param listener listener to handle events raised by this calibrator.
217 */
218 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
219 final Collection<FrameBodyKinematics> measurements,
220 final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
221 this(measurements);
222 this.listener = listener;
223 }
224
225 /**
226 * Constructor.
227 *
228 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
229 * accelerometer and gyroscope.
230 */
231 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(final boolean commonAxisUsed) {
232 this.commonAxisUsed = commonAxisUsed;
233 }
234
235 /**
236 * Constructor.
237 *
238 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
239 * accelerometer and gyroscope.
240 * @param listener listener to handle events raised by this calibrator.
241 */
242 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
243 final boolean commonAxisUsed,
244 final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
245 this(commonAxisUsed);
246 this.listener = listener;
247 }
248
249 /**
250 * Constructor.
251 *
252 * @param measurements collection of body kinematics measurements taken at
253 * different frames (positions, orientations and velocities).
254 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
255 * accelerometer and gyroscope.
256 */
257 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
258 final Collection<FrameBodyKinematics> measurements, final boolean commonAxisUsed) {
259 this(measurements);
260 this.commonAxisUsed = commonAxisUsed;
261 }
262
263 /**
264 * Constructor.
265 *
266 * @param measurements collection of body kinematics measurements taken at
267 * different frames (positions, orientations and velocities).
268 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
269 * accelerometer and gyroscope.
270 * @param listener listener to handle events raised by this calibrator.
271 */
272 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
273 final Collection<FrameBodyKinematics> measurements, final boolean commonAxisUsed,
274 final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
275 this(measurements, commonAxisUsed);
276 this.listener = listener;
277 }
278
279 /**
280 * Constructor.
281 *
282 * @param biasX known x coordinate of accelerometer bias expressed in meters per
283 * squared second (m/s^2).
284 * @param biasY known y coordinate of accelerometer bias expressed in meters per
285 * squared second (m/s^2).
286 * @param biasZ known z coordinate of accelerometer bias expressed in meters per
287 * squared second (m/s^2).
288 */
289 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
290 final double biasX, final double biasY, final double biasZ) {
291 try {
292 setBiasCoordinates(biasX, biasY, biasZ);
293 } catch (final LockedException ignore) {
294 // never happens
295 }
296 }
297
298 /**
299 * Constructor.
300 *
301 * @param biasX known x coordinate of accelerometer bias expressed in meters per
302 * squared second (m/s^2).
303 * @param biasY known y coordinate of accelerometer bias expressed in meters per
304 * squared second (m/s^2).
305 * @param biasZ known z coordinate of accelerometer bias expressed in meters per
306 * squared second (m/s^2).
307 * @param listener listener to handle events raised by this calibrator.
308 */
309 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
310 final double biasX, final double biasY, final double biasZ,
311 final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
312 this(biasX, biasY, biasZ);
313 this.listener = listener;
314 }
315
316 /**
317 * Constructor.
318 *
319 * @param measurements collection of body kinematics measurements taken at
320 * different frames (positions, orientations and velocities).
321 * @param biasX known x coordinate of accelerometer bias expressed in meters
322 * per squared second (m/s^2).
323 * @param biasY known y coordinate of accelerometer bias expressed in meters
324 * per squared second (m/s^2).
325 * @param biasZ known z coordinate of accelerometer bias expressed in meters
326 * per squared second (m/s^2).
327 */
328 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
329 final Collection<FrameBodyKinematics> measurements,
330 final double biasX, final double biasY, final double biasZ) {
331 this(biasX, biasY, biasZ);
332 this.measurements = measurements;
333 }
334
335 /**
336 * Constructor.
337 *
338 * @param measurements collection of body kinematics measurements taken at
339 * different frames (positions, orientations and velocities).
340 * @param biasX known x coordinate of accelerometer bias expressed in meters
341 * per squared second (m/s^2).
342 * @param biasY known y coordinate of accelerometer bias expressed in meters
343 * per squared second (m/s^2).
344 * @param biasZ known z coordinate of accelerometer bias expressed in meters
345 * per squared second (m/s^2).
346 * @param listener listener to handle events raised by this calibrator.
347 */
348 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
349 final Collection<FrameBodyKinematics> measurements,
350 final double biasX, final double biasY, final double biasZ,
351 final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
352 this(measurements, biasX, biasY, biasZ);
353 this.listener = listener;
354 }
355
356 /**
357 * Constructor.
358 *
359 * @param biasX known x coordinate of accelerometer bias expressed in
360 * meters per squared second (m/s^2).
361 * @param biasY known y coordinate of accelerometer bias expressed in
362 * meters per squared second (m/s^2).
363 * @param biasZ known z coordinate of accelerometer bias expressed in
364 * meters per squared second (m/s^2).
365 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
366 * accelerometer and gyroscope.
367 */
368 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
369 final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed) {
370 this(biasX, biasY, biasZ);
371 this.commonAxisUsed = commonAxisUsed;
372 }
373
374 /**
375 * Constructor.
376 *
377 * @param biasX known x coordinate of accelerometer bias expressed in
378 * meters per squared second (m/s^2).
379 * @param biasY known y coordinate of accelerometer bias expressed in
380 * meters per squared second (m/s^2).
381 * @param biasZ known z coordinate of accelerometer bias expressed in
382 * meters per squared second (m/s^2).
383 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
384 * accelerometer and gyroscope.
385 * @param listener listener to handle events raised by this calibrator.
386 */
387 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
388 final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
389 final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
390 this(biasX, biasY, biasZ, commonAxisUsed);
391 this.listener = listener;
392 }
393
394 /**
395 * Constructor.
396 *
397 * @param measurements collection of body kinematics measurements taken at
398 * different frames (positions, orientations and velocities).
399 * @param biasX known x coordinate of accelerometer bias expressed in
400 * meters per squared second (m/s^2).
401 * @param biasY known y coordinate of accelerometer bias expressed in
402 * meters per squared second (m/s^2).
403 * @param biasZ known z coordinate of accelerometer bias expressed in
404 * meters per squared second (m/s^2).
405 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
406 * accelerometer and gyroscope.
407 */
408 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
409 final Collection<FrameBodyKinematics> measurements,
410 final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed) {
411 this(measurements, biasX, biasY, biasZ);
412 this.commonAxisUsed = commonAxisUsed;
413 }
414
415 /**
416 * Constructor.
417 *
418 * @param measurements collection of body kinematics measurements taken at
419 * different frames (positions, orientations and velocities).
420 * @param biasX known x coordinate of accelerometer bias expressed in
421 * meters per squared second (m/s^2).
422 * @param biasY known y coordinate of accelerometer bias expressed in
423 * meters per squared second (m/s^2).
424 * @param biasZ known z coordinate of accelerometer bias expressed in
425 * meters per squared second (m/s^2).
426 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
427 * accelerometer and gyroscope.
428 * @param listener listener to handle events raised by this calibrator.
429 */
430 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
431 final Collection<FrameBodyKinematics> measurements,
432 final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
433 final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
434 this(measurements, biasX, biasY, biasZ, commonAxisUsed);
435 this.listener = listener;
436 }
437
438 /**
439 * Constructor.
440 *
441 * @param biasX known x coordinate of accelerometer bias.
442 * @param biasY known y coordinate of accelerometer bias.
443 * @param biasZ known z coordinate of accelerometer bias.
444 */
445 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
446 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) {
447 try {
448 setBiasCoordinates(biasX, biasY, biasZ);
449 } catch (final LockedException ignore) {
450 // never happens
451 }
452 }
453
454 /**
455 * Constructor.
456 *
457 * @param biasX known x coordinate of accelerometer bias.
458 * @param biasY known y coordinate of accelerometer bias.
459 * @param biasZ known z coordinate of accelerometer bias.
460 * @param listener listener to handle events raised by this calibrator.
461 */
462 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
463 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
464 final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
465 this(biasX, biasY, biasZ);
466 this.listener = listener;
467 }
468
469 /**
470 * Constructor.
471 *
472 * @param measurements collection of body kinematics measurements taken at
473 * different frames (positions, orientations and velocities).
474 * @param biasX known x coordinate of accelerometer bias.
475 * @param biasY known y coordinate of accelerometer bias.
476 * @param biasZ known z coordinate of accelerometer bias.
477 */
478 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
479 final Collection<FrameBodyKinematics> measurements,
480 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) {
481 this(biasX, biasY, biasZ);
482 this.measurements = measurements;
483 }
484
485 /**
486 * Constructor.
487 *
488 * @param measurements collection of body kinematics measurements taken at
489 * different frames (positions, orientations and velocities).
490 * @param biasX known x coordinate of accelerometer bias.
491 * @param biasY known y coordinate of accelerometer bias.
492 * @param biasZ known z coordinate of accelerometer bias.
493 * @param listener listener to handle events raised by this calibrator.
494 */
495 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
496 final Collection<FrameBodyKinematics> measurements,
497 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
498 final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
499 this(measurements, biasX, biasY, biasZ);
500 this.listener = listener;
501 }
502
503 /**
504 * Constructor.
505 *
506 * @param biasX known x coordinate of accelerometer bias.
507 * @param biasY known y coordinate of accelerometer bias.
508 * @param biasZ known z coordinate of accelerometer bias.
509 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
510 * accelerometer and gyroscope.
511 */
512 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
513 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
514 final boolean commonAxisUsed) {
515 this(biasX, biasY, biasZ);
516 this.commonAxisUsed = commonAxisUsed;
517 }
518
519 /**
520 * Constructor.
521 *
522 * @param biasX known x coordinate of accelerometer bias.
523 * @param biasY known y coordinate of accelerometer bias.
524 * @param biasZ known z coordinate of accelerometer bias.
525 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
526 * accelerometer and gyroscope.
527 * @param listener listener to handle events raised by this calibrator.
528 */
529 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
530 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final boolean commonAxisUsed,
531 final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
532 this(biasX, biasY, biasZ, commonAxisUsed);
533 this.listener = listener;
534 }
535
536 /**
537 * Constructor.
538 *
539 * @param measurements collection of body kinematics measurements taken at
540 * different frames (positions, orientations and velocities).
541 * @param biasX known x coordinate of accelerometer bias.
542 * @param biasY known y coordinate of accelerometer bias.
543 * @param biasZ known z coordinate of accelerometer bias.
544 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
545 * accelerometer and gyroscope.
546 */
547 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
548 final Collection<FrameBodyKinematics> measurements,
549 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
550 final boolean commonAxisUsed) {
551 this(measurements, biasX, biasY, biasZ);
552 this.commonAxisUsed = commonAxisUsed;
553 }
554
555 /**
556 * Constructor.
557 *
558 * @param measurements collection of body kinematics measurements taken at
559 * different frames (positions, orientations and velocities).
560 * @param biasX known x coordinate of accelerometer bias.
561 * @param biasY known y coordinate of accelerometer bias.
562 * @param biasZ known z coordinate of accelerometer bias.
563 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
564 * accelerometer and gyroscope.
565 * @param listener listener to handle events raised by this calibrator.
566 */
567 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator(
568 final Collection<FrameBodyKinematics> measurements,
569 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final boolean commonAxisUsed,
570 final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) {
571 this(measurements, biasX, biasY, biasZ, commonAxisUsed);
572 this.listener = listener;
573 }
574
575 /**
576 * Gets a collection of body kinematics measurements taken at different
577 * frames (positions, orientations and velocities).
578 * If a single device IMU needs to be calibrated, typically all measurements are
579 * taken at the same position, with zero velocity and multiple orientations.
580 * However, if we just want to calibrate the a given IMU model (e.g. obtain
581 * an average and less precise calibration for the IMU of a given phone model),
582 * we could take measurements collected throughout the planet at multiple positions
583 * while the phone remains static (e.g. while charging), hence each measurement
584 * position will change, velocity will remain zero and orientation will be
585 * typically constant at horizontal orientation while the phone remains on a
586 * flat surface.
587 *
588 * @return a collection of body kinematics measurements taken at different
589 * frames (positions, orientations and velocities).
590 */
591 @Override
592 public Collection<FrameBodyKinematics> getMeasurements() {
593 return measurements;
594 }
595
596 /**
597 * Sets a collection of body kinematics measurements taken at different
598 * frames (positions, orientations and velocities).
599 * If a single device IMU needs to be calibrated, typically all measurements are
600 * taken at the same position, with zero velocity and multiple orientations.
601 * However, if we just want to calibrate the a given IMU model (e.g. obtain
602 * an average and less precise calibration for the IMU of a given phone model),
603 * we could take measurements collected throughout the planet at multiple positions
604 * while the phone remains static (e.g. while charging), hence each measurement
605 * position will change, velocity will remain zero and orientation will be
606 * typically constant at horizontal orientation while the phone remains on a
607 * flat surface.
608 *
609 * @param measurements collection of body kinematics measurements taken at different
610 * frames (positions, orientations and velocities).
611 * @throws LockedException if estimator is currently running.
612 */
613 @Override
614 public void setMeasurements(final Collection<? extends FrameBodyKinematics> measurements) throws LockedException {
615 if (running) {
616 throw new LockedException();
617 }
618 //noinspection unchecked
619 this.measurements = (Collection<FrameBodyKinematics>) measurements;
620 }
621
622 /**
623 * Indicates the type of measurement used by this calibrator.
624 *
625 * @return type of measurement used by this calibrator.
626 */
627 @Override
628 public AccelerometerCalibratorMeasurementType getMeasurementType() {
629 return AccelerometerCalibratorMeasurementType.FRAME_BODY_KINEMATICS;
630 }
631
632 /**
633 * Indicates whether this calibrator requires ordered measurements in a
634 * list or not.
635 *
636 * @return true if measurements must be ordered, false otherwise.
637 */
638 @Override
639 public boolean isOrderedMeasurementsRequired() {
640 return false;
641 }
642
643 /**
644 * Indicates whether this calibrator requires quality scores for each
645 * measurement or not.
646 *
647 * @return true if quality scores are required, false otherwise.
648 */
649 @Override
650 public boolean isQualityScoresRequired() {
651 return false;
652 }
653
654 /**
655 * Indicates whether z-axis is assumed to be common for accelerometer and
656 * gyroscope.
657 * When enabled, this eliminates 3 variables from Ma matrix.
658 *
659 * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
660 * false otherwise.
661 */
662 @Override
663 public boolean isCommonAxisUsed() {
664 return commonAxisUsed;
665 }
666
667 /**
668 * Specifies whether z-axis is assumed to be common for accelerometer and
669 * gyroscope.
670 * When enabled, this eliminates 3 variables from Ma matrix.
671 *
672 * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
673 * and gyroscope, false otherwise.
674 * @throws LockedException if estimator is currently running.
675 */
676 @Override
677 public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
678 if (running) {
679 throw new LockedException();
680 }
681
682 this.commonAxisUsed = commonAxisUsed;
683 }
684
685 /**
686 * Gets listener to handle events raised by this estimator.
687 *
688 * @return listener to handle events raised by this estimator.
689 */
690 @Override
691 public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener getListener() {
692 return listener;
693 }
694
695 /**
696 * Sets listener to handle events raised by this estimator.
697 *
698 * @param listener listener to handle events raised by this estimator.
699 * @throws LockedException if estimator is currently running.
700 */
701 @Override
702 public void setListener(
703 final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) throws LockedException {
704 if (running) {
705 throw new LockedException();
706 }
707
708 this.listener = listener;
709 }
710
711 /**
712 * Gets known x coordinate of accelerometer bias expressed in meters per squared
713 * second (m/s^2).
714 *
715 * @return x coordinate of accelerometer bias.
716 */
717 @Override
718 public double getBiasX() {
719 return biasX;
720 }
721
722 /**
723 * Sets known x coordinate of accelerometer bias expressed in meters per squared
724 * second (m/s^2).
725 *
726 * @param biasX x coordinate of accelerometer bias.
727 * @throws LockedException if estimator is currently running.
728 */
729 @Override
730 public void setBiasX(final double biasX) throws LockedException {
731 if (running) {
732 throw new LockedException();
733 }
734 this.biasX = biasX;
735 }
736
737 /**
738 * Gets known y coordinate of accelerometer bias expressed in meters per squared
739 * second (m/s^2).
740 *
741 * @return y coordinate of accelerometer bias.
742 */
743 @Override
744 public double getBiasY() {
745 return biasY;
746 }
747
748 /**
749 * Sets known y coordinate of accelerometer bias expressed in meters per squared
750 * second (m/s^2).
751 *
752 * @param biasY y coordinate of accelerometer bias.
753 * @throws LockedException if estimator is currently running.
754 */
755 @Override
756 public void setBiasY(final double biasY) throws LockedException {
757 if (running) {
758 throw new LockedException();
759 }
760
761 this.biasY = biasY;
762 }
763
764 /**
765 * Gets known z coordinate of accelerometer bias expressed in meters per squared
766 * second (m/s^2).
767 *
768 * @return z coordinate of accelerometer bias.
769 */
770 @Override
771 public double getBiasZ() {
772 return biasZ;
773 }
774
775 /**
776 * Sets known z coordinate of accelerometer bias expressed in meters per squared
777 * second (m/s^2).
778 *
779 * @param biasZ z coordinate of accelerometer bias.
780 * @throws LockedException if estimator is currently running.
781 */
782 @Override
783 public void setBiasZ(final double biasZ) throws LockedException {
784 if (running) {
785 throw new LockedException();
786 }
787
788 this.biasZ = biasZ;
789 }
790
791 /**
792 * Gets known x coordinate of accelerometer bias.
793 *
794 * @return x coordinate of accelerometer bias.
795 */
796 @Override
797 public Acceleration getBiasXAsAcceleration() {
798 return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
799 }
800
801 /**
802 * Gets known x coordinate of accelerometer bias.
803 *
804 * @param result instance where result data will be stored.
805 */
806 @Override
807 public void getBiasXAsAcceleration(final Acceleration result) {
808 result.setValue(biasX);
809 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
810 }
811
812 /**
813 * Sets known x coordinate of accelerometer bias.
814 *
815 * @param biasX x coordinate of accelerometer bias.
816 * @throws LockedException if estimator is currently running.
817 */
818 @Override
819 public void setBiasX(final Acceleration biasX) throws LockedException {
820 if (running) {
821 throw new LockedException();
822 }
823
824 this.biasX = convertAcceleration(biasX);
825 }
826
827 /**
828 * Gets known y coordinate of accelerometer bias.
829 *
830 * @return y coordinate of accelerometer bias.
831 */
832 @Override
833 public Acceleration getBiasYAsAcceleration() {
834 return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
835 }
836
837 /**
838 * Gets known y coordinate of accelerometer bias.
839 *
840 * @param result instance where result data will be stored.
841 */
842 @Override
843 public void getBiasYAsAcceleration(final Acceleration result) {
844 result.setValue(biasY);
845 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
846 }
847
848 /**
849 * Sets known y coordinate of accelerometer bias.
850 *
851 * @param biasY y coordinate of accelerometer bias.
852 * @throws LockedException if estimator is currently running.
853 */
854 @Override
855 public void setBiasY(final Acceleration biasY) throws LockedException {
856 if (running) {
857 throw new LockedException();
858 }
859
860 this.biasY = convertAcceleration(biasY);
861 }
862
863 /**
864 * Gets known z coordinate of accelerometer bias.
865 *
866 * @return z coordinate of accelerometer bias.
867 */
868 @Override
869 public Acceleration getBiasZAsAcceleration() {
870 return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
871 }
872
873 /**
874 * Gets known z coordinate of accelerometer bias.
875 *
876 * @param result instance where result data will be stored.
877 */
878 @Override
879 public void getBiasZAsAcceleration(final Acceleration result) {
880 result.setValue(biasZ);
881 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
882 }
883
884 /**
885 * Sets known z coordinate of accelerometer bias.
886 *
887 * @param biasZ z coordinate of accelerometer bias.
888 * @throws LockedException if estimator is currently running.
889 */
890 @Override
891 public void setBiasZ(final Acceleration biasZ) throws LockedException {
892 if (running) {
893 throw new LockedException();
894 }
895
896 this.biasZ = convertAcceleration(biasZ);
897 }
898
899 /**
900 * Sets known accelerometer bias coordinates expressed in meters per squared
901 * second (m/s^2).
902 *
903 * @param biasX x coordinate of accelerometer bias.
904 * @param biasY y coordinate of accelerometer bias.
905 * @param biasZ z coordinate of accelerometer bias.
906 * @throws LockedException if estimator is currently running.
907 */
908 @Override
909 public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
910 if (running) {
911 throw new LockedException();
912 }
913
914 this.biasX = biasX;
915 this.biasY = biasY;
916 this.biasZ = biasZ;
917 }
918
919 /**
920 * Sets known accelerometer bias coordinates.
921 *
922 * @param biasX z coordinate of accelerometer bias.
923 * @param biasY y coordinate of accelerometer bias.
924 * @param biasZ z coordinate of accelerometer bias.
925 * @throws LockedException if estimator is currently running.
926 */
927 @Override
928 public void setBiasCoordinates(final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ)
929 throws LockedException {
930 if (running) {
931 throw new LockedException();
932 }
933
934 this.biasX = convertAcceleration(biasX);
935 this.biasY = convertAcceleration(biasY);
936 this.biasZ = convertAcceleration(biasZ);
937 }
938
939 /**
940 * Gets known accelerometer bias.
941 *
942 * @return known accelerometer bias.
943 */
944 @Override
945 public AccelerationTriad getBiasAsTriad() {
946 return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
947 }
948
949 /**
950 * Gets known accelerometer bias.
951 *
952 * @param result instance where result will be stored.
953 */
954 @Override
955 public void getBiasAsTriad(final AccelerationTriad result) {
956 result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
957 }
958
959 /**
960 * Sets known accelerometer bias.
961 *
962 * @param bias accelerometer bias to be set.
963 * @throws LockedException if calibrator is currently running.
964 */
965 @Override
966 public void setBias(final AccelerationTriad bias) throws LockedException {
967 if (running) {
968 throw new LockedException();
969 }
970
971 biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
972 biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
973 biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
974 }
975
976 /**
977 * Gets known accelerometer bias as an array.
978 * Array values are expressed in meters per squared second (m/s^2).
979 *
980 * @return array containing coordinates of known bias.
981 */
982 @Override
983 public double[] getBias() {
984 final var result = new double[BodyKinematics.COMPONENTS];
985 getBias(result);
986 return result;
987 }
988
989 /**
990 * Gets known accelerometer bias as an array.
991 * Array values are expressed in meters per squared second (m/s^2).
992 *
993 * @param result instance where result data will be copied to.
994 * @throws IllegalArgumentException if provided array does not have length 3.
995 */
996 @Override
997 public void getBias(final double[] result) {
998 if (result.length != BodyKinematics.COMPONENTS) {
999 throw new IllegalArgumentException();
1000 }
1001 result[0] = biasX;
1002 result[1] = biasY;
1003 result[2] = biasZ;
1004 }
1005
1006 /**
1007 * Sets known accelerometer bias as an array.
1008 * Array values are expressed in meters per squared second (m/s^2).
1009 *
1010 * @param bias known accelerometer bias.
1011 * @throws LockedException if calibrator is currently running.
1012 * @throws IllegalArgumentException if provided array does not have length 3.
1013 */
1014 @Override
1015 public void setBias(final double[] bias) throws LockedException {
1016 if (running) {
1017 throw new LockedException();
1018 }
1019
1020 if (bias.length != BodyKinematics.COMPONENTS) {
1021 throw new IllegalArgumentException();
1022 }
1023 biasX = bias[0];
1024 biasY = bias[1];
1025 biasZ = bias[2];
1026 }
1027
1028 /**
1029 * Gets known accelerometer bias as a column matrix.
1030 *
1031 * @return known accelerometer bias as a column matrix.
1032 */
1033 @Override
1034 public Matrix getBiasAsMatrix() {
1035 Matrix result;
1036 try {
1037 result = new Matrix(BodyKinematics.COMPONENTS, 1);
1038 getBiasAsMatrix(result);
1039 } catch (final WrongSizeException ignore) {
1040 // never happens
1041 result = null;
1042 }
1043 return result;
1044 }
1045
1046 /**
1047 * Gets known accelerometer bias as a column matrix.
1048 *
1049 * @param result instance where result data will be copied to.
1050 * @throws IllegalArgumentException if provided matrix is not 3x1.
1051 */
1052 @Override
1053 public void getBiasAsMatrix(final Matrix result) {
1054 if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
1055 throw new IllegalArgumentException();
1056 }
1057 result.setElementAtIndex(0, biasX);
1058 result.setElementAtIndex(1, biasY);
1059 result.setElementAtIndex(2, biasZ);
1060 }
1061
1062 /**
1063 * Sets known accelerometer bias as a column matrix.
1064 *
1065 * @param bias accelerometer bias to be set.
1066 * @throws LockedException if calibrator is currently running
1067 * @throws IllegalArgumentException if provided matrix is not 3x1.
1068 */
1069 @Override
1070 public void setBias(final Matrix bias) throws LockedException {
1071 if (running) {
1072 throw new LockedException();
1073 }
1074 if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
1075 throw new IllegalArgumentException();
1076 }
1077
1078 biasX = bias.getElementAtIndex(0);
1079 biasY = bias.getElementAtIndex(1);
1080 biasZ = bias.getElementAtIndex(2);
1081 }
1082
1083 /**
1084 * Gets minimum number of required measurements.
1085 *
1086 * @return minimum number of required measurements.
1087 */
1088 @Override
1089 public int getMinimumRequiredMeasurements() {
1090 return MINIMUM_MEASUREMENTS;
1091 }
1092
1093 /**
1094 * Indicates whether estimator is ready to start the estimator.
1095 *
1096 * @return true if estimator is ready, false otherwise.
1097 */
1098 @Override
1099 public boolean isReady() {
1100 return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
1101 }
1102
1103 /**
1104 * Indicates whether estimator is currently running or not.
1105 *
1106 * @return true if estimator is running, false otherwise.
1107 */
1108 @Override
1109 public boolean isRunning() {
1110 return running;
1111 }
1112
1113 /**
1114 * Estimates accelerometer calibration parameters containing scale factors
1115 * and cross-coupling errors.
1116 *
1117 * @throws LockedException if estimator is currently running.
1118 * @throws NotReadyException if estimator is not ready.
1119 * @throws CalibrationException if estimation fails for numerical reasons.
1120 */
1121 @Override
1122 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
1123 if (running) {
1124 throw new LockedException();
1125 }
1126
1127 if (!isReady()) {
1128 throw new NotReadyException();
1129 }
1130
1131 try {
1132 running = true;
1133
1134 if (listener != null) {
1135 listener.onCalibrateStart(this);
1136 }
1137
1138 if (commonAxisUsed) {
1139 calibrateCommonAxis();
1140 } else {
1141 calibrateGeneral();
1142 }
1143
1144 if (listener != null) {
1145 listener.onCalibrateEnd(this);
1146 }
1147
1148 } catch (final AlgebraException e) {
1149 throw new CalibrationException(e);
1150 } finally {
1151 running = false;
1152 }
1153 }
1154
1155 /**
1156 * Gets estimated accelerometer scale factors and ross coupling errors.
1157 * This is the product of matrix Ta containing cross coupling errors and Ka
1158 * containing scaling factors.
1159 * So tat:
1160 * <pre>
1161 * Ma = [sx mxy mxz] = Ta*Ka
1162 * [myx sy myz]
1163 * [mzx mzy sz ]
1164 * </pre>
1165 * Where:
1166 * <pre>
1167 * Ka = [sx 0 0 ]
1168 * [0 sy 0 ]
1169 * [0 0 sz]
1170 * </pre>
1171 * and
1172 * <pre>
1173 * Ta = [1 -alphaXy alphaXz ]
1174 * [alphaYx 1 -alphaYz]
1175 * [-alphaZx alphaZy 1 ]
1176 * </pre>
1177 * Hence:
1178 * <pre>
1179 * Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
1180 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
1181 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
1182 * </pre>
1183 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
1184 * are considered to be zero if the accelerometer z-axis is assumed to be the same
1185 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
1186 * becomes upper diagonal:
1187 * <pre>
1188 * Ma = [sx mxy mxz]
1189 * [0 sy myz]
1190 * [0 0 sz ]
1191 * </pre>
1192 * Values of this matrix are unit-less.
1193 *
1194 * @return estimated accelerometer scale factors and cross coupling errors, or null
1195 * if not available.
1196 */
1197 @Override
1198 public Matrix getEstimatedMa() {
1199 return estimatedMa;
1200 }
1201
1202 /**
1203 * Gets estimated x-axis scale factor.
1204 *
1205 * @return estimated x-axis scale factor or null if not available.
1206 */
1207 @Override
1208 public Double getEstimatedSx() {
1209 return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
1210 }
1211
1212 /**
1213 * Gets estimated y-axis scale factor.
1214 *
1215 * @return estimated y-axis scale factor or null if not available.
1216 */
1217 @Override
1218 public Double getEstimatedSy() {
1219 return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
1220 }
1221
1222 /**
1223 * Gets estimated z-axis scale factor.
1224 *
1225 * @return estimated z-axis scale factor or null if not available.
1226 */
1227 @Override
1228 public Double getEstimatedSz() {
1229 return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
1230 }
1231
1232 /**
1233 * Gets estimated x-y cross-coupling error.
1234 *
1235 * @return estimated x-y cross-coupling error or null if not available.
1236 */
1237 @Override
1238 public Double getEstimatedMxy() {
1239 return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
1240 }
1241
1242 /**
1243 * Gets estimated x-z cross-coupling error.
1244 *
1245 * @return estimated x-z cross-coupling error or null if not available.
1246 */
1247 @Override
1248 public Double getEstimatedMxz() {
1249 return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
1250 }
1251
1252 /**
1253 * Gets estimated y-x cross-coupling error.
1254 *
1255 * @return estimated y-x cross-coupling error or null if not available.
1256 */
1257 @Override
1258 public Double getEstimatedMyx() {
1259 return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
1260 }
1261
1262 /**
1263 * Gets estimated y-z cross-coupling error.
1264 *
1265 * @return estimated y-z cross-coupling error or null if not available.
1266 */
1267 @Override
1268 public Double getEstimatedMyz() {
1269 return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
1270 }
1271
1272 /**
1273 * Gets estimated z-x cross-coupling error.
1274 *
1275 * @return estimated z-x cross-coupling error or null if not available.
1276 */
1277 @Override
1278 public Double getEstimatedMzx() {
1279 return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
1280 }
1281
1282 /**
1283 * Gets estimated z-y cross-coupling error.
1284 *
1285 * @return estimated z-y cross-coupling error or null if not available.
1286 */
1287 @Override
1288 public Double getEstimatedMzy() {
1289 return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
1290 }
1291
1292 /**
1293 * Internal method to perform calibration when common z-axis is assumed for both
1294 * the accelerometer and gyroscope.
1295 *
1296 * @throws AlgebraException if there are numerical errors.
1297 */
1298 private void calibrateCommonAxis() throws AlgebraException {
1299 // The accelerometer model is:
1300 // fmeas = ba + (I + Ma) * ftrue + w
1301
1302 // Ideally a least squares solution tries to minimize noise component, so:
1303 // fmeas = ba + (I + Ma) * ftrue
1304
1305 // Hence:
1306 // [fmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [ftruex]
1307 // [fmeasy] = [by] [0 1 0] [myx sy myz] [ftruey]
1308 // [fmeasz] = [bz] [0 0 1] [mzx mzy sz ] [ftruez]
1309
1310 // where myx = mzx = mzy = 0
1311
1312 // Hence:
1313 // [fmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [ftruex]
1314 // [fmeasy] = [by] [0 1 0] [0 sy myz] [ftruey]
1315 // [fmeasz] = [bz] [0 0 1] [0 0 sz ] [ftruez]
1316
1317 // [fmeasx] = [bx] + [1+sx mxy mxz ][ftruex]
1318 // [fmeasy] [by] [0 1+sy myz ][ftruey]
1319 // [fmeasz] [bz] [0 0 1+sz][ftruez]
1320
1321 // fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
1322 // fmeasy = by + (1+sy) * ftruey + myz * ftruez
1323 // fmeasz = bz + (1+sz) * ftruez
1324
1325 // Where the unknowns are: sx, sy, sz, mxy mxz, myz
1326 // Reordering:
1327 // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
1328 // fmeasy = by + ftruey + sy * ftruey + myz * ftruez
1329 // fmeasz = bz + ftruez + sz * ftruez
1330
1331 // fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
1332 // fmeasy - ftruey - by = sy * ftruey + myz * ftruez
1333 // fmeasz - ftruez - bz = sz * ftruez
1334
1335 // [ftruex 0 0 ftruey ftruez 0 ][sx ] = [fmeasx - ftruex - bx]
1336 // [0 ftruey 0 0 0 ftruez][sy ] [fmeasy - ftruey - by]
1337 // [0 0 ftruez 0 0 0 ][sz ] [fmeasz - ftruez - bz]
1338 // [mxy]
1339 // [mxz]
1340 // [myz]
1341
1342 final var expectedKinematics = new BodyKinematics();
1343
1344 final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
1345 final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
1346 final var b = new Matrix(rows, 1);
1347 var i = 0;
1348 for (final var measurement : measurements) {
1349 final var measuredKinematics = measurement.getKinematics();
1350 final var ecefFrame = measurement.getFrame();
1351 final var previousEcefFrame = measurement.getPreviousFrame();
1352 final var timeInterval = measurement.getTimeInterval();
1353
1354 ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
1355
1356 final var fMeasX = measuredKinematics.getFx();
1357 final var fMeasY = measuredKinematics.getFy();
1358 final var fMeasZ = measuredKinematics.getFz();
1359
1360 final var fTrueX = expectedKinematics.getFx();
1361 final var fTrueY = expectedKinematics.getFy();
1362 final var fTrueZ = expectedKinematics.getFz();
1363
1364 a.setElementAt(i, 0, fTrueX);
1365 a.setElementAt(i, 1, 0.0);
1366 a.setElementAt(i, 2, 0.0);
1367 a.setElementAt(i, 3, fTrueY);
1368 a.setElementAt(i, 4, fTrueZ);
1369 a.setElementAt(i, 5, 0.0);
1370
1371 b.setElementAtIndex(i, fMeasX - fTrueX - biasX);
1372 i++;
1373
1374 a.setElementAt(i, 0, 0.0);
1375 a.setElementAt(i, 1, fTrueY);
1376 a.setElementAt(i, 2, 0.0);
1377 a.setElementAt(i, 3, 0.0);
1378 a.setElementAt(i, 4, 0.0);
1379 a.setElementAt(i, 5, fTrueZ);
1380
1381 b.setElementAtIndex(i, fMeasY - fTrueY - biasY);
1382 i++;
1383
1384 a.setElementAt(i, 0, 0.0);
1385 a.setElementAt(i, 1, 0.0);
1386 a.setElementAt(i, 2, fTrueZ);
1387 a.setElementAt(i, 3, 0.0);
1388 a.setElementAt(i, 4, 0.0);
1389 a.setElementAt(i, 5, 0.0);
1390
1391 b.setElementAtIndex(i, fMeasZ - fTrueZ - biasZ);
1392 i++;
1393 }
1394
1395 final var unknowns = Utils.solve(a, b);
1396
1397 final var sx = unknowns.getElementAtIndex(0);
1398 final var sy = unknowns.getElementAtIndex(1);
1399 final var sz = unknowns.getElementAtIndex(2);
1400 final var mxy = unknowns.getElementAtIndex(3);
1401 final var mxz = unknowns.getElementAtIndex(4);
1402 final var myz = unknowns.getElementAtIndex(5);
1403
1404 fillMa(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
1405 }
1406
1407 /**
1408 * Internal method to perform general calibration.
1409 *
1410 * @throws AlgebraException if there are numerical errors.
1411 */
1412 private void calibrateGeneral() throws AlgebraException {
1413 // The accelerometer model is:
1414 // fmeas = ba + (I + Ma) * ftrue + w
1415
1416 // Ideally a least squares solution tries to minimize noise component, so:
1417 // fmeas = ba + (I + Ma) * ftrue
1418
1419 // Hence:
1420 // [fmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [ftruex]
1421 // [fmeasy] = [by] [0 1 0] [myx sy myz] [ftruey]
1422 // [fmeasz] = [bz] [0 0 1] [mzx mzy sz ] [ftruez]
1423
1424 // [fmeasx] = [bx] + [1+sx mxy mxz ][ftruex]
1425 // [fmeasy] [by] [myx 1+sy myz ][ftruey]
1426 // [fmeasz] [bz] [mzx mzy 1+sz][ftruez]
1427
1428 // fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
1429 // fmeasy = by + myx * ftruex + (1+sy) * ftruey + myz * ftruez
1430 // fmeasz = bz + mzx * ftruex + mzy * ftruey + (1+sz) * ftruez
1431
1432 // Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
1433 // Reordering:
1434 // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
1435 // fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
1436 // fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez
1437
1438 // fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
1439 // fmeasy - ftruey - by = myx * ftruex + sy * ftruey + myz * ftruez
1440 // fmeasz - ftruez - bz = mzx * ftruex + mzy * ftruey + sz * ftruez
1441
1442 // [ftruex 0 0 ftruey ftruez 0 0 0 0 ][sx ] = [fmeasx - ftruex - bx]
1443 // [0 ftruey 0 0 0 ftruex ftruez 0 0 ][sy ] [fmeasy - ftruey - by]
1444 // [0 0 ftruez 0 0 0 0 ftruex ftruey][sz ] [fmeasz - ftruez - bz]
1445 // [mxy]
1446 // [mxz]
1447 // [myx]
1448 // [myz]
1449 // [mzx]
1450 // [mzy]
1451
1452 final var expectedKinematics = new BodyKinematics();
1453
1454 final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
1455 final var a = new Matrix(rows, GENERAL_UNKNOWNS);
1456 final var b = new Matrix(rows, 1);
1457 var i = 0;
1458 for (final var measurement : measurements) {
1459 final var measuredKinematics = measurement.getKinematics();
1460 final var ecefFrame = measurement.getFrame();
1461 final var previousEcefFrame = measurement.getPreviousFrame();
1462 final var timeInterval = measurement.getTimeInterval();
1463
1464 ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
1465
1466 final var fMeasX = measuredKinematics.getFx();
1467 final var fMeasY = measuredKinematics.getFy();
1468 final var fMeasZ = measuredKinematics.getFz();
1469
1470 final var fTrueX = expectedKinematics.getFx();
1471 final var fTrueY = expectedKinematics.getFy();
1472 final var fTrueZ = expectedKinematics.getFz();
1473
1474 a.setElementAt(i, 0, fTrueX);
1475 a.setElementAt(i, 1, 0.0);
1476 a.setElementAt(i, 2, 0.0);
1477 a.setElementAt(i, 3, fTrueY);
1478 a.setElementAt(i, 4, fTrueZ);
1479 a.setElementAt(i, 5, 0.0);
1480 a.setElementAt(i, 6, 0.0);
1481 a.setElementAt(i, 7, 0.0);
1482 a.setElementAt(i, 8, 0.0);
1483
1484 b.setElementAtIndex(i, fMeasX - fTrueX - biasX);
1485 i++;
1486
1487 a.setElementAt(i, 0, 0.0);
1488 a.setElementAt(i, 1, fTrueY);
1489 a.setElementAt(i, 2, 0.0);
1490 a.setElementAt(i, 3, 0.0);
1491 a.setElementAt(i, 4, 0.0);
1492 a.setElementAt(i, 5, fTrueX);
1493 a.setElementAt(i, 6, fTrueZ);
1494 a.setElementAt(i, 7, 0.0);
1495 a.setElementAt(i, 8, 0.0);
1496
1497 b.setElementAtIndex(i, fMeasY - fTrueY - biasY);
1498 i++;
1499
1500 a.setElementAt(i, 0, 0.0);
1501 a.setElementAt(i, 1, 0.0);
1502 a.setElementAt(i, 2, fTrueZ);
1503 a.setElementAt(i, 3, 0.0);
1504 a.setElementAt(i, 4, 0.0);
1505 a.setElementAt(i, 5, 0.0);
1506 a.setElementAt(i, 6, 0.0);
1507 a.setElementAt(i, 7, fTrueX);
1508 a.setElementAt(i, 8, fTrueY);
1509
1510 b.setElementAtIndex(i, fMeasZ - fTrueZ - biasZ);
1511 i++;
1512 }
1513
1514 final var unknowns = Utils.solve(a, b);
1515
1516 final var sx = unknowns.getElementAtIndex(0);
1517 final var sy = unknowns.getElementAtIndex(1);
1518 final var sz = unknowns.getElementAtIndex(2);
1519 final var mxy = unknowns.getElementAtIndex(3);
1520 final var mxz = unknowns.getElementAtIndex(4);
1521 final var myx = unknowns.getElementAtIndex(5);
1522 final var myz = unknowns.getElementAtIndex(6);
1523 final var mzx = unknowns.getElementAtIndex(7);
1524 final var mzy = unknowns.getElementAtIndex(8);
1525
1526 fillMa(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
1527 }
1528
1529 /**
1530 * Fills scale factor and cross coupling error matrix with estimated values.
1531 *
1532 * @param sx x scale factor
1533 * @param sy y scale factor
1534 * @param sz z scale factor
1535 * @param mxy x-y cross coupling
1536 * @param mxz x-z cross coupling
1537 * @param myx y-x cross coupling
1538 * @param myz y-z cross coupling
1539 * @param mzx z-x cross coupling
1540 * @param mzy z-y cross coupling
1541 * @throws WrongSizeException never happens.
1542 */
1543 private void fillMa(final double sx, final double sy, final double sz,
1544 final double mxy, final double mxz, final double myx,
1545 final double myz, final double mzx, final double mzy) throws WrongSizeException {
1546 if (estimatedMa == null) {
1547 estimatedMa = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
1548 }
1549
1550 estimatedMa.setElementAt(0, 0, sx);
1551 estimatedMa.setElementAt(1, 0, myx);
1552 estimatedMa.setElementAt(2, 0, mzx);
1553
1554 estimatedMa.setElementAt(0, 1, mxy);
1555 estimatedMa.setElementAt(1, 1, sy);
1556 estimatedMa.setElementAt(2, 1, mzy);
1557
1558 estimatedMa.setElementAt(0, 2, mxz);
1559 estimatedMa.setElementAt(1, 2, myz);
1560 estimatedMa.setElementAt(2, 2, sz);
1561 }
1562
1563 /**
1564 * Converts acceleration value and unit to meters per squared second.
1565 *
1566 * @param value acceleration value.
1567 * @param unit unit of acceleration value.
1568 * @return converted value.
1569 */
1570 private static double convertAcceleration(final double value, final AccelerationUnit unit) {
1571 return AccelerationConverter.convert(value, unit, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1572 }
1573
1574 /**
1575 * Converts acceleration instance to meters per squared second.
1576 *
1577 * @param acceleration acceleration instance to be converted.
1578 * @return converted value.
1579 */
1580 private static double convertAcceleration(final Acceleration acceleration) {
1581 return convertAcceleration(acceleration.getValue().doubleValue(), acceleration.getUnit());
1582 }
1583 }