1 /*
2 * Copyright (C) 2021 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.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.navigation.inertial.BodyKinematics;
21 import com.irurueta.units.Acceleration;
22 import com.irurueta.units.AngularSpeed;
23
24 /**
25 * Fixes body kinematics (acceleration + angular rate) values taking into
26 * account provided biases, cross coupling errors and G-dependent errors.
27 */
28 public class BodyKinematicsFixer {
29
30 /**
31 * Fixes specific force (acceleration) components of body kinematics measurements.
32 */
33 private final AccelerationFixer accelerationFixer = new AccelerationFixer();
34
35 /**
36 * Fixes angular rate components of body kinematics measurements.
37 */
38 private final AngularRateFixer angularRateFixer = new AngularRateFixer();
39
40 /**
41 * Contains measured acceleration to be reused.
42 */
43 private final AccelerationTriad measuredAcceleration = new AccelerationTriad();
44
45 /**
46 * Contains measured angular speed to be reused.
47 */
48 private final AngularSpeedTriad measuredAngularSpeed = new AngularSpeedTriad();
49
50 /**
51 * Contains fixed acceleration to be reused.
52 */
53 private final AccelerationTriad fixedAcceleration = new AccelerationTriad();
54
55 /**
56 * Contains fixed angular speed to be reused.
57 */
58 private final AngularSpeedTriad fixedAngularSpeed = new AngularSpeedTriad();
59
60 /**
61 * Gets acceleration bias values expressed in meters per squared second (m/s^2).
62 *
63 * @return bias values expressed in meters per squared second.
64 */
65 public Matrix getAccelerationBias() {
66 return accelerationFixer.getBias();
67 }
68
69 /**
70 * Gets acceleration bias values expressed in meters per squared second (m/s^2).
71 *
72 * @param result instance where result will be stored.
73 */
74 public void getAccelerationBias(final Matrix result) {
75 accelerationFixer.getBias(result);
76 }
77
78 /**
79 * Sets acceleration bias values expressed in meters per squared second (m/s^2).
80 *
81 * @param bias bias values expressed in meters per squared second.
82 * Must be 3x1.
83 * @throws IllegalArgumentException if provided matrix is not 3x1.
84 */
85 public void setAccelerationBias(final Matrix bias) {
86 accelerationFixer.setBias(bias);
87 }
88
89 /**
90 * Gets acceleration bias values expressed in meters per squared second (m/s^2).
91 *
92 * @return bias values expressed in meters per squared second.
93 */
94 public double[] getAccelerationBiasArray() {
95 return accelerationFixer.getBiasArray();
96 }
97
98 /**
99 * Gets acceleration bias values expressed in meters per squared second (m/s^2).
100 *
101 * @param result instance where result data will be stored.
102 * @throws IllegalArgumentException if provided array does not have length 3.
103 */
104 public void getAccelerationBiasArray(final double[] result) {
105 accelerationFixer.getBiasArray(result);
106 }
107
108 /**
109 * Sets acceleration bias values expressed in meters per squared second (m/s^2).
110 *
111 * @param bias bias values expressed in meters per squared second (m/s^2).
112 * Must have length 3.
113 * @throws IllegalArgumentException if provided array does not have length 3.
114 */
115 public void setAccelerationBias(final double[] bias) {
116 accelerationFixer.setBias(bias);
117 }
118
119 /**
120 * Gets acceleration bias.
121 *
122 * @return acceleration bias.
123 */
124 public AccelerationTriad getAccelerationBiasAsTriad() {
125 return accelerationFixer.getBiasAsTriad();
126 }
127
128 /**
129 * Gets acceleration bias.
130 *
131 * @param result instance where result will be stored.
132 */
133 public void getAccelerationBiasAsTriad(final AccelerationTriad result) {
134 accelerationFixer.getBiasAsTriad(result);
135 }
136
137 /**
138 * Sets acceleration bias.
139 *
140 * @param bias acceleration bias to be set.
141 */
142 public void setAccelerationBias(final AccelerationTriad bias) {
143 accelerationFixer.setBias(bias);
144 }
145
146 /**
147 * Gets acceleration x-coordinate of bias expressed in meters per squared
148 * second (m/s^2).
149 *
150 * @return x-coordinate of bias expressed in meters per squared second (m/s^2).
151 */
152 public double getAccelerationBiasX() {
153 return accelerationFixer.getBiasX();
154 }
155
156 /**
157 * Sets acceleration x-coordinate of bias expressed in meters per squared
158 * second (m/s^2).
159 *
160 * @param biasX x-coordinate of bias expressed in meters per squared second
161 * (m/s^2).
162 */
163 public void setAccelerationBiasX(final double biasX) {
164 accelerationFixer.setBiasX(biasX);
165 }
166
167 /**
168 * Gets acceleration y-coordinate of bias expressed in meters per squared
169 * second (m/s^2).
170 *
171 * @return y-coordinate of bias expressed in meters per squared second (m/s^2).
172 */
173 public double getAccelerationBiasY() {
174 return accelerationFixer.getBiasY();
175 }
176
177 /**
178 * Sets acceleration y-coordinate of bias expressed in meters per squared
179 * second (m/s^2).
180 *
181 * @param biasY y-coordinate of bias expressed in meters per squared second
182 * (m/s^2).
183 */
184 public void setAccelerationBiasY(final double biasY) {
185 accelerationFixer.setBiasY(biasY);
186 }
187
188 /**
189 * Gets acceleration z-coordinate of bias expressed in meters per squared
190 * second (m/s^2).
191 *
192 * @return z-coordinate of bias expressed in meters per squared second (m/s^2).
193 */
194 public double getAccelerationBiasZ() {
195 return accelerationFixer.getBiasZ();
196 }
197
198 /**
199 * Sets acceleration z-coordinate of bias expressed in meters per squared
200 * second (m/s^2).
201 *
202 * @param biasZ z-coordinate of bias expressed in meters per squared second (m/s^2).
203 */
204 public void setAccelerationBiasZ(final double biasZ) {
205 accelerationFixer.setBiasZ(biasZ);
206 }
207
208 /**
209 * Sets acceleration coordinates of bias expressed in meters per squared
210 * second (m/s^2).
211 *
212 * @param biasX x-coordinate of bias.
213 * @param biasY y-coordinate of bias.
214 * @param biasZ z-coordinate of bias.
215 */
216 public void setAccelerationBias(final double biasX, final double biasY, final double biasZ) {
217 accelerationFixer.setBias(biasX, biasY, biasZ);
218 }
219
220 /**
221 * Gets acceleration x-coordinate of bias.
222 *
223 * @return acceleration x-coordinate of bias.
224 */
225 public Acceleration getAccelerationBiasXAsAcceleration() {
226 return accelerationFixer.getBiasXAsAcceleration();
227 }
228
229 /**
230 * Gets acceleration x-coordinate of bias.
231 *
232 * @param result instance where result will be stored.
233 */
234 public void getAccelerationBiasXAsAcceleration(final Acceleration result) {
235 accelerationFixer.getBiasXAsAcceleration(result);
236 }
237
238 /**
239 * Sets acceleration x-coordinate of bias.
240 *
241 * @param biasX acceleration x-coordinate of bias.
242 */
243 public void setAccelerationBiasX(final Acceleration biasX) {
244 accelerationFixer.setBiasX(biasX);
245 }
246
247 /**
248 * Gets acceleration y-coordinate of bias.
249 *
250 * @return acceleration y-coordinate of bias.
251 */
252 public Acceleration getAccelerationBiasYAsAcceleration() {
253 return accelerationFixer.getBiasYAsAcceleration();
254 }
255
256 /**
257 * Gets acceleration y-coordinate of bias.
258 *
259 * @param result instance where result will be stored.
260 */
261 public void getAccelerationBiasYAsAcceleration(final Acceleration result) {
262 accelerationFixer.getBiasYAsAcceleration(result);
263 }
264
265 /**
266 * Sets acceleration y-coordinate of bias.
267 *
268 * @param biasY acceleration y-coordinate of bias.
269 */
270 public void setAccelerationBiasY(final Acceleration biasY) {
271 accelerationFixer.setBiasY(biasY);
272 }
273
274 /**
275 * Gets acceleration z-coordinate of bias.
276 *
277 * @return acceleration z-coordinate of bias.
278 */
279 public Acceleration getAccelerationBiasZAsAcceleration() {
280 return accelerationFixer.getBiasZAsAcceleration();
281 }
282
283 /**
284 * Gets acceleration z-coordinate of bias.
285 *
286 * @param result instance where result will be stored.
287 */
288 public void getAccelerationBiasZAsAcceleration(final Acceleration result) {
289 accelerationFixer.getBiasZAsAcceleration(result);
290 }
291
292 /**
293 * Sets acceleration z-coordinate of bias.
294 *
295 * @param biasZ z-coordinate of bias.
296 */
297 public void setAccelerationBiasZ(final Acceleration biasZ) {
298 accelerationFixer.setBiasZ(biasZ);
299 }
300
301 /**
302 * Sets acceleration coordinates of bias.
303 *
304 * @param biasX x-coordinate of bias.
305 * @param biasY y-coordinate of bias.
306 * @param biasZ z-coordinate of bias.
307 */
308 public void setAccelerationBias(final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) {
309 accelerationFixer.setBias(biasX, biasY, biasZ);
310 }
311
312 /**
313 * Gets acceleration cross coupling errors matrix.
314 *
315 * @return acceleration cross coupling errors matrix.
316 */
317 public Matrix getAccelerationCrossCouplingErrors() {
318 return accelerationFixer.getCrossCouplingErrors();
319 }
320
321 /**
322 * Gets acceleration cross coupling errors matrix.
323 *
324 * @param result instance where result will be stored.
325 */
326 public void getAccelerationCrossCouplingErrors(final Matrix result) {
327 accelerationFixer.getCrossCouplingErrors(result);
328 }
329
330 /**
331 * Sets acceleration cross coupling errors matrix.
332 *
333 * @param crossCouplingErrors acceleration cross coupling errors matrix.
334 * Must be 3x3.
335 * @throws AlgebraException if provided matrix cannot be inverted.
336 * @throws IllegalArgumentException if provided matrix is not 3x3.
337 */
338 public void setAccelerationCrossCouplingErrors(final Matrix crossCouplingErrors) throws AlgebraException {
339 accelerationFixer.setCrossCouplingErrors(crossCouplingErrors);
340 }
341
342 /**
343 * Gets acceleration x scaling factor.
344 *
345 * @return x scaling factor.
346 */
347 public double getAccelerationSx() {
348 return accelerationFixer.getSx();
349 }
350
351 /**
352 * Sets acceleration x scaling factor.
353 *
354 * @param sx x scaling factor.
355 * @throws AlgebraException if provided value makes cross coupling matrix
356 * non-invertible.
357 */
358 public void setAccelerationSx(final double sx) throws AlgebraException {
359 accelerationFixer.setSx(sx);
360 }
361
362 /**
363 * Gets acceleration y scaling factor.
364 *
365 * @return y scaling factor.
366 */
367 public double getAccelerationSy() {
368 return accelerationFixer.getSy();
369 }
370
371 /**
372 * Sets acceleration y scaling factor.
373 *
374 * @param sy y scaling factor.
375 * @throws AlgebraException if provided value makes cross coupling matrix
376 * non-invertible.
377 */
378 public void setAccelerationSy(final double sy) throws AlgebraException {
379 accelerationFixer.setSy(sy);
380 }
381
382 /**
383 * Gets acceleration z scaling factor.
384 *
385 * @return z scaling factor.
386 */
387 public double getAccelerationSz() {
388 return accelerationFixer.getSz();
389 }
390
391 /**
392 * Sets acceleration z scaling factor.
393 *
394 * @param sz z scaling factor.
395 * @throws AlgebraException if provided value makes cross coupling matrix
396 * non-invertible.
397 */
398 public void setAccelerationSz(final double sz) throws AlgebraException {
399 accelerationFixer.setSz(sz);
400 }
401
402 /**
403 * Gets acceleration x-y cross coupling error.
404 *
405 * @return acceleration x-y cross coupling error.
406 */
407 public double getAccelerationMxy() {
408 return accelerationFixer.getMxy();
409 }
410
411 /**
412 * Sets acceleration x-y cross coupling error.
413 *
414 * @param mxy acceleration x-y cross coupling error.
415 * @throws AlgebraException if provided value makes cross coupling matrix
416 * non-invertible.
417 */
418 public void setAccelerationMxy(final double mxy) throws AlgebraException {
419 accelerationFixer.setMxy(mxy);
420 }
421
422 /**
423 * Gets acceleration x-z cross coupling error.
424 *
425 * @return acceleration x-z cross coupling error.
426 */
427 public double getAccelerationMxz() {
428 return accelerationFixer.getMxz();
429 }
430
431 /**
432 * Sets acceleration x-z cross coupling error.
433 *
434 * @param mxz acceleration x-z cross coupling error.
435 * @throws AlgebraException if provided value makes cross coupling matrix
436 * non-invertible.
437 */
438 public void setAccelerationMxz(final double mxz) throws AlgebraException {
439 accelerationFixer.setMxz(mxz);
440 }
441
442 /**
443 * Gets acceleration y-x cross coupling error.
444 *
445 * @return acceleration y-x cross coupling error.
446 */
447 public double getAccelerationMyx() {
448 return accelerationFixer.getMyx();
449 }
450
451 /**
452 * Sets acceleration y-x cross coupling error.
453 *
454 * @param myx acceleration y-x cross coupling error.
455 * @throws AlgebraException if provided value makes cross coupling matrix
456 * non-invertible.
457 */
458 public void setAccelerationMyx(final double myx) throws AlgebraException {
459 accelerationFixer.setMyx(myx);
460 }
461
462 /**
463 * Gets acceleration y-z cross coupling error.
464 *
465 * @return y-z cross coupling error.
466 */
467 public double getAccelerationMyz() {
468 return accelerationFixer.getMyz();
469 }
470
471 /**
472 * Sets acceleration y-z cross coupling error.
473 *
474 * @param myz y-z cross coupling error.
475 * @throws AlgebraException if provided value makes cross coupling matrix
476 * non-invertible.
477 */
478 public void setAccelerationMyz(final double myz) throws AlgebraException {
479 accelerationFixer.setMyz(myz);
480 }
481
482 /**
483 * Gets acceleration z-x cross coupling error.
484 *
485 * @return acceleration z-x cross coupling error.
486 */
487 public double getAccelerationMzx() {
488 return accelerationFixer.getMzx();
489 }
490
491 /**
492 * Sets acceleration z-x cross coupling error.
493 *
494 * @param mzx acceleration z-x cross coupling error.
495 * @throws AlgebraException if provided value makes cross coupling matrix
496 * non-invertible.
497 */
498 public void setAccelerationMzx(final double mzx) throws AlgebraException {
499 accelerationFixer.setMzx(mzx);
500 }
501
502 /**
503 * Gets acceleration z-y cross coupling error.
504 *
505 * @return acceleration z-y cross coupling error.
506 */
507 public double getAccelerationMzy() {
508 return accelerationFixer.getMzy();
509 }
510
511 /**
512 * Sets acceleration z-y cross coupling error.
513 *
514 * @param mzy acceleration z-y cross coupling error.
515 * @throws AlgebraException if provided value makes cross coupling matrix
516 * non-invertible.
517 */
518 public void setAccelerationMzy(final double mzy) throws AlgebraException {
519 accelerationFixer.setMzy(mzy);
520 }
521
522 /**
523 * Sets acceleration scaling factors.
524 *
525 * @param sx x scaling factor.
526 * @param sy y scaling factor.
527 * @param sz z scaling factor.
528 * @throws AlgebraException if provided values make cross coupling matrix
529 * non-invertible.
530 */
531 public void setAccelerationScalingFactors(final double sx, final double sy, final double sz)
532 throws AlgebraException {
533 accelerationFixer.setScalingFactors(sx, sy, sz);
534 }
535
536 /**
537 * Sets acceleration cross coupling errors.
538 *
539 * @param mxy x-y cross coupling error.
540 * @param mxz x-z cross coupling error.
541 * @param myx y-x cross coupling error.
542 * @param myz y-z cross coupling error.
543 * @param mzx z-x cross coupling error.
544 * @param mzy z-y cross coupling error.
545 * @throws AlgebraException if provided values make cross coupling matrix
546 * non-invertible.
547 */
548 public void setAccelerationCrossCouplingErrors(
549 final double mxy, final double mxz, final double myx,
550 final double myz, final double mzx, final double mzy) throws AlgebraException {
551 accelerationFixer.setCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
552 }
553
554 /**
555 * Sets acceleration scaling factors and cross coupling errors.
556 *
557 * @param sx x scaling factor.
558 * @param sy y scaling factor.
559 * @param sz z scaling factor.
560 * @param mxy x-y cross coupling error.
561 * @param mxz x-z cross coupling error.
562 * @param myx y-x cross coupling error.
563 * @param myz y-z cross coupling error.
564 * @param mzx z-x cross coupling error.
565 * @param mzy z-y cross coupling error.
566 * @throws AlgebraException if provided values make cross coupling matrix
567 * non-invertible.
568 */
569 public void setAccelerationScalingFactorsAndCrossCouplingErrors(
570 final double sx, final double sy, final double sz,
571 final double mxy, final double mxz, final double myx,
572 final double myz, final double mzx, final double mzy) throws AlgebraException {
573 accelerationFixer.setScalingFactorsAndCrossCouplingErrors(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
574 }
575
576 /**
577 * Gets angular speed bias values expressed in radians per second (rad/s).
578 *
579 * @return angular speed bias values expressed in radians per second.
580 */
581 public Matrix getAngularSpeedBias() {
582 return angularRateFixer.getBias();
583 }
584
585 /**
586 * Gets angular speed bias values expressed in radians per second (rad/s).
587 *
588 * @param result instance where result will be stored.
589 */
590 public void getAngularSpeedBias(final Matrix result) {
591 angularRateFixer.getBias(result);
592 }
593
594 /**
595 * Sets angular speed bias values expressed in radians per second (rad/s).
596 *
597 * @param bias bias values expressed in radians per second. Must be 3x1.
598 * @throws IllegalArgumentException if provided matrix is not 3x1.
599 */
600 public void setAngularSpeedBias(final Matrix bias) {
601 angularRateFixer.setBias(bias);
602 }
603
604 /**
605 * Gets angular speed bias values expressed in radians per second (rad/s).
606 *
607 * @return bias values expressed in radians per second.
608 */
609 public double[] getAngularSpeedBiasArray() {
610 return angularRateFixer.getBiasArray();
611 }
612
613 /**
614 * Gets angular speed bias values expressed in radians per second (rad/s).
615 *
616 * @param result instance where result data will be stored.
617 * @throws IllegalArgumentException if provided array does not have length 3.
618 */
619 public void getAngularSpeedBiasArray(final double[] result) {
620 angularRateFixer.getBiasArray(result);
621 }
622
623 /**
624 * Sets angular speed bias values expressed in radians per second (rad/s).
625 *
626 * @param bias bias values expressed in radians per second (rad/s). Must
627 * have length 3.
628 * @throws IllegalArgumentException if provided array does not have length 3.
629 */
630 public void setAngularSpeedBias(final double[] bias) {
631 angularRateFixer.setBias(bias);
632 }
633
634 /**
635 * Gets angular speed bias.
636 *
637 * @return angular speed bias.
638 */
639 public AngularSpeedTriad getAngularSpeedBiasAsTriad() {
640 return angularRateFixer.getBiasAsTriad();
641 }
642
643 /**
644 * Gets angular speed bias.
645 *
646 * @param result instance where result will be stored.
647 */
648 public void getAngularSpeedBiasAsTriad(final AngularSpeedTriad result) {
649 angularRateFixer.getBiasAsTriad(result);
650 }
651
652 /**
653 * Sets angular speed bias.
654 *
655 * @param bias angular speed bias to be set.
656 */
657 public void setAngularSpeedBias(final AngularSpeedTriad bias) {
658 angularRateFixer.setBias(bias);
659 }
660
661 /**
662 * Gets angular speed x-coordinate of bias expressed in radians per second
663 * (rad/s).
664 *
665 * @return x-coordinate of bias expressed in radians per second (rad/s).
666 */
667 public double getAngularSpeedBiasX() {
668 return angularRateFixer.getBiasX();
669 }
670
671 /**
672 * Sets angular speed x-coordinate of bias expressed in radians per second
673 * (rad/s).
674 *
675 * @param biasX x-coordinate of bias expressed in radians per second (rad/s).
676 */
677 public void setAngularSpeedBiasX(final double biasX) {
678 angularRateFixer.setBiasX(biasX);
679 }
680
681 /**
682 * Gets angular speed y-coordinate of bias expressed in radians per second
683 * (rad/s).
684 *
685 * @return y-coordinate of bias expressed in radians per second (rad/s).
686 */
687 public double getAngularSpeedBiasY() {
688 return angularRateFixer.getBiasY();
689 }
690
691 /**
692 * Sets angular speed y-coordinate of bias expressed in radians per second
693 * (rad/s).
694 *
695 * @param biasY y-coordinate of bias expressed in radians per second (rad/s).
696 */
697 public void setAngularSpeedBiasY(final double biasY) {
698 angularRateFixer.setBiasY(biasY);
699 }
700
701 /**
702 * Gets angular speed z-coordinate of bias expressed in radians per second
703 * (rad/s).
704 *
705 * @return z-coordinate of bias expressed in radians per second (rad/s).
706 */
707 public double getAngularSpeedBiasZ() {
708 return angularRateFixer.getBiasZ();
709 }
710
711 /**
712 * Sets angular speed z-coordinate of bias expressed in radians per second
713 * (rad/s).
714 *
715 * @param biasZ z-coordinate of bias expressed in radians per second (rad/s).
716 */
717 public void setAngularSpeedBiasZ(final double biasZ) {
718 angularRateFixer.setBiasZ(biasZ);
719 }
720
721 /**
722 * Sets angular speed coordinates of bias expressed in radians per second
723 * (rad/s).
724 *
725 * @param biasX x-coordinate of bias.
726 * @param biasY y-coordinate of bias.
727 * @param biasZ z-coordinate of bias.
728 */
729 public void setAngularSpeedBias(final double biasX, final double biasY, final double biasZ) {
730 angularRateFixer.setBias(biasX, biasY, biasZ);
731 }
732
733 /**
734 * Gets angular speed x-coordinate of bias.
735 *
736 * @return x-coordinate of bias.
737 */
738 public AngularSpeed getAngularSpeedBiasXAsAngularSpeed() {
739 return angularRateFixer.getBiasXAsAngularSpeed();
740 }
741
742 /**
743 * Gets angular speed x-coordinate of bias.
744 *
745 * @param result instance where result will be stored.
746 */
747 public void getAngularSpeedBiasXAsAngularSpeed(final AngularSpeed result) {
748 angularRateFixer.getBiasXAsAngularSpeed(result);
749 }
750
751 /**
752 * Sets angular speed x-coordinate of bias.
753 *
754 * @param biasX x-coordinate of bias.
755 */
756 public void setAngularSpeedBiasX(final AngularSpeed biasX) {
757 angularRateFixer.setBiasX(biasX);
758 }
759
760 /**
761 * Gets angular speed y-coordinate of bias.
762 *
763 * @return y-coordinate of bias.
764 */
765 public AngularSpeed getAngularSpeedBiasYAsAngularSpeed() {
766 return angularRateFixer.getBiasYAsAngularSpeed();
767 }
768
769 /**
770 * Gets angular speed y-coordinate of bias.
771 *
772 * @param result instance where result will be stored.
773 */
774 public void getAngularSpeedBiasYAsAngularSpeed(final AngularSpeed result) {
775 angularRateFixer.getBiasYAsAngularSpeed(result);
776 }
777
778 /**
779 * Sets angular speed y-coordinate of bias.
780 *
781 * @param biasY y-coordinate of bias.
782 */
783 public void setAngularSpeedBiasY(final AngularSpeed biasY) {
784 angularRateFixer.setBiasY(biasY);
785 }
786
787 /**
788 * Gets angular speed z-coordinate of bias.
789 *
790 * @return z-coordinate of bias.
791 */
792 public AngularSpeed getAngularSpeedBiasZAsAngularSpeed() {
793 return angularRateFixer.getBiasZAsAngularSpeed();
794 }
795
796 /**
797 * Gets angular speed z-coordinate of bias.
798 *
799 * @param result instance where result will be stored.
800 */
801 public void getAngularSpeedBiasZAsAngularSpeed(final AngularSpeed result) {
802 angularRateFixer.getBiasZAsAngularSpeed(result);
803 }
804
805 /**
806 * Sets angular speed z-coordinate of bias.
807 *
808 * @param biasZ z-coordinate of bias.
809 */
810 public void setAngularSpeedBiasZ(final AngularSpeed biasZ) {
811 angularRateFixer.setBiasZ(biasZ);
812 }
813
814 /**
815 * Sets angular speed coordinates of bias.
816 *
817 * @param biasX x-coordinate of bias.
818 * @param biasY y-coordinate of bias.
819 * @param biasZ z-coordinate of bias.
820 */
821 public void setAngularSpeedBias(final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ) {
822 angularRateFixer.setBias(biasX, biasY, biasZ);
823 }
824
825 /**
826 * Gets angular speed cross coupling errors matrix.
827 *
828 * @return cross coupling errors matrix.
829 */
830 public Matrix getAngularSpeedCrossCouplingErrors() {
831 return angularRateFixer.getCrossCouplingErrors();
832 }
833
834 /**
835 * Gets angular speed cross coupling errors matrix.
836 *
837 * @param result instance where result will be stored.
838 */
839 public void getAngularSpeedCrossCouplingErrors(final Matrix result) {
840 angularRateFixer.getCrossCouplingErrors(result);
841 }
842
843 /**
844 * Sets angular speed cross coupling errors matrix.
845 *
846 * @param crossCouplingErrors cross coupling errors matrix. Must be 3x3.
847 * @throws AlgebraException if provided matrix cannot be inverted.
848 * @throws IllegalArgumentException if provided matrix is not 3x3.
849 */
850 public void setAngularSpeedCrossCouplingErrors(final Matrix crossCouplingErrors) throws AlgebraException {
851 angularRateFixer.setCrossCouplingErrors(crossCouplingErrors);
852 }
853
854 /**
855 * Gets angular speed x scaling factor.
856 *
857 * @return x scaling factor.
858 */
859 public double getAngularSpeedSx() {
860 return angularRateFixer.getSx();
861 }
862
863 /**
864 * sets angular speed x scaling factor.
865 *
866 * @param sx x scaling factor.
867 * @throws AlgebraException if provided value makes cross coupling matrix
868 * non-invertible.
869 */
870 public void setAngularSpeedSx(final double sx) throws AlgebraException {
871 angularRateFixer.setSx(sx);
872 }
873
874 /**
875 * Gets angular speed y scaling factor.
876 *
877 * @return y scaling factor.
878 */
879 public double getAngularSpeedSy() {
880 return angularRateFixer.getSy();
881 }
882
883 /**
884 * Sets angular speed y scaling factor.
885 *
886 * @param sy y scaling factor.
887 * @throws AlgebraException if provided value makes cross coupling matrix
888 * non-invertible.
889 */
890 public void setAngularSpeedSy(final double sy) throws AlgebraException {
891 angularRateFixer.setSy(sy);
892 }
893
894 /**
895 * Gets angular speed z scaling factor.
896 *
897 * @return z scaling factor.
898 */
899 public double getAngularSpeedSz() {
900 return angularRateFixer.getSz();
901 }
902
903 /**
904 * Sets angular speed z scaling factor.
905 *
906 * @param sz z scaling factor.
907 * @throws AlgebraException if provided value makes cross coupling matrix
908 * non-invertible.
909 */
910 public void setAngularSpeedSz(final double sz) throws AlgebraException {
911 angularRateFixer.setSz(sz);
912 }
913
914 /**
915 * Gets angular speed x-y cross coupling error.
916 *
917 * @return x-y cross coupling error.
918 */
919 public double getAngularSpeedMxy() {
920 return angularRateFixer.getMxy();
921 }
922
923 /**
924 * Sets angular speed x-y cross coupling error.
925 *
926 * @param mxy x-y cross coupling error.
927 * @throws AlgebraException if provided value makes cross coupling matrix
928 * non-invertible.
929 */
930 public void setAngularSpeedMxy(final double mxy) throws AlgebraException {
931 angularRateFixer.setMxy(mxy);
932 }
933
934 /**
935 * Gets angular speed x-z cross coupling error.
936 *
937 * @return x-z cross coupling error.
938 */
939 public double getAngularSpeedMxz() {
940 return angularRateFixer.getMxz();
941 }
942
943 /**
944 * Sets angular speed x-z cross coupling error.
945 *
946 * @param mxz x-z cross coupling error.
947 * @throws AlgebraException if provided value makes cross coupling matrix
948 * non-invertible.
949 */
950 public void setAngularSpeedMxz(final double mxz) throws AlgebraException {
951 angularRateFixer.setMxz(mxz);
952 }
953
954 /**
955 * Gets angular speed y-x cross coupling error.
956 *
957 * @return y-x cross coupling error.
958 */
959 public double getAngularSpeedMyx() {
960 return angularRateFixer.getMyx();
961 }
962
963 /**
964 * Sets angular speed y-x cross coupling error.
965 *
966 * @param myx y-x cross coupling error.
967 * @throws AlgebraException if provided value makes cross coupling matrix
968 * non-invertible.
969 */
970 public void setAngularSpeedMyx(final double myx) throws AlgebraException {
971 angularRateFixer.setMyx(myx);
972 }
973
974 /**
975 * Gets angular speed y-z cross coupling error.
976 *
977 * @return y-z cross coupling error.
978 */
979 public double getAngularSpeedMyz() {
980 return angularRateFixer.getMyz();
981 }
982
983 /**
984 * Sets angular speed y-z cross coupling error.
985 *
986 * @param myz y-z cross coupling error.
987 * @throws AlgebraException if provided value makes cross coupling matrix
988 * non-invertible.
989 */
990 public void setAngularSpeedMyz(final double myz) throws AlgebraException {
991 angularRateFixer.setMyz(myz);
992 }
993
994 /**
995 * Gets angular speed z-x cross coupling error.
996 *
997 * @return z-x cross coupling error.
998 */
999 public double getAngularSpeedMzx() {
1000 return angularRateFixer.getMzx();
1001 }
1002
1003 /**
1004 * Sets angular speed z-x cross coupling error.
1005 *
1006 * @param mzx z-x cross coupling error.
1007 * @throws AlgebraException if provided value makes cross coupling matrix
1008 * non-invertible.
1009 */
1010 public void setAngularSpeedMzx(final double mzx) throws AlgebraException {
1011 angularRateFixer.setMzx(mzx);
1012 }
1013
1014 /**
1015 * Gets angular speed z-y cross coupling error.
1016 *
1017 * @return z-y cross coupling error.
1018 */
1019 public double getAngularSpeedMzy() {
1020 return angularRateFixer.getMzy();
1021 }
1022
1023 /**
1024 * Sets angular speed z-y cross coupling error.
1025 *
1026 * @param mzy z-y cross coupling error.
1027 * @throws AlgebraException if provided value makes cross coupling matrix
1028 * non-invertible.
1029 */
1030 public void setAngularSpeedMzy(final double mzy) throws AlgebraException {
1031 angularRateFixer.setMzy(mzy);
1032 }
1033
1034 /**
1035 * Sets angular speed scaling factors.
1036 *
1037 * @param sx x scaling factor.
1038 * @param sy y scaling factor.
1039 * @param sz z scaling factor.
1040 * @throws AlgebraException if provided values make cross coupling matrix
1041 * non-invertible.
1042 */
1043 public void setAngularSpeedScalingFactors(final double sx, final double sy, final double sz)
1044 throws AlgebraException {
1045 angularRateFixer.setScalingFactors(sx, sy, sz);
1046 }
1047
1048 /**
1049 * Sets angular speed cross coupling errors.
1050 *
1051 * @param mxy x-y cross coupling error.
1052 * @param mxz x-z cross coupling error.
1053 * @param myx y-x cross coupling error.
1054 * @param myz y-z cross coupling error.
1055 * @param mzx z-x cross coupling error.
1056 * @param mzy z-y cross coupling error.
1057 * @throws AlgebraException if provided values make cross coupling matrix
1058 * non-invertible.
1059 */
1060 public void setAngularSpeedCrossCouplingErrors(
1061 final double mxy, final double mxz, final double myx,
1062 final double myz, final double mzx, final double mzy) throws AlgebraException {
1063 angularRateFixer.setCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
1064 }
1065
1066 /**
1067 * Sets angular speed scaling factors and cross coupling errors.
1068 *
1069 * @param sx x scaling factor.
1070 * @param sy y scaling factor.
1071 * @param sz z scaling factor.
1072 * @param mxy x-y cross coupling error.
1073 * @param mxz x-z cross coupling error.
1074 * @param myx y-x cross coupling error.
1075 * @param myz y-z cross coupling error.
1076 * @param mzx z-x cross coupling error.
1077 * @param mzy z-y cross coupling error.
1078 * @throws AlgebraException if provided values make cross coupling matrix
1079 * non-invertible.
1080 */
1081 public void setAngularSpeedScalingFactorsAndCrossCouplingErrors(
1082 final double sx, final double sy, final double sz,
1083 final double mxy, final double mxz, final double myx,
1084 final double myz, final double mzx, final double mzy) throws AlgebraException {
1085 angularRateFixer.setScalingFactorsAndCrossCouplingErrors(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
1086 }
1087
1088 /**
1089 * Gets angular speed g-dependant cross biases matrix.
1090 *
1091 * @return g-dependant cross biases matrix.
1092 */
1093 public Matrix getAngularSpeedGDependantCrossBias() {
1094 return angularRateFixer.getGDependantCrossBias();
1095 }
1096
1097 /**
1098 * Gets angular speed g-dependant cross biases matrix.
1099 *
1100 * @param result instance where result will be stored.
1101 */
1102 public void getAngularSpeedGDependantCrossBias(final Matrix result) {
1103 angularRateFixer.getGDependantCrossBias(result);
1104 }
1105
1106 /**
1107 * Sets angular speed g-dependant cross biases matrix.
1108 *
1109 * @param gDependantCrossBias g-dependant cross biases matrix.
1110 * @throws IllegalArgumentException if provided matrix is not 3x3.
1111 */
1112 public void setAngularSpeedGDependantCrossBias(final Matrix gDependantCrossBias) {
1113 angularRateFixer.setGDependantCrossBias(gDependantCrossBias);
1114 }
1115
1116 /**
1117 * Fixes provided measured body kinematics by undoing the errors introduced
1118 * by the accelerometer and gyroscope models to restore the true body
1119 * kinematics values.
1120 * This method uses last provided accelerometer and gyroscope bias and
1121 * cross coupling errors.
1122 *
1123 * @param measuredKinematics measured body kinematics to be fixed.
1124 * @param result instance where fixed body kinematics will be
1125 * stored.
1126 * @throws AlgebraException if there are numerical instabilities.
1127 */
1128 public void fix(final BodyKinematics measuredKinematics, final BodyKinematics result) throws AlgebraException {
1129 measuredKinematics.getSpecificForceTriad(measuredAcceleration);
1130 measuredKinematics.getAngularRateTriad(measuredAngularSpeed);
1131
1132 fix(measuredAcceleration, measuredAngularSpeed, fixedAcceleration, fixedAngularSpeed);
1133
1134 result.setSpecificForceTriad(fixedAcceleration);
1135 result.setAngularRateTriad(fixedAngularSpeed);
1136 }
1137
1138 /**
1139 * Fixes provided measured body kinematics by undoing the errors introduced
1140 * by the accelerometer and gyroscope models to restore the true
1141 * body kinematics values.
1142 * This method uses last provided accelerometer and gyroscope bias and
1143 * cross coupling errors.
1144 *
1145 * @param measuredSpecificForce measured specific force to be fixed.
1146 * @param measuredAngularSpeed measured angular speed to be fixed.
1147 * @param fixedSpecificForce instance where fixed specific force will be
1148 * stored.
1149 * @param fixedAngularSpeed instance where fixed angular speed will be
1150 * stored.
1151 * @throws AlgebraException if there are numerical instabilities.
1152 */
1153 public void fix(
1154 final AccelerationTriad measuredSpecificForce, final AngularSpeedTriad measuredAngularSpeed,
1155 final AccelerationTriad fixedSpecificForce, final AngularSpeedTriad fixedAngularSpeed)
1156 throws AlgebraException {
1157
1158 accelerationFixer.fix(measuredSpecificForce, fixedSpecificForce);
1159 angularRateFixer.fix(measuredAngularSpeed, fixedSpecificForce, fixedAngularSpeed);
1160 }
1161
1162 /**
1163 * Fixes provided measured body kinematics by undoing the errors introduced
1164 * by the accelerometer and gyroscope models to restore the true
1165 * body kinematics values.
1166 * This method uses last provided accelerometer and gyroscope bias and
1167 * cross coupling errors.
1168 *
1169 * @param measuredKinematics measured body kinematics to be fixed.
1170 * @param fixedSpecificForce instance where fixed specific force will be
1171 * stored.
1172 * @param fixedAngularSpeed instance where fixed angular speed will be
1173 * stored.
1174 * @throws AlgebraException if there are numerical instabilities.
1175 */
1176 public void fix(final BodyKinematics measuredKinematics, final AccelerationTriad fixedSpecificForce,
1177 final AngularSpeedTriad fixedAngularSpeed) throws AlgebraException {
1178 measuredKinematics.getSpecificForceTriad(measuredAcceleration);
1179 measuredKinematics.getAngularRateTriad(measuredAngularSpeed);
1180
1181 fix(measuredAcceleration, measuredAngularSpeed, fixedSpecificForce, fixedAngularSpeed);
1182 }
1183
1184 /**
1185 * Fixes provided measured body kinematics by undoing the errors introduced
1186 * by the accelerometer and gyroscope models to restore the true
1187 * body kinematics values.
1188 * This method uses last provided accelerometer and gyroscope bias and
1189 * cross coupling errors.
1190 *
1191 * @param measuredSpecificForce measured specific force to be fixed.
1192 * @param measuredAngularSpeed measured angular speed to be fixed.
1193 * @param result instance where fixed body kinematics will be
1194 * stored.
1195 * @throws AlgebraException if there are numerical instabilities.
1196 */
1197 public void fix(final AccelerationTriad measuredSpecificForce, final AngularSpeedTriad measuredAngularSpeed,
1198 final BodyKinematics result) throws AlgebraException {
1199 fix(measuredSpecificForce, measuredAngularSpeed, fixedAcceleration, fixedAngularSpeed);
1200
1201 result.setSpecificForceTriad(fixedAcceleration);
1202 result.setAngularRateTriad(fixedAngularSpeed);
1203 }
1204
1205 /**
1206 * Fixes provided measured body kinematics by undoing the errors introduced
1207 * by the accelerometer and gyroscope models to restore the true body
1208 * kinematics values.
1209 * This method uses last provided accelerometer and gyroscope bias and
1210 * cross coupling errors.
1211 *
1212 * @param measuredKinematics measured body kinematics to be fixed.
1213 * @return restored true body kinematics.
1214 * @throws AlgebraException if there are numerical instabilities.
1215 */
1216 public BodyKinematics fixAndReturnNew(final BodyKinematics measuredKinematics) throws AlgebraException {
1217 final var result = new BodyKinematics();
1218 fix(measuredKinematics, result);
1219 return result;
1220 }
1221
1222 /**
1223 * Fixes provided measured body kinematics by undoing the errors introduced
1224 * by the accelerometer and gyroscope models to restore the true body
1225 * kinematics values.
1226 * This method uses last provided accelerometer and gyroscope bias and
1227 * cross coupling errors.
1228 *
1229 * @param measuredSpecificForce measured specific force to be fixed.
1230 * @param measuredAngularSpeed measured angular speed to be fixed.
1231 * @return restored true body kinematics.
1232 * @throws AlgebraException if there are numerical instabilities.
1233 */
1234 public BodyKinematics fixAndReturnNew(
1235 final AccelerationTriad measuredSpecificForce, final AngularSpeedTriad measuredAngularSpeed)
1236 throws AlgebraException {
1237 final var result = new BodyKinematics();
1238 fix(measuredSpecificForce, measuredAngularSpeed, result);
1239 return result;
1240 }
1241 }