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