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;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20 import com.irurueta.units.Acceleration;
21 import com.irurueta.units.AccelerationConverter;
22 import com.irurueta.units.AccelerationUnit;
23 import com.irurueta.units.AngularSpeed;
24 import com.irurueta.units.AngularSpeedConverter;
25 import com.irurueta.units.AngularSpeedUnit;
26
27 import java.io.Serial;
28 import java.io.Serializable;
29 import java.util.Arrays;
30 import java.util.Objects;
31
32 /**
33 * Contains Inertial Measurement Unit (IMU) errors statistics obtained from
34 * calibration.
35 * This data can also be used to generate synthetic IMU data.
36 * IMU errors are related to accelerometer and gyroscope calibration parameters.
37 */
38 public class IMUErrors implements Serializable, Cloneable {
39
40 /**
41 * Number of components of accelerometer measures.
42 */
43 public static final int ACCELEROMETER_COMPONENTS = 3;
44
45 /**
46 * Number of components og gyro measures.
47 */
48 public static final int GYRO_COMPONENTS = 3;
49
50 /**
51 * Number of components minus one.
52 */
53 private static final int COMPONENTS_MINUS_ONE = 2;
54
55 /**
56 * Serialization version. This is used to ensure compatibility of deserialization of permanently stored serialized
57 * instances.
58 */
59 @Serial
60 private static final long serialVersionUID = 0L;
61
62 /**
63 * Accelerometer biases for each IMU axis expressed in meters per squared
64 * second (m/s^2).
65 * By default, it is assumed to be all zeros.
66 */
67 private double[] accelerometerBiases = new double[ACCELEROMETER_COMPONENTS];
68
69 /**
70 * Gyro biases for each IMU axis expressed in radians per second (rad/s).
71 * By default, it is assumed to be all zeros.
72 */
73 private double[] gyroBiases = new double[GYRO_COMPONENTS];
74
75 /**
76 * Contains accelerometer scale factors and cross coupling errors.
77 * This is the product of matrix Ta containing cross coupling errors and Ka
78 * containing scaling factors.
79 * So that:
80 * <pre>
81 * Ma = [sx mxy mxz] = Ta*Ka
82 * [myx sy myz]
83 * [mzx mzy sz ]
84 * </pre>
85 * Where:
86 * <pre>
87 * Ka = [sx 0 0 ]
88 * [0 sy 0 ]
89 * [0 0 sz]
90 * </pre>
91 * and
92 * <pre>
93 * Ta = [1 -alphaXy alphaXz ]
94 * [alphaYx 1 -alphaYz]
95 * [-alphaZx alphaZy 1 ]
96 * </pre>
97 * Hence:
98 * <pre>
99 * Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
100 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
101 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
102 * </pre>
103 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
104 * are considered to be zero if the accelerometer z-axis is assumed to be the same
105 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
106 * becomes upper diagonal:
107 * <pre>
108 * Ma = [sx mxy mxz]
109 * [0 sy myz]
110 * [0 0 sz ]
111 * </pre>
112 * Values of this matrix are unit-less.
113 * By default, it is the 3x3 zero matrix.
114 */
115 private Matrix accelerometerScaleFactorAndCrossCouplingErrors;
116
117 /**
118 * Contains gyro scale factors and cross coupling errors.
119 * This is the product of matrix Tg containing cross coupling errors and Kg
120 * containing scaling factors.
121 * So that:
122 * <pre>
123 * Mg = [sx mxy mxz] = Tg*Kg
124 * [myx sy myz]
125 * [mzx mzy sz ]
126 * </pre>
127 * Where:
128 * <pre>
129 * Kg = [sx 0 0 ]
130 * [0 sy 0 ]
131 * [0 0 sz]
132 * </pre>
133 * and
134 * <pre>
135 * Tg = [1 -gammaXy gammaXz ]
136 * [gammaYx 1 -gammaYz]
137 * [-gammaZx gammaZy 1 ]
138 * </pre>
139 * Hence:
140 * <pre>
141 * Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * gammaXy sz * gammaXz ]
142 * [myx sy myz] [sx * gammaYx sy -sz * gammaYz]
143 * [mzx mzy sz ] [-sx * gammaZx sy * gammaZy sz ]
144 * </pre>
145 * This instance allows any 3x3 matrix however, typically gammaYx, gammaZx and gammaZy
146 * are considered to be zero if the accelerometer z-axis is assumed to be the same
147 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
148 * becomes upper diagonal:
149 * <pre>
150 * Ma = [sx mxy mxz]
151 * [0 sy myz]
152 * [0 0 sz ]
153 * </pre>
154 * Values of this matrix are unit-less.
155 * By default, it is the 3x3 zero matrix.
156 */
157 private Matrix gyroScaleFactorAndCrossCouplingErrors;
158
159 /**
160 * 3x3 matrix containing cross biases introduced by the specific forces sensed
161 * by the accelerometer.
162 * Values of this matrix are expressed in (rad-sec/m).
163 * By default, it is all zeros.
164 */
165 private Matrix gyroGDependentBiases;
166
167 /**
168 * Accelerometer noise root PSD expressed in (m * s^-1.5).
169 * By default it is zero.
170 */
171 private double accelerometerNoiseRootPSD;
172
173 /**
174 * Gyro noise root PSD expressed in (rad * s^-0.5).
175 * By default, it is zero.
176 */
177 private double gyroNoiseRootPSD;
178
179 /**
180 * Accelerometer quantization level expressed in meters per squared second (m/s^2).
181 * By default, it is zero when no quantization is assumed.
182 */
183 private double accelerometerQuantizationLevel;
184
185 /**
186 * Gyro quantization level expressed in radians per second (rad/s).
187 * By default, it is zero when no quantization is assumed.
188 */
189 private double gyroQuantizationLevel;
190
191 /**
192 * Constructor.
193 */
194 public IMUErrors() {
195 try {
196 accelerometerScaleFactorAndCrossCouplingErrors = Matrix.identity(
197 ACCELEROMETER_COMPONENTS, ACCELEROMETER_COMPONENTS);
198 gyroScaleFactorAndCrossCouplingErrors = Matrix.identity(GYRO_COMPONENTS, GYRO_COMPONENTS);
199 gyroGDependentBiases = new Matrix(ACCELEROMETER_COMPONENTS, ACCELEROMETER_COMPONENTS);
200 } catch (final WrongSizeException ignore) {
201 // never happens
202 }
203 }
204
205 /**
206 * Constructor.
207 *
208 * @param accelerometerBiases accelerometer biases for each IMU axis expressed in meters
209 * per squared second (m/s^2). Must have length 3.
210 * @param gyroBiases gyro biases for each IMU axis expressed in radians per
211 * second (rad/s). Must have length 3.
212 * @param accelerometerScaleFactorAndCrossCouplingErrors accelerometer scale factors and cross coupling errors.
213 * Must be 3x3.
214 * @param gyroScaleFactorAndCrossCouplingErrors gyro scale factors and cross coupling errors. Must be 3x3.
215 * @param accelerometerNoiseRootPSD accelerometer noise root PSD expressed in (m * s^-1.5).
216 * @param gyroNoiseRootPSD gyro noise root PSD expressed in (rad * s^-0.5).
217 * @throws IllegalArgumentException if any value is invalid.
218 */
219 public IMUErrors(final double[] accelerometerBiases, final double[] gyroBiases,
220 final Matrix accelerometerScaleFactorAndCrossCouplingErrors,
221 final Matrix gyroScaleFactorAndCrossCouplingErrors, final double accelerometerNoiseRootPSD,
222 final double gyroNoiseRootPSD) {
223 this();
224 setAccelerometerBiases(accelerometerBiases);
225 setGyroBiases(gyroBiases);
226 setAccelerometerScaleFactorAndCrossCouplingErrors(accelerometerScaleFactorAndCrossCouplingErrors);
227 setGyroScaleFactorAndCrossCouplingErrors(gyroScaleFactorAndCrossCouplingErrors);
228 setAccelerometerNoiseRootPSD(accelerometerNoiseRootPSD);
229 setGyroNoiseRootPSD(gyroNoiseRootPSD);
230 }
231
232 /**
233 * Constructor.
234 *
235 * @param accelerometerBiases accelerometer biases for each IMU axis expressed in meters
236 * per squared second (m/s^2). Must be 3x1.
237 * @param gyroBiases gyro biases for each IMU axis expressed in radians per
238 * second (rad/s). Must be 3x1.
239 * @param accelerometerScaleFactorAndCrossCouplingErrors accelerometer scale factors and cross coupling errors.
240 * Must be 3x3.
241 * @param gyroScaleFactorAndCrossCouplingErrors gyro scale factors and cross coupling errors. Must be 3x3.
242 * @param accelerometerNoiseRootPSD accelerometer noise root PSD expressed in (m * s^-1.5).
243 * @param gyroNoiseRootPSD gyro noise root PSD expressed in (rad * s^-0.5).
244 * @throws IllegalArgumentException if any value is invalid.
245 */
246 public IMUErrors(final Matrix accelerometerBiases, final Matrix gyroBiases,
247 final Matrix accelerometerScaleFactorAndCrossCouplingErrors,
248 final Matrix gyroScaleFactorAndCrossCouplingErrors, final double accelerometerNoiseRootPSD,
249 final double gyroNoiseRootPSD) {
250 this();
251 setAccelerometerBiases(accelerometerBiases);
252 setGyroBiases(gyroBiases);
253 setAccelerometerScaleFactorAndCrossCouplingErrors(accelerometerScaleFactorAndCrossCouplingErrors);
254 setGyroScaleFactorAndCrossCouplingErrors(gyroScaleFactorAndCrossCouplingErrors);
255 setAccelerometerNoiseRootPSD(accelerometerNoiseRootPSD);
256 setGyroNoiseRootPSD(gyroNoiseRootPSD);
257 }
258
259 /**
260 * Constructor.
261 *
262 * @param accelerometerBiases accelerometer biases for each IMU axis. Must have length 3.
263 * @param gyroBiases gyro biases for each IMU axis. Must have length 3.
264 * @param accelerometerScaleFactorAndCrossCouplingErrors accelerometer scale factors and cross coupling errors.
265 * Must be 3x3.
266 * @param gyroScaleFactorAndCrossCouplingErrors gyro scale factors and cross coupling errors. Must be 3x3.
267 * @param accelerometerNoiseRootPSD accelerometer noise root PSD expressed in (m * s^-1.5).
268 * @param gyroNoiseRootPSD gyro noise root PSD expressed in (rad * s^-0.5).
269 * @throws IllegalArgumentException if any value is invalid.
270 */
271 public IMUErrors(final Acceleration[] accelerometerBiases, final AngularSpeed[] gyroBiases,
272 final Matrix accelerometerScaleFactorAndCrossCouplingErrors,
273 final Matrix gyroScaleFactorAndCrossCouplingErrors, final double accelerometerNoiseRootPSD,
274 final double gyroNoiseRootPSD) {
275 this();
276 setAccelerometerBiases(accelerometerBiases);
277 setGyroBiases(gyroBiases);
278 setAccelerometerScaleFactorAndCrossCouplingErrors(accelerometerScaleFactorAndCrossCouplingErrors);
279 setGyroScaleFactorAndCrossCouplingErrors(gyroScaleFactorAndCrossCouplingErrors);
280 setAccelerometerNoiseRootPSD(accelerometerNoiseRootPSD);
281 setGyroNoiseRootPSD(gyroNoiseRootPSD);
282 }
283
284 /**
285 * Constructor.
286 *
287 * @param accelerometerBiases accelerometer biases for each IMU axis expressed in meters
288 * per squared second (m/s^2). Must have length 3.
289 * @param gyroBiases gyro biases for each IMU axis expressed in radians per
290 * second (rad/s). Must have length 3.
291 * @param accelerometerScaleFactorAndCrossCouplingErrors accelerometer scale factors and cross coupling errors.
292 * Must be 3x3.
293 * @param gyroScaleFactorAndCrossCouplingErrors gyro scale factors and cross coupling errors. Must be 3x3.
294 * @param gyroGDependentBiases Cross biases introduced by the specific forces sensed by
295 * the accelerometer expressed in (rad-sec/m). Must be 3x3.
296 * @param accelerometerNoiseRootPSD accelerometer noise root PSD expressed in (m * s^-1.5).
297 * @param gyroNoiseRootPSD gyro noise root PSD expressed in (rad * s^-0.5).
298 * @param accelerometerQuantizationLevel accelerometer quantization level expressed in meters per
299 * squared second (m/s^2).
300 * @param gyroQuantizationLevel gyro quantization level expressed in radians per second
301 * (rad/s).
302 * @throws IllegalArgumentException if any value is invalid.
303 */
304 public IMUErrors(final double[] accelerometerBiases, final double[] gyroBiases,
305 final Matrix accelerometerScaleFactorAndCrossCouplingErrors,
306 final Matrix gyroScaleFactorAndCrossCouplingErrors, final Matrix gyroGDependentBiases,
307 final double accelerometerNoiseRootPSD, final double gyroNoiseRootPSD,
308 final double accelerometerQuantizationLevel, final double gyroQuantizationLevel) {
309 this();
310 setAccelerometerBiases(accelerometerBiases);
311 setGyroBiases(gyroBiases);
312 setAccelerometerScaleFactorAndCrossCouplingErrors(accelerometerScaleFactorAndCrossCouplingErrors);
313 setGyroScaleFactorAndCrossCouplingErrors(gyroScaleFactorAndCrossCouplingErrors);
314 setGyroGDependentBiases(gyroGDependentBiases);
315 setAccelerometerNoiseRootPSD(accelerometerNoiseRootPSD);
316 setGyroNoiseRootPSD(gyroNoiseRootPSD);
317 setAccelerometerQuantizationLevel(accelerometerQuantizationLevel);
318 setGyroQuantizationLevel(gyroQuantizationLevel);
319 }
320
321 /**
322 * Constructor.
323 *
324 * @param accelerometerBiases accelerometer biases for each IMU axis expressed in meters
325 * per squared second (m/s^2). Must be 3x1.
326 * @param gyroBiases gyro biases for each IMU axis expressed in radians per
327 * second (rad/s). Must be 3x1.
328 * @param accelerometerScaleFactorAndCrossCouplingErrors accelerometer scale factors and cross coupling errors.
329 * Must be 3x3.
330 * @param gyroScaleFactorAndCrossCouplingErrors gyro scale factors and cross coupling errors. Must be 3x3.
331 * @param gyroGDependentBiases Cross biases introduced by the specific forces sensed by
332 * the accelerometer expressed in (rad-sec/m). Must be 3x3.
333 * @param accelerometerNoiseRootPSD accelerometer noise root PSD expressed in (m * s^-1.5).
334 * @param gyroNoiseRootPSD gyro noise root PSD expressed in (rad * s^-0.5).
335 * @param accelerometerQuantizationLevel accelerometer quantization level expressed in meters per
336 * squared second (m/s^2).
337 * @param gyroQuantizationLevel gyro quantization level expressed in radians per second
338 * (rad/s).
339 * @throws IllegalArgumentException if any value is invalid.
340 */
341 public IMUErrors(final Matrix accelerometerBiases, final Matrix gyroBiases,
342 final Matrix accelerometerScaleFactorAndCrossCouplingErrors,
343 final Matrix gyroScaleFactorAndCrossCouplingErrors, final Matrix gyroGDependentBiases,
344 final double accelerometerNoiseRootPSD, final double gyroNoiseRootPSD,
345 final double accelerometerQuantizationLevel, final double gyroQuantizationLevel) {
346 this();
347 setAccelerometerBiases(accelerometerBiases);
348 setGyroBiases(gyroBiases);
349 setAccelerometerScaleFactorAndCrossCouplingErrors(accelerometerScaleFactorAndCrossCouplingErrors);
350 setGyroScaleFactorAndCrossCouplingErrors(gyroScaleFactorAndCrossCouplingErrors);
351 setGyroGDependentBiases(gyroGDependentBiases);
352 setAccelerometerNoiseRootPSD(accelerometerNoiseRootPSD);
353 setGyroNoiseRootPSD(gyroNoiseRootPSD);
354 setAccelerometerQuantizationLevel(accelerometerQuantizationLevel);
355 setGyroQuantizationLevel(gyroQuantizationLevel);
356 }
357
358 /**
359 * Constructor.
360 *
361 * @param accelerometerBiases accelerometer biases for each IMU axis. Must have length 3.
362 * @param gyroBiases gyro biases for each IMU axis. Must have length 3.
363 * @param accelerometerScaleFactorAndCrossCouplingErrors accelerometer scale factors and cross coupling errors.
364 * Must be 3x3.
365 * @param gyroScaleFactorAndCrossCouplingErrors gyro scale factors and cross coupling errors. Must be 3x3.
366 * @param gyroGDependentBiases Cross biases introduced by the specific forces sensed by
367 * the accelerometer expressed in (rad-sec/m). Must be 3x3.
368 * @param accelerometerNoiseRootPSD accelerometer noise root PSD expressed in (m * s^-1.5).
369 * @param gyroNoiseRootPSD gyro noise root PSD expressed in (rad * s^-0.5).
370 * @param accelerometerQuantizationLevel accelerometer quantization level.
371 * @param gyroQuantizationLevel gyro quantization level.
372 * @throws IllegalArgumentException if any value is invalid.
373 */
374 public IMUErrors(final Acceleration[] accelerometerBiases, final AngularSpeed[] gyroBiases,
375 final Matrix accelerometerScaleFactorAndCrossCouplingErrors,
376 final Matrix gyroScaleFactorAndCrossCouplingErrors, final Matrix gyroGDependentBiases,
377 final double accelerometerNoiseRootPSD, final double gyroNoiseRootPSD,
378 final Acceleration accelerometerQuantizationLevel,
379 final AngularSpeed gyroQuantizationLevel) {
380 this();
381 setAccelerometerBiases(accelerometerBiases);
382 setGyroBiases(gyroBiases);
383 setAccelerometerScaleFactorAndCrossCouplingErrors(accelerometerScaleFactorAndCrossCouplingErrors);
384 setGyroScaleFactorAndCrossCouplingErrors(gyroScaleFactorAndCrossCouplingErrors);
385 setGyroGDependentBiases(gyroGDependentBiases);
386 setAccelerometerNoiseRootPSD(accelerometerNoiseRootPSD);
387 setGyroNoiseRootPSD(gyroNoiseRootPSD);
388 setAccelerometerQuantizationLevel(accelerometerQuantizationLevel);
389 setGyroQuantizationLevel(gyroQuantizationLevel);
390 }
391
392 /**
393 * Constructor.
394 *
395 * @param input instance to copy data from.
396 */
397 @SuppressWarnings("CopyConstructorMissesField")
398 public IMUErrors(final IMUErrors input) {
399 this();
400 copyFrom(input);
401 }
402
403 /**
404 * Gets accelerometer biases for each IMU axis expressed in meters per squared
405 * second (m/s^2).
406 * By default, it is assumed to be all zeros.
407 *
408 * @return accelerometer biases for each IMU axis.
409 */
410 public double[] getAccelerometerBiases() {
411 final var result = new double[ACCELEROMETER_COMPONENTS];
412 getAccelerometerBiases(result);
413 return result;
414 }
415
416 /**
417 * Gets accelerometer biases for each IMU axis expressed in meters per squared
418 * second (m/s^2).
419 * By default, it is assumed to be all zeros.
420 *
421 * @param result instance where data will be stored.
422 * @throws IllegalArgumentException if provided array does not have length 3.
423 */
424 public void getAccelerometerBiases(final double[] result) {
425 if (result.length != ACCELEROMETER_COMPONENTS) {
426 throw new IllegalArgumentException();
427 }
428
429 System.arraycopy(accelerometerBiases, 0, result, 0, ACCELEROMETER_COMPONENTS);
430 }
431
432 /**
433 * Sets accelerometer biases for each IMU axis expressed in meters per squared
434 * second (m/s^2).
435 *
436 * @param accelerometerBiases accelerometer biases for each IMU axis.
437 * @throws IllegalArgumentException if provided array does not have length 3.
438 */
439 public void setAccelerometerBiases(final double[] accelerometerBiases) {
440 if (accelerometerBiases.length != ACCELEROMETER_COMPONENTS) {
441 throw new IllegalArgumentException();
442 }
443
444 this.accelerometerBiases = accelerometerBiases;
445 }
446
447 /**
448 * Gets accelerometer biases for each IMU axis expressed in meters per squared
449 * second (m/s^2) as a column matrix.
450 * By default, it is assumed to be all zeros.
451 *
452 * @return 3x1 column matrix containing accelerometer biases for each IMU axis.
453 */
454 public Matrix getAccelerometerBiasesAsMatrix() {
455 return Matrix.newFromArray(accelerometerBiases);
456 }
457
458 /**
459 * Gets accelerometer biases for each IMU axis expressed in meters per squared
460 * second (m/s^2) as a column matrix.
461 * By default, it is assumed to be all zeros.
462 *
463 * @param result instance where data will be stored.
464 * @throws IllegalArgumentException if provided result matrix is not 3x1.
465 */
466 public void getAccelerometerBiasesAsMatrix(final Matrix result) {
467 result.setSubmatrix(0, 0, COMPONENTS_MINUS_ONE, 0,
468 accelerometerBiases);
469 }
470
471 /**
472 * Sets accelerometer biases for each IMU axis expressed in meters per squared
473 * second (m/s^2) from a 3x1 column matrix.
474 *
475 * @param accelerometerBiases 3x1 column matrix containing values to be set.
476 * @throws IllegalArgumentException if provided matrix is not 3x1.
477 */
478 public void setAccelerometerBiases(final Matrix accelerometerBiases) {
479 if (accelerometerBiases.getRows() != ACCELEROMETER_COMPONENTS || accelerometerBiases.getColumns() != 1) {
480 throw new IllegalArgumentException();
481 }
482
483 try {
484 accelerometerBiases.getSubmatrixAsArray(0, 0,
485 COMPONENTS_MINUS_ONE, 0, this.accelerometerBiases);
486 } catch (final WrongSizeException ignore) {
487 // never happens
488 }
489 }
490
491 /**
492 * Gets accelerometer biases for each IMU axis.
493 * By default, it is assumed to be all zeros.
494 *
495 * @return accelerometer biases for each IMU axis.
496 */
497 public Acceleration[] getAccelerometerBiasesAsAcceleration() {
498 final var result = new Acceleration[ACCELEROMETER_COMPONENTS];
499 getAccelerometerBiasesAsAcceleration(result);
500 return result;
501 }
502
503 /**
504 * Gets accelerometer biases for each IMU axis.
505 * By default, it is assumed to be all zeros.
506 *
507 * @param result instance where data will be copied to.
508 * @throws IllegalArgumentException if provided array does not have length 3.
509 */
510 public void getAccelerometerBiasesAsAcceleration(final Acceleration[] result) {
511 if (result.length != ACCELEROMETER_COMPONENTS) {
512 throw new IllegalArgumentException();
513 }
514
515 for (var i = 0; i < ACCELEROMETER_COMPONENTS; i++) {
516 final var a = result[i];
517 if (a == null) {
518 result[i] = new Acceleration(accelerometerBiases[i], AccelerationUnit.METERS_PER_SQUARED_SECOND);
519 } else {
520 a.setValue(accelerometerBiases[i]);
521 a.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
522 }
523 }
524 }
525
526 /**
527 * Sets accelerometer biases for each IMU axis.
528 *
529 * @param accelerometerBiases accelerometer biases to be set.
530 * @throws IllegalArgumentException if provided array does not have length 3.
531 */
532 public void setAccelerometerBiases(final Acceleration[] accelerometerBiases) {
533 if (accelerometerBiases.length != ACCELEROMETER_COMPONENTS) {
534 throw new IllegalArgumentException();
535 }
536
537 for (var i = 0; i < ACCELEROMETER_COMPONENTS; i++) {
538 this.accelerometerBiases[i] = convertAcceleration(accelerometerBiases[i]);
539 }
540 }
541
542 /**
543 * Gets gyro biases for each IMU axis expressed in radians per second (rad/s).
544 * By default, it is assumed to be all zeros.
545 *
546 * @return gyro biases for each IMU axis.
547 */
548 public double[] getGyroBiases() {
549 final var result = new double[GYRO_COMPONENTS];
550 getGyroBiases(result);
551 return result;
552 }
553
554 /**
555 * Gets gyro biases for each IMU axis expressed in radians per second (rad/s).
556 * By default, it is assumed to be all zeros.
557 *
558 * @param result instance where data will be stored.
559 * @throws IllegalArgumentException if provided array does not have length 3.
560 */
561 public void getGyroBiases(final double[] result) {
562 if (result.length != GYRO_COMPONENTS) {
563 throw new IllegalArgumentException();
564 }
565
566 System.arraycopy(gyroBiases, 0, result, 0, GYRO_COMPONENTS);
567 }
568
569 /**
570 * Sets gyro biases for each IMU axis expressed in radians per second (rad/s).
571 *
572 * @param gyroBiases gyro biases for each IMU axis.
573 * @throws IllegalArgumentException if provided array does not have length 3.
574 */
575 public void setGyroBiases(final double[] gyroBiases) {
576 if (gyroBiases.length != GYRO_COMPONENTS) {
577 throw new IllegalArgumentException();
578 }
579
580 this.gyroBiases = gyroBiases;
581 }
582
583 /**
584 * Gets gyro biases for each IMU axis expressed in radians per second (rad/s)
585 * as a column matrix.
586 * By default, it is assumed to be all zeros.
587 *
588 * @return 3x1 column matrix containing gyro biases for each IMU axis.
589 */
590 public Matrix getGyroBiasesAsMatrix() {
591 return Matrix.newFromArray(gyroBiases);
592 }
593
594 /**
595 * Gets gyro biases for each IMU axis expressed in radians per second (rad/s)
596 * as a column matrix.
597 * By default, it is assumed to be all zeros.
598 *
599 * @param result instance where data will be stored.
600 * @throws IllegalArgumentException if provided result matrix is not 3x1.
601 */
602 public void getGyroBiasesAsMatrix(final Matrix result) {
603 result.setSubmatrix(0, 0, COMPONENTS_MINUS_ONE, 0, gyroBiases);
604 }
605
606 /**
607 * Sets gyro biases for each IMU axis expressed in radians per second (rad/s)
608 * from a 3x1 column matrix.
609 *
610 * @param gyroBiases 3x1 column matrix containing values to be set.
611 * @throws IllegalArgumentException if provided matrix is not 3x1.
612 */
613 public void setGyroBiases(final Matrix gyroBiases) {
614 if (gyroBiases.getRows() != GYRO_COMPONENTS || gyroBiases.getColumns() != 1) {
615 throw new IllegalArgumentException();
616 }
617
618 try {
619 gyroBiases.getSubmatrixAsArray(0, 0, COMPONENTS_MINUS_ONE, 0,
620 this.gyroBiases);
621 } catch (final WrongSizeException ignore) {
622 // never happens
623 }
624 }
625
626 /**
627 * Gets gyro biases for each IMU axis.
628 * By default, it is assumed to be all zeros.
629 *
630 * @return gyro biases for each IMU axis.
631 */
632 public AngularSpeed[] getGyroBiasesAsAngularSpeed() {
633 final var result = new AngularSpeed[GYRO_COMPONENTS];
634 getGyroBiasesAsAngularSpeed(result);
635 return result;
636 }
637
638 /**
639 * Gets gyro biases for each IMU axis.
640 * By default, it is assumed to be all zeros.
641 *
642 * @param result instance where data will be copied to.
643 * @throws IllegalArgumentException if provided array does not have length 3.
644 */
645 public void getGyroBiasesAsAngularSpeed(final AngularSpeed[] result) {
646 if (result.length != GYRO_COMPONENTS) {
647 throw new IllegalArgumentException();
648 }
649
650 for (var i = 0; i < GYRO_COMPONENTS; i++) {
651 final var as = result[i];
652 if (as == null) {
653 result[i] = new AngularSpeed(gyroBiases[i], AngularSpeedUnit.RADIANS_PER_SECOND);
654 } else {
655 as.setValue(gyroBiases[i]);
656 as.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
657 }
658 }
659 }
660
661 /**
662 * Sets gyro biases for each IMU axis.
663 *
664 * @param gyroBiases gyro biases to be set.
665 * @throws IllegalArgumentException if provided array does not have length 3.
666 */
667 public void setGyroBiases(final AngularSpeed[] gyroBiases) {
668 if (gyroBiases.length != GYRO_COMPONENTS) {
669 throw new IllegalArgumentException();
670 }
671
672 for (int i = 0; i < GYRO_COMPONENTS; i++) {
673 this.gyroBiases[i] = convertAngularSpeed(gyroBiases[i]);
674 }
675 }
676
677 /**
678 * Gets accelerometer scale factors and cross coupling errors.
679 * This is the product of matrix Ta containing cross coupling errors and Ka
680 * containing scaling factors.
681 * So that:
682 * <pre>
683 * Ma = [sx mxy mxz] = Ta*Ka
684 * [myx sy myz]
685 * [mzx mzy sz ]
686 * </pre>
687 * Where:
688 * <pre>
689 * Ka = [sx 0 0 ]
690 * [0 sy 0 ]
691 * [0 0 sz]
692 * </pre>
693 * and
694 * <pre>
695 * Ta = [1 -alphaXy alphaXz ]
696 * [alphaYx 1 -alphaYz]
697 * [-alphaZx alphaZy 1 ]
698 * </pre>
699 * Hence:
700 * <pre>
701 * Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
702 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
703 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
704 * </pre>
705 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
706 * are considered to be zero if the accelerometer z-axis is assumed to be the same
707 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
708 * becomes upper diagonal:
709 * <pre>
710 * Ma = [sx mxy mxz]
711 * [0 sy myz]
712 * [0 0 sz ]
713 * </pre>
714 * Values of this matrix are unit-less.
715 * By default, it is the 3x3 identity matrix.
716 *
717 * @return accelerometer scale factors and cross coupling errors.
718 */
719 public Matrix getAccelerometerScaleFactorAndCrossCouplingErrors() {
720 return new Matrix(accelerometerScaleFactorAndCrossCouplingErrors);
721 }
722
723 /**
724 * Gets accelerometer scale factors and cross coupling errors.
725 * This is the product of matrix Ta containing cross coupling errors and Ka
726 * containing scaling factors.
727 * So that:
728 * <pre>
729 * Ma = [sx mxy mxz] = Ta*Ka
730 * [myx sy myz]
731 * [mzx mzy sz ]
732 * </pre>
733 * Where:
734 * <pre>
735 * Ka = [sx 0 0 ]
736 * [0 sy 0 ]
737 * [0 0 sz]
738 * </pre>
739 * and
740 * <pre>
741 * Ta = [1 -alphaXy alphaXz ]
742 * [alphaYx 1 -alphaYz]
743 * [-alphaZx alphaZy 1 ]
744 * </pre>
745 * Hence:
746 * <pre>
747 * Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
748 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
749 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
750 * </pre>
751 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
752 * are considered to be zero if the accelerometer z-axis is assumed to be the same
753 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
754 * becomes upper diagonal:
755 * <pre>
756 * Ma = [sx mxy mxz]
757 * [0 sy myz]
758 * [0 0 sz ]
759 * </pre>
760 * Values of this matrix are unit-less.
761 * By default, it is the 3x3 identity matrix.
762 *
763 * @param result instance where data of scale factor and cross coupling matrix will
764 * be copied to. If needed, result instance will be resized.
765 */
766 public void getAccelerometerScaleFactorAndCrossCouplingErrors(final Matrix result) {
767 result.copyFrom(accelerometerScaleFactorAndCrossCouplingErrors);
768 }
769
770 /**
771 * Sets accelerometer scale factors and cross coupling errors.
772 * This is the product of matrix Ta containing cross coupling errors and Ka
773 * containing scaling factors.
774 * So that:
775 * <pre>
776 * Ma = [sx mxy mxz] = Ta*Ka
777 * [myx sy myz]
778 * [mzx mzy sz ]
779 * </pre>
780 * Where:
781 * <pre>
782 * Ka = [sx 0 0 ]
783 * [0 sy 0 ]
784 * [0 0 sz]
785 * </pre>
786 * and
787 * <pre>
788 * Ta = [1 -alphaXy alphaXz ]
789 * [alphaYx 1 -alphaYz]
790 * [-alphaZx alphaZy 1 ]
791 * </pre>
792 * Hence:
793 * <pre>
794 * Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
795 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
796 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
797 * </pre>
798 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
799 * are considered to be zero if the accelerometer z-axis is assumed to be the same
800 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
801 * becomes upper diagonal:
802 * <pre>
803 * Ma = [sx mxy mxz]
804 * [0 sy myz]
805 * [0 0 sz ]
806 * </pre>
807 * Values of this matrix are unit-less.
808 *
809 * @param accelerometerScaleFactorAndCrossCouplingErrors scale factors and cross coupling
810 * matrix to be set.
811 * @throws IllegalArgumentException if provided matrix is not 3x3.
812 */
813 public void setAccelerometerScaleFactorAndCrossCouplingErrors(
814 final Matrix accelerometerScaleFactorAndCrossCouplingErrors) {
815 if (accelerometerScaleFactorAndCrossCouplingErrors.getRows() != ACCELEROMETER_COMPONENTS
816 || accelerometerScaleFactorAndCrossCouplingErrors.getColumns() != ACCELEROMETER_COMPONENTS) {
817 throw new IllegalArgumentException();
818 }
819
820 this.accelerometerScaleFactorAndCrossCouplingErrors.copyFrom(accelerometerScaleFactorAndCrossCouplingErrors);
821 }
822
823 /**
824 * Gets gyro scale factors and cross coupling errors.
825 * This is the product of matrix Tg containing cross coupling errors and Kg
826 * containing scaling factors.
827 * So that:
828 * <pre>
829 * Mg = [sx mxy mxz] = Tg*Kg
830 * [myx sy myz]
831 * [mzx mzy sz ]
832 * </pre>
833 * Where:
834 * <pre>
835 * Kg = [sx 0 0 ]
836 * [0 sy 0 ]
837 * [0 0 sz]
838 * </pre>
839 * and
840 * <pre>
841 * Tg = [1 -gammaXy gammaXz ]
842 * [gammaYx 1 -gammaYz]
843 * [-gammaZx gammaZy 1 ]
844 * </pre>
845 * Hence:
846 * <pre>
847 * Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * gammaXy sz * gammaXz ]
848 * [myx sy myz] [sx * gammaYx sy -sz * gammaYz]
849 * [mzx mzy sz ] [-sx * gammaZx sy * gammaZy sz ]
850 * </pre>
851 * This instance allows any 3x3 matrix however, typically gammaYx, gammaZx and gammaZy
852 * are considered to be zero if the accelerometer z-axis is assumed to be the same
853 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
854 * becomes upper diagonal:
855 * <pre>
856 * Ma = [sx mxy mxz]
857 * [0 sy myz]
858 * [0 0 sz ]
859 * </pre>
860 * Values of this matrix are unit-less.
861 * By default, it is the 3x3 identity matrix.
862 *
863 * @return gyro scale factors and cross coupling errors.
864 */
865 public Matrix getGyroScaleFactorAndCrossCouplingErrors() {
866 return new Matrix(gyroScaleFactorAndCrossCouplingErrors);
867 }
868
869 /**
870 * Gets gyro scale factors and cross coupling errors.
871 * This is the product of matrix Tg containing cross coupling errors and Kg
872 * containing scaling factors.
873 * So that:
874 * <pre>
875 * Mg = [sx mxy mxz] = Tg*Kg
876 * [myx sy myz]
877 * [mzx mzy sz ]
878 * </pre>
879 * Where:
880 * <pre>
881 * Kg = [sx 0 0 ]
882 * [0 sy 0 ]
883 * [0 0 sz]
884 * </pre>
885 * and
886 * <pre>
887 * Tg = [1 -gammaXy gammaXz ]
888 * [gammaYx 1 -gammaYz]
889 * [-gammaZx gammaZy 1 ]
890 * </pre>
891 * Hence:
892 * <pre>
893 * Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * gammaXy sz * gammaXz ]
894 * [myx sy myz] [sx * gammaYx sy -sz * gammaYz]
895 * [mzx mzy sz ] [-sx * gammaZx sy * gammaZy sz ]
896 * </pre>
897 * This instance allows any 3x3 matrix however, typically gammaYx, gammaZx and gammaZy
898 * are considered to be zero if the accelerometer z-axis is assumed to be the same
899 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
900 * becomes upper diagonal:
901 * <pre>
902 * Ma = [sx mxy mxz]
903 * [0 sy myz]
904 * [0 0 sz ]
905 * </pre>
906 * Values of this matrix are unit-less.
907 * By default, it is the 3x3 identity matrix.
908 *
909 * @param result instance where data of scale factor and cross coupling matrix will
910 * be copied to. If needed, result instance will be resized.
911 */
912 public void getGyroScaleFactorAndCrossCouplingErrors(final Matrix result) {
913 result.copyFrom(gyroScaleFactorAndCrossCouplingErrors);
914 }
915
916 /**
917 * Sets gyro scale factors and cross coupling errors.
918 * This is the product of matrix Tg containing cross coupling errors and Kg
919 * containing scaling factors.
920 * So that:
921 * <pre>
922 * Mg = [sx mxy mxz] = Tg*Kg
923 * [myx sy myz]
924 * [mzx mzy sz ]
925 * </pre>
926 * Where:
927 * <pre>
928 * Kg = [sx 0 0 ]
929 * [0 sy 0 ]
930 * [0 0 sz]
931 * </pre>
932 * and
933 * <pre>
934 * Tg = [1 -gammaXy gammaXz ]
935 * [gammaYx 1 -gammaYz]
936 * [-gammaZx gammaZy 1 ]
937 * </pre>
938 * Hence:
939 * <pre>
940 * Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * gammaXy sz * gammaXz ]
941 * [myx sy myz] [sx * gammaYx sy -sz * gammaYz]
942 * [mzx mzy sz ] [-sx * gammaZx sy * gammaZy sz ]
943 * </pre>
944 * This instance allows any 3x3 matrix however, typically gammaYx, gammaZx and gammaZy
945 * are considered to be zero if the accelerometer z-axis is assumed to be the same
946 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
947 * becomes upper diagonal:
948 * <pre>
949 * Ma = [sx mxy mxz]
950 * [0 sy myz]
951 * [0 0 sz ]
952 * </pre>
953 * Values of this matrix are unit-less.
954 *
955 * @param gyroScaleFactorAndCrossCouplingErrors scale factors and cross coupling
956 * matrix to be set.
957 * @throws IllegalArgumentException if provided matrix is not 3x3.
958 */
959 public void setGyroScaleFactorAndCrossCouplingErrors(final Matrix gyroScaleFactorAndCrossCouplingErrors) {
960 if (gyroScaleFactorAndCrossCouplingErrors.getRows() != GYRO_COMPONENTS
961 || gyroScaleFactorAndCrossCouplingErrors.getColumns() != GYRO_COMPONENTS) {
962 throw new IllegalArgumentException();
963 }
964
965 this.gyroScaleFactorAndCrossCouplingErrors.copyFrom(gyroScaleFactorAndCrossCouplingErrors);
966 }
967
968 /**
969 * Gets 3x3 matrix containing cross biases introduced by the specific forces
970 * sensed by the accelerometer.
971 * Values of this matrix are expressed in (rad-sec/m).
972 * By default, it is all zeros.
973 *
974 * @return cross biases introduced by the specific forces sensed by the
975 * accelerometer.
976 */
977 public Matrix getGyroGDependentBiases() {
978 return new Matrix(gyroGDependentBiases);
979 }
980
981 /**
982 * Gets 3x3 matrix containing cross biases introduced by the specific forces
983 * sensed by the accelerometer.
984 * Values of this matrix are expressed in (rad-sec/m).
985 * By default, it is all zeros.
986 *
987 * @param result instance where data will be stored. If needed, result instance
988 * will be resized.
989 */
990 public void getGyroGDependentBiases(final Matrix result) {
991 result.copyFrom(gyroGDependentBiases);
992 }
993
994 /**
995 * Sets 3x3 matrix containing cross biases introduced by the specific forces
996 * sensed by the accelerometer.
997 * Values of this matrix are expressed in (rad-sec/m).
998 *
999 * @param gyroGDependentBiases cross biases introduced by the specific forces
1000 * sensed by the accelerometer to be set.
1001 * @throws IllegalArgumentException if provided matrix is not 3x3.
1002 */
1003 public void setGyroGDependentBiases(final Matrix gyroGDependentBiases) {
1004 if (gyroGDependentBiases.getRows() != ACCELEROMETER_COMPONENTS
1005 || gyroGDependentBiases.getColumns() != ACCELEROMETER_COMPONENTS) {
1006 throw new IllegalArgumentException();
1007 }
1008
1009 this.gyroGDependentBiases.copyFrom(gyroGDependentBiases);
1010 }
1011
1012 /**
1013 * Gets accelerometer noise root PSD expressed in (m * s^-1.5).
1014 * By default it is zero.
1015 *
1016 * @return accelerometer noise root PSD.
1017 */
1018 public double getAccelerometerNoiseRootPSD() {
1019 return accelerometerNoiseRootPSD;
1020 }
1021
1022 /**
1023 * Sets accelerometer noise root PSD expressed in (m * s^-1.5)
1024 *
1025 * @param accelerometerNoiseRootPSD accelerometer noise root PSD to be set.
1026 */
1027 public void setAccelerometerNoiseRootPSD(final double accelerometerNoiseRootPSD) {
1028 this.accelerometerNoiseRootPSD = accelerometerNoiseRootPSD;
1029 }
1030
1031 /**
1032 * Gets accelerometer noise PSD expressed in (m^2 * s^-3).
1033 * By default, it is zero.
1034 *
1035 * @return accelerometer noise PSD.
1036 */
1037 public double getAccelerometerNoisePSD() {
1038 return accelerometerNoiseRootPSD * accelerometerNoiseRootPSD;
1039 }
1040
1041 /**
1042 * Sets accelerometer noise PSD expressed in (m^2 * s^-3).
1043 *
1044 * @param accelerometerNoisePSD accelerometer noise PSD to be set.
1045 * @throws IllegalArgumentException if provided value is negative.
1046 */
1047 public void setAccelerometerNoisePSD(final double accelerometerNoisePSD) {
1048 if (accelerometerNoisePSD < 0.0) {
1049 throw new IllegalArgumentException();
1050 }
1051
1052 accelerometerNoiseRootPSD = Math.sqrt(accelerometerNoisePSD);
1053 }
1054
1055 /**
1056 * Gets gyro noise root PSD expressed in (rad * s^-0.5).
1057 * By default, it is zero.
1058 *
1059 * @return gyro noise root PSD.
1060 */
1061 public double getGyroNoiseRootPSD() {
1062 return gyroNoiseRootPSD;
1063 }
1064
1065 /**
1066 * Sets gyro noise root PSD expressed in (rad * s^-0.5).
1067 *
1068 * @param gyroNoiseRootPSD gyro noise root PSD to be set.
1069 */
1070 public void setGyroNoiseRootPSD(final double gyroNoiseRootPSD) {
1071 this.gyroNoiseRootPSD = gyroNoiseRootPSD;
1072 }
1073
1074 /**
1075 * Gets gyro noise PSD expressed in (rad^2/s).
1076 * By default, it is zero.
1077 *
1078 * @return gyro noise PSD.
1079 */
1080 public double getGyroNoisePSD() {
1081 return gyroNoiseRootPSD * gyroNoiseRootPSD;
1082 }
1083
1084 /**
1085 * Sets gyro noise PSD expressed in (rad^2/s).
1086 *
1087 * @param gyroNoisePSD gyro noise PSD.
1088 * @throws IllegalArgumentException if provided value is negative.
1089 */
1090 public void setGyroNoisePSD(final double gyroNoisePSD) {
1091 if (gyroNoisePSD < 0.0) {
1092 throw new IllegalArgumentException();
1093 }
1094
1095 gyroNoiseRootPSD = Math.sqrt(gyroNoisePSD);
1096 }
1097
1098 /**
1099 * Gets accelerometer quantization level expressed in meters per squared second
1100 * (m/s^2).
1101 * By default, it is zero when no quantization is assumed.
1102 *
1103 * @return accelerometer quantization level.
1104 */
1105 public double getAccelerometerQuantizationLevel() {
1106 return accelerometerQuantizationLevel;
1107 }
1108
1109 /**
1110 * Sets accelerometer quantization level expressed in meters per squared second
1111 * (m/s^2).
1112 *
1113 * @param accelerometerQuantizationLevel accelerometer quantization level to be
1114 * set.
1115 */
1116 public void setAccelerometerQuantizationLevel(final double accelerometerQuantizationLevel) {
1117 this.accelerometerQuantizationLevel = accelerometerQuantizationLevel;
1118 }
1119
1120 /**
1121 * Gets accelerometer quantization level.
1122 * By default, it is zero when no quantization is assumed.
1123 *
1124 * @return accelerometer quantization level.
1125 */
1126 public Acceleration getAccelerometerQuantizationLevelAsAcceleration() {
1127 return new Acceleration(accelerometerQuantizationLevel, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1128 }
1129
1130 /**
1131 * Gets accelerometer quantization level.
1132 * By default, it is zero when no quantization is assumed.
1133 *
1134 * @param result instance where value will be stored.
1135 */
1136 public void getAccelerometerQuantizationLevelAsAcceleration(final Acceleration result) {
1137 result.setValue(accelerometerQuantizationLevel);
1138 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
1139 }
1140
1141 /**
1142 * Sets accelerometer quantization level.
1143 *
1144 * @param accelerometerQuantizationLevel accelerometer quantization level to be set.
1145 */
1146 public void setAccelerometerQuantizationLevel(final Acceleration accelerometerQuantizationLevel) {
1147 this.accelerometerQuantizationLevel = convertAcceleration(accelerometerQuantizationLevel);
1148 }
1149
1150 /**
1151 * Gets gyro quantization level expressed in radians per second (rad/s).
1152 * By default, it is zero when no quantization is assumed.
1153 *
1154 * @return gyro quantization level expressed in radians per second.
1155 */
1156 public double getGyroQuantizationLevel() {
1157 return gyroQuantizationLevel;
1158 }
1159
1160 /**
1161 * Sets gyro quantization level expressed in radians per second (rad/s).
1162 *
1163 * @param gyroQuantizationLevel gyro quantization level to be set.
1164 */
1165 public void setGyroQuantizationLevel(final double gyroQuantizationLevel) {
1166 this.gyroQuantizationLevel = gyroQuantizationLevel;
1167 }
1168
1169 /**
1170 * Gets gyro quantization level.
1171 * By default, it is zero when no quantization is assumed.
1172 *
1173 * @return gyro quantization level.
1174 */
1175 public AngularSpeed getGyroQuantizationLevelAsAngularSpeed() {
1176 return new AngularSpeed(gyroQuantizationLevel, AngularSpeedUnit.RADIANS_PER_SECOND);
1177 }
1178
1179 /**
1180 * Gets gyro quantization level.
1181 * By default, it is zero when no quantization is assumed.
1182 *
1183 * @param result instance where value will be stored.
1184 */
1185 public void getGyroQuantizationLevelAsAngularSpeed(final AngularSpeed result) {
1186 result.setValue(gyroQuantizationLevel);
1187 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
1188 }
1189
1190 /**
1191 * Sets gyro quantization level.
1192 *
1193 * @param gyroQuantizationLevel gyro quantization level.
1194 */
1195 public void setGyroQuantizationLevel(final AngularSpeed gyroQuantizationLevel) {
1196 this.gyroQuantizationLevel = convertAngularSpeed(gyroQuantizationLevel);
1197 }
1198
1199 /**
1200 * Copies this instance data into provided instance.
1201 *
1202 * @param output destination instance where data will be copied to.
1203 */
1204 public void copyTo(final IMUErrors output) {
1205 output.copyFrom(this);
1206 }
1207
1208 /**
1209 * Copies data of provided instance into this instance.
1210 *
1211 * @param input instance to copy data from.
1212 */
1213 public void copyFrom(final IMUErrors input) {
1214 input.getAccelerometerBiases(accelerometerBiases);
1215 input.getGyroBiases(gyroBiases);
1216 accelerometerScaleFactorAndCrossCouplingErrors.copyFrom(input.accelerometerScaleFactorAndCrossCouplingErrors);
1217 gyroScaleFactorAndCrossCouplingErrors.copyFrom(input.gyroScaleFactorAndCrossCouplingErrors);
1218 gyroGDependentBiases.copyFrom(input.gyroGDependentBiases);
1219 accelerometerNoiseRootPSD = input.accelerometerNoiseRootPSD;
1220 gyroNoiseRootPSD = input.gyroNoiseRootPSD;
1221 accelerometerQuantizationLevel = input.accelerometerQuantizationLevel;
1222 gyroQuantizationLevel = input.gyroQuantizationLevel;
1223 }
1224
1225 /**
1226 * Computes and returns hash code for this instance. Hash codes are almost unique
1227 * values that are useful for fast classification and storage of objects in
1228 * collections.
1229 *
1230 * @return Hash code.
1231 */
1232 @Override
1233 public int hashCode() {
1234 return Objects.hash(accelerometerScaleFactorAndCrossCouplingErrors, gyroScaleFactorAndCrossCouplingErrors,
1235 gyroGDependentBiases, accelerometerNoiseRootPSD, gyroNoiseRootPSD, accelerometerQuantizationLevel,
1236 gyroQuantizationLevel, Arrays.hashCode(accelerometerBiases), Arrays.hashCode(gyroBiases));
1237 }
1238
1239 /**
1240 * Checks if provided object is an IMUErrors instance having exactly the same
1241 * contents as this instance.
1242 *
1243 * @param o Object to be compared.
1244 * @return true if both objects are considered to be equal, false otherwise.
1245 */
1246 @Override
1247 public boolean equals(Object o) {
1248 if (this == o) {
1249 return true;
1250 }
1251 if (o == null || getClass() != o.getClass()) {
1252 return false;
1253 }
1254
1255 final var imuErrors = (IMUErrors) o;
1256 return Double.compare(imuErrors.accelerometerNoiseRootPSD, accelerometerNoiseRootPSD) == 0 &&
1257 Double.compare(imuErrors.gyroNoiseRootPSD, gyroNoiseRootPSD) == 0 &&
1258 Double.compare(imuErrors.accelerometerQuantizationLevel, accelerometerQuantizationLevel) == 0 &&
1259 Double.compare(imuErrors.gyroQuantizationLevel, gyroQuantizationLevel) == 0 &&
1260 Arrays.equals(accelerometerBiases, imuErrors.accelerometerBiases) &&
1261 Arrays.equals(gyroBiases, imuErrors.gyroBiases) &&
1262 accelerometerScaleFactorAndCrossCouplingErrors.equals(
1263 imuErrors.accelerometerScaleFactorAndCrossCouplingErrors) &&
1264 gyroScaleFactorAndCrossCouplingErrors.equals(imuErrors.gyroScaleFactorAndCrossCouplingErrors) &&
1265 gyroGDependentBiases.equals(imuErrors.gyroGDependentBiases);
1266 }
1267
1268 /**
1269 * Makes a copy of this instance.
1270 *
1271 * @return a copy of this instance.
1272 * @throws CloneNotSupportedException if clone fails for some reason.
1273 */
1274 @Override
1275 protected Object clone() throws CloneNotSupportedException {
1276 final var result = (IMUErrors) super.clone();
1277 copyTo(result);
1278 return result;
1279 }
1280
1281 /**
1282 * Converts acceleration instance to meters per squared second (m/s^2).
1283 *
1284 * @param acceleration instance to be converted.
1285 * @return converted value.
1286 */
1287 private double convertAcceleration(final Acceleration acceleration) {
1288 return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
1289 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1290 }
1291
1292 /**
1293 * Converts angular speed instance to radians per second (rad/s).
1294 *
1295 * @param angularSpeed instance ot be converted.
1296 * @return converted value.
1297 */
1298 private double convertAngularSpeed(final AngularSpeed angularSpeed) {
1299 return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit(),
1300 AngularSpeedUnit.RADIANS_PER_SECOND);
1301 }
1302 }