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