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