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.magnetometer;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.Utils;
21 import com.irurueta.algebra.WrongSizeException;
22 import com.irurueta.navigation.LockedException;
23 import com.irurueta.navigation.NotReadyException;
24 import com.irurueta.navigation.frames.CoordinateTransformation;
25 import com.irurueta.navigation.frames.FrameType;
26 import com.irurueta.navigation.frames.NEDFrame;
27 import com.irurueta.navigation.frames.converters.ECEFtoNEDFrameConverter;
28 import com.irurueta.navigation.inertial.BodyMagneticFluxDensity;
29 import com.irurueta.navigation.inertial.calibration.CalibrationException;
30 import com.irurueta.navigation.inertial.calibration.FrameBodyMagneticFluxDensity;
31 import com.irurueta.navigation.inertial.calibration.MagneticFluxDensityTriad;
32 import com.irurueta.navigation.inertial.estimators.BodyMagneticFluxDensityEstimator;
33 import com.irurueta.navigation.inertial.wmm.NEDMagneticFluxDensity;
34 import com.irurueta.navigation.inertial.wmm.WMMEarthMagneticFluxDensityEstimator;
35 import com.irurueta.navigation.inertial.wmm.WorldMagneticModel;
36 import com.irurueta.units.MagneticFluxDensity;
37 import com.irurueta.units.MagneticFluxDensityUnit;
38
39 import java.io.IOException;
40 import java.util.Collection;
41
42 /**
43 * Estimates magnetometer hard-iron biases, soft-iron cross couplings and
44 * scaling factors.
45 * <p>
46 * This calibrator uses a linear approach to find a minimum least squared error
47 * solution.
48 * <p>
49 * To use this calibrator at least 4 measurements at different known frames must
50 * be provided. In other words, magnetometer samples must be obtained at 4
51 * different positions or orientations.
52 * Notice that frame velocities are ignored by this calibrator.
53 * <p>
54 * Measured magnetic flux density is assumed to follow the model shown below:
55 * <pre>
56 * mBmeas = bm + (I + Mm) * mBtrue + w
57 * </pre>
58 * Where:
59 * - mBmeas is the measured magnetic flux density. This is a 3x1 vector.
60 * - bm is magnetometer hard-iron bias. Ideally, on a perfect magnetometer,
61 * this should be a 3x1 zero vector.
62 * - I is the 3x3 identity matrix.
63 * - Mm is the 3x3 soft-iron matrix containing cross-couplings and scaling
64 * factors. Ideally, on a perfect magnetometer, this should be a 3x3 zero
65 * matrix.
66 * - mBtrue is ground-truth magnetic flux density. This is a 3x1 vector.
67 * - w is measurement noise. This is a 3x1 vector.
68 */
69 @SuppressWarnings("DuplicatedCode")
70 public class KnownFrameMagnetometerLinearLeastSquaresCalibrator implements
71 KnownFrameMagnetometerCalibrator<FrameBodyMagneticFluxDensity,
72 KnownFrameMagnetometerLinearLeastSquaresCalibratorListener>,
73 UnorderedFrameBodyMagneticFluxDensityMagnetometerCalibrator {
74
75 /**
76 * Indicates whether by default a common z-axis is assumed for the accelerometer,
77 * gyroscope and magnetometer.
78 */
79 public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;
80
81 /**
82 * Required minimum number of measurements.
83 */
84 public static final int MINIMUM_MEASUREMENTS = 4;
85
86 /**
87 * Number of equations generated for each measurement.
88 */
89 private static final int EQUATIONS_PER_MEASUREMENT = 3;
90
91 /**
92 * Number of unknowns when common z-axis is assumed for the accelerometer,
93 * gyroscope and magnetometer.
94 */
95 private static final int COMMON_Z_AXIS_UNKNOWNS = 9;
96
97 /**
98 * Number of unknowns for the general case.
99 */
100 private static final int GENERAL_UNKNOWNS = 12;
101
102 /**
103 * Contains a collection of body magnetic flux density measurements taken
104 * at different frames (positions and orientations).
105 * If a single device magnetometer needs to be calibrated, typically all
106 * measurements are taken at the same position, with zero velocity and
107 * multiple orientations.
108 * However, if we just want to calibrate a given magnetometer model (e.g.
109 * obtain an average and less precise calibration for the magnetometer of
110 * a given phone model), we could take measurements collected throughout
111 * the planet at multiple positions while the phone remains static (e.g.
112 * while charging), hence each measurement position will change, velocity
113 * will remain zero and orientation will be typically constant at
114 * horizontal orientation while the phone remains on a
115 * flat surface.
116 */
117 private Collection<FrameBodyMagneticFluxDensity> measurements;
118
119 /**
120 * This flag indicates whether z-axis is assumed to be common for accelerometer,
121 * gyroscope and magnetometer.
122 * When enabled, this eliminates 3 variables from Mm matrix.
123 */
124 private boolean commonAxisUsed = DEFAULT_USE_COMMON_Z_AXIS;
125
126 /**
127 * Listener to handle events raised by this calibrator.
128 */
129 private KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener;
130
131 /**
132 * Estimated magnetometer hard-iron biases for each magnetometer axis
133 * expressed in Teslas (T).
134 */
135 private double[] estimatedHardIron;
136
137 /**
138 * Estimated magnetometer soft-iron matrix containing scale factors
139 * and cross coupling errors.
140 * This is the product of matrix Tm containing cross coupling errors and Km
141 * containing scaling factors.
142 * So tat:
143 * <pre>
144 * Mm = [sx mxy mxz] = Tm*Km
145 * [myx sy myz]
146 * [mzx mzy sz ]
147 * </pre>
148 * Where:
149 * <pre>
150 * Km = [sx 0 0 ]
151 * [0 sy 0 ]
152 * [0 0 sz]
153 * </pre>
154 * and
155 * <pre>
156 * Tm = [1 -alphaXy alphaXz ]
157 * [alphaYx 1 -alphaYz]
158 * [-alphaZx alphaZy 1 ]
159 * </pre>
160 * Hence:
161 * <pre>
162 * Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
163 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
164 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
165 * </pre>
166 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
167 * are considered to be zero if the accelerometer z-axis is assumed to be the same
168 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
169 * becomes upper diagonal:
170 * <pre>
171 * Mm = [sx mxy mxz]
172 * [0 sy myz]
173 * [0 0 sz ]
174 * </pre>
175 * Values of this matrix are unit-less.
176 */
177 private Matrix estimatedMm;
178
179 /**
180 * Indicates whether calibrator is running.
181 */
182 private boolean running;
183
184 /**
185 * Contains Earth's magnetic model.
186 */
187 private WorldMagneticModel magneticModel;
188
189 /**
190 * Constructor.
191 */
192 public KnownFrameMagnetometerLinearLeastSquaresCalibrator() {
193 }
194
195 /**
196 * Constructor.
197 *
198 * @param listener listener to handle events raised by this calibrator.
199 */
200 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(
201 final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener) {
202 this.listener = listener;
203 }
204
205 /**
206 * Constructor.
207 *
208 * @param measurements collection of body magnetic flux density measurements
209 * taken at different frames (positions and orientations).
210 */
211 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(
212 final Collection<? extends FrameBodyMagneticFluxDensity> measurements) {
213 //noinspection unchecked
214 this.measurements = (Collection<FrameBodyMagneticFluxDensity>) measurements;
215 }
216
217 /**
218 * Constructor.
219 *
220 * @param measurements collection of body magnetic flux density measurements
221 * taken at different frames (positions and orientations).
222 * @param listener listener to handle events raised by this calibrator.
223 */
224 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(
225 final Collection<? extends FrameBodyMagneticFluxDensity> measurements,
226 final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener) {
227 this(measurements);
228 this.listener = listener;
229 }
230
231 /**
232 * Constructor.
233 *
234 * @param commonAxisUsed indicates whether z-axis is assumed to be common
235 * for the accelerometer, gyroscope and magnetometer.
236 */
237 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(final boolean commonAxisUsed) {
238 this.commonAxisUsed = commonAxisUsed;
239 }
240
241 /**
242 * Constructor.
243 *
244 * @param commonAxisUsed indicates whether z-axis is assumed to be common
245 * for the accelerometer, gyroscope and magnetometer.
246 * @param listener listener to handle events raised by this calibrator.
247 */
248 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(
249 final boolean commonAxisUsed,
250 final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener) {
251 this(commonAxisUsed);
252 this.listener = listener;
253 }
254
255 /**
256 * Constructor.
257 *
258 * @param measurements collection of body magnetic flux density measurements
259 * taken at different frames (positions and orientations).
260 * @param commonAxisUsed indicates whether z-axis is assumed to be common
261 * for the accelerometer, gyroscope and magnetometer.
262 */
263 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(
264 final Collection<? extends FrameBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed) {
265 this(measurements);
266 this.commonAxisUsed = commonAxisUsed;
267 }
268
269 /**
270 * Constructor.
271 *
272 * @param measurements collection of body magnetic flux density measurements
273 * taken at different frames (positions and orientations).
274 * @param commonAxisUsed indicates whether z-axis is assumed to be common
275 * for the accelerometer, gyroscope and magnetometer.
276 * @param listener listener to handle events raised by this calibrator.
277 */
278 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(
279 final Collection<? extends FrameBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
280 final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener) {
281 this(measurements, commonAxisUsed);
282 this.listener = listener;
283 }
284
285 /**
286 * Constructor.
287 *
288 * @param magneticModel Earth's magnetic model. If null, a default model
289 * will be used instead.
290 */
291 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(final WorldMagneticModel magneticModel) {
292 this.magneticModel = magneticModel;
293 }
294
295 /**
296 * Constructor.
297 *
298 * @param magneticModel Earth's magnetic model. If null, a default model
299 * will be used instead.
300 * @param listener listener to handle events raised by this calibrator.
301 */
302 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(
303 final WorldMagneticModel magneticModel,
304 final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener) {
305 this(listener);
306 this.magneticModel = magneticModel;
307 }
308
309 /**
310 * Constructor.
311 *
312 * @param measurements collection of body magnetic flux density measurements
313 * taken at different frames (positions and orientations).
314 * @param magneticModel Earth's magnetic model. If null, a default model
315 * will be used instead.
316 */
317 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(
318 final Collection<? extends FrameBodyMagneticFluxDensity> measurements,
319 final WorldMagneticModel magneticModel) {
320 this(measurements);
321 this.magneticModel = magneticModel;
322 }
323
324 /**
325 * Constructor.
326 *
327 * @param measurements collection of body magnetic flux density measurements
328 * taken at different frames (positions and orientations).
329 * @param magneticModel Earth's magnetic model. If null, a default model
330 * will be used instead.
331 * @param listener listener to handle events raised by this calibrator.
332 */
333 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(
334 final Collection<? extends FrameBodyMagneticFluxDensity> measurements,
335 final WorldMagneticModel magneticModel,
336 final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener) {
337 this(measurements, listener);
338 this.magneticModel = magneticModel;
339 }
340
341 /**
342 * Constructor.
343 *
344 * @param commonAxisUsed indicates whether z-axis is assumed to be common
345 * for the accelerometer, gyroscope and magnetometer.
346 * @param magneticModel Earth's magnetic model. If null, a default model
347 * will be used instead.
348 */
349 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(
350 final boolean commonAxisUsed, final WorldMagneticModel magneticModel) {
351 this(commonAxisUsed);
352 this.magneticModel = magneticModel;
353 }
354
355 /**
356 * Constructor.
357 *
358 * @param commonAxisUsed indicates whether z-axis is assumed to be common
359 * for the accelerometer, gyroscope and magnetometer.
360 * @param magneticModel Earth's magnetic model. If null, a default model
361 * will be used instead.
362 * @param listener listener to handle events raised by this calibrator.
363 */
364 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(
365 final boolean commonAxisUsed, final WorldMagneticModel magneticModel,
366 final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener) {
367 this(commonAxisUsed, listener);
368 this.magneticModel = magneticModel;
369 }
370
371 /**
372 * Constructor.
373 *
374 * @param measurements collection of body magnetic flux density measurements
375 * taken at different frames (positions and orientations).
376 * @param commonAxisUsed indicates whether z-axis is assumed to be common
377 * for the accelerometer, gyroscope and magnetometer.
378 * @param magneticModel Earth's magnetic model. If null, a default model
379 * will be used instead.
380 */
381 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(
382 final Collection<? extends FrameBodyMagneticFluxDensity> measurements,
383 final boolean commonAxisUsed, final WorldMagneticModel magneticModel) {
384 this(measurements, commonAxisUsed);
385 this.magneticModel = magneticModel;
386 }
387
388 /**
389 * Constructor.
390 *
391 * @param measurements collection of body magnetic flux density measurements
392 * taken at different frames (positions and orientations).
393 * @param commonAxisUsed indicates whether z-axis is assumed to be common
394 * for the accelerometer, gyroscope and magnetometer.
395 * @param magneticModel Earth's magnetic model. If null, a default model
396 * will be used instead.
397 * @param listener listener to handle events raised by this calibrator.
398 */
399 public KnownFrameMagnetometerLinearLeastSquaresCalibrator(
400 final Collection<? extends FrameBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
401 final WorldMagneticModel magneticModel,
402 final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener) {
403 this(measurements, commonAxisUsed, listener);
404 this.magneticModel = magneticModel;
405 }
406
407 /**
408 * Gets a collection of body magnetic flux density measurements taken at different
409 * frames (positions, orientations and velocities).
410 * If a single device IMU needs to be calibrated, typically all measurements are
411 * taken at the same position, with zero velocity and multiple orientations.
412 * However, if we just want to calibrate a given IMU model (e.g. obtain
413 * an average and less precise calibration for the IMU of a given phone model),
414 * we could take measurements collected throughout the planet at multiple positions
415 * while the phone remains static (e.g. while charging), hence each measurement
416 * position will change, velocity will remain zero and orientation will be
417 * typically constant at horizontal orientation while the phone remains on a
418 * flat surface.
419 *
420 * @return a collection of body magnetic flux density measurements taken at different
421 * frames (positions, orientations and velocities).
422 */
423 @Override
424 public Collection<FrameBodyMagneticFluxDensity> getMeasurements() {
425 return measurements;
426 }
427
428 /**
429 * Sets a collection of body magnetic flux density measurements taken at different
430 * frames (positions, orientations and velocities).
431 * If a single device IMU needs to be calibrated, typically all measurements are
432 * taken at the same position, with zero velocity and multiple orientations.
433 * However, if we just want to calibrate the a given IMU model (e.g. obtain
434 * an average and less precise calibration for the IMU of a given phone model),
435 * we could take measurements collected throughout the planet at multiple positions
436 * while the phone remains static (e.g. while charging), hence each measurement
437 * position will change, velocity will remain zero and orientation will be
438 * typically constant at horizontal orientation while the phone remains on a
439 * flat surface.
440 *
441 * @param measurements collection of body magnetic flux density measurements
442 * taken at different frames (positions, orientations
443 * and velocities).
444 * @throws LockedException if estimator is currently running.
445 */
446 @Override
447 public void setMeasurements(final Collection<? extends FrameBodyMagneticFluxDensity> measurements)
448 throws LockedException {
449 if (running) {
450 throw new LockedException();
451 }
452 //noinspection unchecked
453 this.measurements = (Collection<FrameBodyMagneticFluxDensity>) measurements;
454 }
455
456 /**
457 * Indicates the type of measurement used by this calibrator.
458 *
459 * @return type of measurement used by this calibrator.
460 */
461 @Override
462 public MagnetometerCalibratorMeasurementType getMeasurementType() {
463 return MagnetometerCalibratorMeasurementType.FRAME_BODY_MAGNETIC_FLUX_DENSITY;
464 }
465
466 /**
467 * Indicates whether this calibrator requires ordered measurements in a
468 * list or not.
469 *
470 * @return true if measurements must be ordered, false otherwise.
471 */
472 @Override
473 public boolean isOrderedMeasurementsRequired() {
474 return false;
475 }
476
477 /**
478 * Indicates whether this calibrator requires quality scores for each
479 * measurement or not.
480 *
481 * @return true if quality scores are required, false otherwise.
482 */
483 @Override
484 public boolean isQualityScoresRequired() {
485 return false;
486 }
487
488 /**
489 * Indicates whether z-axis is assumed to be common for accelerometer,
490 * gyroscope and magnetometer.
491 * When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
492 *
493 * @return true if z-axis is assumed to be common for accelerometer,
494 * gyroscope and magnetometer, false otherwise.
495 */
496 @Override
497 public boolean isCommonAxisUsed() {
498 return commonAxisUsed;
499 }
500
501 /**
502 * Specifies whether z-axis is assumed to be common for accelerometer and
503 * gyroscope.
504 * When enabled, this eliminates 3 variables from Mm matrix.
505 *
506 * @param commonAxisUsed true if z-axis is assumed to be common for
507 * accelerometer, gyroscope and magnetometer, false
508 * otherwise.
509 * @throws LockedException if estimator is currently running.
510 */
511 @Override
512 public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
513 if (running) {
514 throw new LockedException();
515 }
516
517 this.commonAxisUsed = commonAxisUsed;
518 }
519
520 /**
521 * Gets listener to handle events raised by this calibrator.
522 *
523 * @return listener to handle events raised by this calibrator.
524 */
525 @Override
526 public KnownFrameMagnetometerLinearLeastSquaresCalibratorListener getListener() {
527 return listener;
528 }
529
530 /**
531 * Sets listener to handle events raised by this calibrator.
532 *
533 * @param listener listener to handle events raised by this calibrator.
534 * @throws LockedException if estimator is currently running.
535 */
536 @Override
537 public void setListener(final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener)
538 throws LockedException {
539 if (running) {
540 throw new LockedException();
541 }
542
543 this.listener = listener;
544 }
545
546 /**
547 * Gets minimum number of required measurements.
548 *
549 * @return minimum number of required measurements.
550 */
551 @Override
552 public int getMinimumRequiredMeasurements() {
553 return MINIMUM_MEASUREMENTS;
554 }
555
556 /**
557 * Indicates whether calibrator is ready to start the estimator.
558 *
559 * @return true if calibrator is ready, false otherwise.
560 */
561 @Override
562 public boolean isReady() {
563 return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
564 }
565
566 /**
567 * Indicates whether calibrator is currently running or no.
568 *
569 * @return true if calibrator is running, false otherwise.
570 */
571 @Override
572 public boolean isRunning() {
573 return running;
574 }
575
576 /**
577 * Gets Earth's magnetic model.
578 *
579 * @return Earth's magnetic model or null if not provided.
580 */
581 public WorldMagneticModel getMagneticModel() {
582 return magneticModel;
583 }
584
585 /**
586 * Sets Earth's magnetic model.
587 * If not provided a default model will be loaded internally.
588 *
589 * @param magneticModel Earth's magnetic model to be set.
590 * @throws LockedException if calibrator is currently running.
591 */
592 public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
593 if (running) {
594 throw new LockedException();
595 }
596 this.magneticModel = magneticModel;
597 }
598
599 /**
600 * Estimates accelerometer calibration parameters containing scale factors
601 * and cross-coupling errors.
602 *
603 * @throws LockedException if calibrator is currently running.
604 * @throws NotReadyException if calibrator is not ready.
605 * @throws CalibrationException if calibration fails for numerical reasons.
606 */
607 @Override
608 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
609 if (running) {
610 throw new LockedException();
611 }
612
613 if (!isReady()) {
614 throw new NotReadyException();
615 }
616
617 try {
618 running = true;
619
620 if (listener != null) {
621 listener.onCalibrateStart(this);
622 }
623
624 if (commonAxisUsed) {
625 calibrateCommonAxis();
626 } else {
627 calibrateGeneral();
628 }
629
630 if (listener != null) {
631 listener.onCalibrateEnd(this);
632 }
633
634 } catch (final AlgebraException | IOException e) {
635 throw new CalibrationException(e);
636 } finally {
637 running = false;
638 }
639 }
640
641 /**
642 * Gets array containing x,y,z components of estimated magnetometer
643 * hard-iron biases expressed in Teslas (T).
644 *
645 * @return array containing x,y,z components of estimated magnetometer
646 * hard-iron biases.
647 */
648 @Override
649 public double[] getEstimatedHardIron() {
650 return estimatedHardIron;
651 }
652
653 /**
654 * Gets array containing x,y,z components of estimated magnetometer
655 * hard-iron biases expressed in Teslas (T).
656 *
657 * @param result instance where estimated magnetometer biases will be
658 * stored.
659 * @return true if result instance was updated, false otherwise (when
660 * estimation is not yet available).
661 */
662 @Override
663 public boolean getEstimatedHardIron(final double[] result) {
664 if (estimatedHardIron != null) {
665 System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
666 return true;
667 } else {
668 return false;
669 }
670 }
671
672 /**
673 * Gets column matrix containing x,y,z components of estimated
674 * magnetometer hard-iron biases expressed in Teslas (T).
675 *
676 * @return column matrix containing x,y,z components of estimated
677 * magnetometer hard-iron biases.
678 */
679 @Override
680 public Matrix getEstimatedHardIronAsMatrix() {
681 return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
682 }
683
684 /**
685 * Gets column matrix containing x,y,z components of estimated
686 * magnetometer hard-iron biases expressed in Teslas (T).
687 *
688 * @param result instance where result data will be stored.
689 * @return true if result was updated, false otherwise.
690 * @throws WrongSizeException if provided result instance has invalid size.
691 */
692 @Override
693 public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
694 if (estimatedHardIron != null) {
695 result.fromArray(estimatedHardIron);
696 return true;
697 } else {
698 return false;
699 }
700 }
701
702 /**
703 * Gets x coordinate of estimated magnetometer bias expressed in
704 * Teslas (T).
705 *
706 * @return x coordinate of estimated magnetometer bias or null if not
707 * available.
708 */
709 @Override
710 public Double getEstimatedHardIronX() {
711 return estimatedHardIron != null ? estimatedHardIron[0] : null;
712 }
713
714 /**
715 * Gets y coordinate of estimated magnetometer bias expressed in
716 * Teslas (T).
717 *
718 * @return y coordinate of estimated magnetometer bias or null if not
719 * available.
720 */
721 @Override
722 public Double getEstimatedHardIronY() {
723 return estimatedHardIron != null ? estimatedHardIron[1] : null;
724 }
725
726 /**
727 * Gets z coordinate of estimated magnetometer bias expressed in
728 * Teslas (T).
729 *
730 * @return z coordinate of estimated magnetometer bias or null if not
731 * available.
732 */
733 @Override
734 public Double getEstimatedHardIronZ() {
735 return estimatedHardIron != null ? estimatedHardIron[2] : null;
736 }
737
738 /**
739 * Gets x coordinate of estimated magnetometer bias.
740 *
741 * @return x coordinate of estimated magnetometer bias.
742 */
743 @Override
744 public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
745 return estimatedHardIron != null
746 ? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
747 }
748
749 /**
750 * Gets x coordinate of estimated magnetometer bias.
751 *
752 * @param result instance where result will be stored.
753 * @return true if estimated magnetometer bias is available, false otherwise.
754 */
755 @Override
756 public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
757 if (estimatedHardIron != null) {
758 result.setValue(estimatedHardIron[0]);
759 result.setUnit(MagneticFluxDensityUnit.TESLA);
760 return true;
761 } else {
762 return false;
763 }
764 }
765
766 /**
767 * Gets y coordinate of estimated magnetometer bias.
768 *
769 * @return y coordinate of estimated magnetometer bias.
770 */
771 @Override
772 public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
773 return estimatedHardIron != null
774 ? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
775 }
776
777 /**
778 * Gets y coordinate of estimated magnetometer bias.
779 *
780 * @param result instance where result will be stored.
781 * @return true if estimated magnetometer bias is available, false otherwise.
782 */
783 @Override
784 public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
785 if (estimatedHardIron != null) {
786 result.setValue(estimatedHardIron[1]);
787 result.setUnit(MagneticFluxDensityUnit.TESLA);
788 return true;
789 } else {
790 return false;
791 }
792 }
793
794 /**
795 * Gets z coordinate of estimated magnetometer bias.
796 *
797 * @return z coordinate of estimated magnetometer bias.
798 */
799 @Override
800 public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
801 return estimatedHardIron != null
802 ? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
803 }
804
805 /**
806 * Gets z coordinate of estimated magnetometer bias.
807 *
808 * @param result instance where result will be stored.
809 * @return true if estimated magnetometer bias is available, false otherwise.
810 */
811 @Override
812 public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
813 if (estimatedHardIron != null) {
814 result.setValue(estimatedHardIron[2]);
815 result.setUnit(MagneticFluxDensityUnit.TESLA);
816 return true;
817 } else {
818 return false;
819 }
820 }
821
822 /**
823 * Gets estimated magnetometer bias.
824 *
825 * @return estimated magnetometer bias or null if not available.
826 */
827 @Override
828 public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
829 return estimatedHardIron != null
830 ? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
831 estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2]) : null;
832 }
833
834 /**
835 * Gets estimated magnetometer bias.
836 *
837 * @param result instance where result will be stored.
838 * @return true if estimated magnetometer bias is available and result was
839 * modified, false otherwise.
840 */
841 @Override
842 public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
843 if (estimatedHardIron != null) {
844 result.setValueCoordinatesAndUnit(
845 estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2], MagneticFluxDensityUnit.TESLA);
846 return true;
847 } else {
848 return false;
849 }
850 }
851
852 /**
853 * Gets estimated magnetometer soft-iron matrix containing scale factors
854 * and cross coupling errors.
855 * This is the product of matrix Tm containing cross coupling errors and Km
856 * containing scaling factors.
857 * So tat:
858 * <pre>
859 * Mm = [sx mxy mxz] = Tm*Km
860 * [myx sy myz]
861 * [mzx mzy sz ]
862 * </pre>
863 * Where:
864 * <pre>
865 * Km = [sx 0 0 ]
866 * [0 sy 0 ]
867 * [0 0 sz]
868 * </pre>
869 * and
870 * <pre>
871 * Tm = [1 -alphaXy alphaXz ]
872 * [alphaYx 1 -alphaYz]
873 * [-alphaZx alphaZy 1 ]
874 * </pre>
875 * Hence:
876 * <pre>
877 * Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
878 * [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
879 * [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
880 * </pre>
881 * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
882 * are considered to be zero if the accelerometer z-axis is assumed to be the same
883 * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
884 * becomes upper diagonal:
885 * <pre>
886 * Mm = [sx mxy mxz]
887 * [0 sy myz]
888 * [0 0 sz ]
889 * </pre>
890 * Values of this matrix are unit-less.
891 *
892 * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
893 * or null if not available.
894 */
895 @Override
896 public Matrix getEstimatedMm() {
897 return estimatedMm;
898 }
899
900 /**
901 * Gets estimated x-axis scale factor.
902 *
903 * @return estimated x-axis scale factor or null if not available.
904 */
905 @Override
906 public Double getEstimatedSx() {
907 return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
908 }
909
910 /**
911 * Gets estimated y-axis scale factor.
912 *
913 * @return estimated y-axis scale factor or null if not available.
914 */
915 @Override
916 public Double getEstimatedSy() {
917 return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
918 }
919
920 /**
921 * Gets estimated z-axis scale factor.
922 *
923 * @return estimated z-axis scale factor or null if not available.
924 */
925 @Override
926 public Double getEstimatedSz() {
927 return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
928 }
929
930 /**
931 * Gets estimated x-y cross-coupling error.
932 *
933 * @return estimated x-y cross-coupling error or null if not available.
934 */
935 @Override
936 public Double getEstimatedMxy() {
937 return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
938 }
939
940 /**
941 * Gets estimated x-z cross-coupling error.
942 *
943 * @return estimated x-z cross-coupling error or null if not available.
944 */
945 @Override
946 public Double getEstimatedMxz() {
947 return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
948 }
949
950 /**
951 * Gets estimated y-x cross-coupling error.
952 *
953 * @return estimated y-x cross-coupling error or null if not available.
954 */
955 @Override
956 public Double getEstimatedMyx() {
957 return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
958 }
959
960 /**
961 * Gets estimated y-z cross-coupling error.
962 *
963 * @return estimated y-z cross-coupling error or null if not available.
964 */
965 @Override
966 public Double getEstimatedMyz() {
967 return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
968 }
969
970 /**
971 * Gets estimated z-x cross-coupling error.
972 *
973 * @return estimated z-x cross-coupling error or null if not available.
974 */
975 @Override
976 public Double getEstimatedMzx() {
977 return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
978 }
979
980 /**
981 * Gets estimated z-y cross-coupling error.
982 *
983 * @return estimated z-y cross-coupling error or null if not available.
984 */
985 @Override
986 public Double getEstimatedMzy() {
987 return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
988 }
989
990 /**
991 * Internal method to perform calibration when common z-axis is assumed
992 * for the accelerometer, gyroscope and magnetometer.
993 *
994 * @throws AlgebraException if there are numerical errors.
995 * @throws IOException if world magnetic model cannot be loaded.
996 */
997 private void calibrateCommonAxis() throws AlgebraException, IOException {
998 // The magnetometer model is:
999 // mBmeas = bm + (I + Mm) * mBtrue + w
1000
1001 // Ideally a least squares solution tries to minimize noise component, so:
1002 // mBmeas = bm + (I + Mm) * mBtrue
1003
1004 // Hence:
1005 // [mBmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [mBtruex]
1006 // [mBmeasy] = [by] [0 1 0] [myx sy myz] [mBtruey]
1007 // [mBmeasz] = [bz] [0 0 1] [mzx mzy sz ] [mBtruez]
1008
1009 // where myx = mzx = mzy = 0
1010
1011 // Hence:
1012 // [mBmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [mBtruex]
1013 // [mBmeasy] = [by] [0 1 0] [0 sy myz] [mBtruey]
1014 // [mBmeasz] = [bz] [0 0 1] [0 0 sz ] [mBtruez]
1015
1016 // [mBmeasx] = [bx] + [1+sx mxy mxz ][mBtruex]
1017 // [mBmeasy] [by] [0 1+sy myz ][mBtruey]
1018 // [mBmeasz] [bz] [0 0 1+sz][mBtruez]
1019
1020 // mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
1021 // mBmeasy = by + (1+sy) * mBtruey + myz * mBtruez
1022 // mBmeasz = bz + (1+sz) * mBtruez
1023
1024 // Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myz
1025 // Reordering:
1026 // mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
1027 // mBmeasy = by + mBtruey + sy * mBtruey + myz * mBtruez
1028 // mBmeasz = bz + mBtruez + sz * mBtruez
1029
1030 // mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
1031 // mBmeasy - mBtruey = by + sy * mBtruey + myz * mBtruez
1032 // mBmeasz - mBtruez = bz + sz * mBtruez
1033
1034 // [1 0 0 mBtruex 0 0 mBtruey mBtruez 0 ][bx ] = [mBmeasx - mBtruex]
1035 // [0 1 0 0 mBtruey 0 0 0 mBtruez][by ] [mBmeasy - mBtruey]
1036 // [0 0 1 0 0 mBtruez 0 0 0 ][bz ] [mBmeasz - mBtruez]
1037 // [sx ]
1038 // [sy ]
1039 // [sz ]
1040 // [mxy]
1041 // [mxz]
1042 // [myz]
1043
1044 final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
1045 if (magneticModel != null) {
1046 wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
1047 } else {
1048 wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
1049 }
1050
1051 final var expectedMagneticFluxDensity = new BodyMagneticFluxDensity();
1052 final var nedFrame = new NEDFrame();
1053 final var earthB = new NEDMagneticFluxDensity();
1054 final var cbn = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
1055 final var cnb = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
1056
1057 final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
1058 final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
1059 final var b = new Matrix(rows, 1);
1060 var i = 0;
1061 for (final var measurement : measurements) {
1062 final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
1063
1064 // estimate Earth magnetic flux density at frame position and
1065 // timestamp using WMM
1066 final var ecefFrame = measurement.getFrame();
1067 ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);
1068
1069 final var year = measurement.getYear();
1070
1071 final var latitude = nedFrame.getLatitude();
1072 final var longitude = nedFrame.getLongitude();
1073 final var height = nedFrame.getHeight();
1074
1075 nedFrame.getCoordinateTransformation(cbn);
1076 cbn.inverse(cnb);
1077
1078 wmmEstimator.estimate(latitude, longitude, height, year, earthB);
1079
1080 // estimate expected body magnetic flux density taking into
1081 // account body attitude (inverse of frame orientation) and
1082 // estimated Earth magnetic flux density
1083 BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);
1084
1085 final var bMeasX = measuredMagneticFluxDensity.getBx();
1086 final var bMeasY = measuredMagneticFluxDensity.getBy();
1087 final var bMeasZ = measuredMagneticFluxDensity.getBz();
1088
1089 final var bTrueX = expectedMagneticFluxDensity.getBx();
1090 final var bTrueY = expectedMagneticFluxDensity.getBy();
1091 final var bTrueZ = expectedMagneticFluxDensity.getBz();
1092
1093 a.setElementAt(i, 0, 1.0);
1094 a.setElementAt(i, 3, bTrueX);
1095 a.setElementAt(i, 6, bTrueY);
1096 a.setElementAt(i, 7, bTrueZ);
1097
1098 b.setElementAtIndex(i, bMeasX - bTrueX);
1099 i++;
1100
1101 a.setElementAt(i, 1, 1.0);
1102 a.setElementAt(i, 4, bTrueY);
1103 a.setElementAt(i, 8, bTrueZ);
1104
1105 b.setElementAtIndex(i, bMeasY - bTrueY);
1106 i++;
1107
1108 a.setElementAt(i, 2, 1.0);
1109 a.setElementAt(i, 5, bTrueZ);
1110
1111 b.setElementAtIndex(i, bMeasZ - bTrueZ);
1112 i++;
1113 }
1114
1115 final var unknowns = Utils.solve(a, b);
1116
1117 final var bx = unknowns.getElementAtIndex(0);
1118 final var by = unknowns.getElementAtIndex(1);
1119 final var bz = unknowns.getElementAtIndex(2);
1120 final var sx = unknowns.getElementAtIndex(3);
1121 final var sy = unknowns.getElementAtIndex(4);
1122 final var sz = unknowns.getElementAtIndex(5);
1123 final var mxy = unknowns.getElementAtIndex(6);
1124 final var mxz = unknowns.getElementAtIndex(7);
1125 final var myz = unknowns.getElementAtIndex(8);
1126
1127 fillHardIronBiases(bx, by, bz);
1128 fillMm(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
1129 }
1130
1131 /**
1132 * Internal method to perform general calibration.
1133 *
1134 * @throws AlgebraException if there are numerical errors.
1135 * @throws IOException if world magnetic model cannot be loaded.
1136 */
1137 private void calibrateGeneral() throws AlgebraException, IOException {
1138 // The magnetometer model is:
1139 // mBmeas = bm + (I + Mm) * mBtrue + w
1140
1141 // Ideally a least squares solution tries to minimize noise component, so:
1142 // mBmeas = bm + (I + Mm) * mBtrue
1143
1144 // Hence:
1145 // [mBmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [mBtruex]
1146 // [mBmeasy] = [by] [0 1 0] [myx sy myz] [mBtruey]
1147 // [mBmeasz] = [bz] [0 0 1] [mzx mzy sz ] [mBtruez]
1148
1149 // [mBmeasx] = [bx] + [1+sx mxy mxz ][mBtruex]
1150 // [mBmeasy] [by] [myx 1+sy myz ][mBtruey]
1151 // [mBmeasz] [bz] [mzx mzy 1+sz][mBtruez]
1152
1153 // mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
1154 // mBmeasy = by + myx * mBtruex + (1+sy) * mBtruey + myz * mBtruez
1155 // mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + (1+sz) * mBtruez
1156
1157 // Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
1158 // Reordering:
1159 // mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
1160 // mBmeasy = by + myx * mBtruex + mBtruey + sy * mBtruey + myz * mBtruez
1161 // mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + mBtruez + sz * mBtruez
1162
1163 // mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
1164 // mBmeasy - mBtruey = by + myx * mBtruex + sy * mBtruey + myz * mBtruez
1165 // mBmeasz - mBtruez = bz + mzx * mBtruex + mzy * mBtruey + sz * mBtruez
1166
1167 // [1 0 0 mBtruex 0 0 mBtruey mBtruez 0 0 0 0 ][bx ] = [mBmeasx - mBtruex]
1168 // [0 1 0 0 mBtruey 0 0 0 mBtruex mBtruez 0 0 ][by ] [mBmeasy - mBtruey]
1169 // [0 0 1 0 0 mBtruez 0 0 0 0 mBtruex mBtruey][bz ] [mBmeasz - mBtruez]
1170 // [sx ]
1171 // [sy ]
1172 // [sz ]
1173 // [mxy]
1174 // [mxz]
1175 // [myx]
1176 // [myz]
1177 // [mzx]
1178 // [mzy]
1179
1180 final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
1181 if (magneticModel != null) {
1182 wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
1183 } else {
1184 wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
1185 }
1186
1187 final var expectedMagneticFluxDensity = new BodyMagneticFluxDensity();
1188 final var nedFrame = new NEDFrame();
1189 final var earthB = new NEDMagneticFluxDensity();
1190 final var cbn = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
1191 final var cnb = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
1192
1193 final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
1194 final var a = new Matrix(rows, GENERAL_UNKNOWNS);
1195 final var b = new Matrix(rows, 1);
1196 var i = 0;
1197 for (final var measurement : measurements) {
1198 final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
1199
1200 // estimate Earth magnetic flux density at frame position and
1201 // timestamp using WMM
1202 final var ecefFrame = measurement.getFrame();
1203 ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);
1204
1205 final var year = measurement.getYear();
1206
1207 final var latitude = nedFrame.getLatitude();
1208 final var longitude = nedFrame.getLongitude();
1209 final var height = nedFrame.getHeight();
1210
1211 nedFrame.getCoordinateTransformation(cbn);
1212 cbn.inverse(cnb);
1213
1214 wmmEstimator.estimate(latitude, longitude, height, year, earthB);
1215
1216 // estimate expected body magnetic flux density taking into
1217 // account body attitude (inverse of frame orientation) and
1218 // estimated Earth magnetic flux density
1219 BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);
1220
1221 final var bMeasX = measuredMagneticFluxDensity.getBx();
1222 final var bMeasY = measuredMagneticFluxDensity.getBy();
1223 final var bMeasZ = measuredMagneticFluxDensity.getBz();
1224
1225 final var bTrueX = expectedMagneticFluxDensity.getBx();
1226 final var bTrueY = expectedMagneticFluxDensity.getBy();
1227 final var bTrueZ = expectedMagneticFluxDensity.getBz();
1228
1229 a.setElementAt(i, 0, 1.0);
1230 a.setElementAt(i, 3, bTrueX);
1231 a.setElementAt(i, 6, bTrueY);
1232 a.setElementAt(i, 7, bTrueZ);
1233
1234 b.setElementAtIndex(i, bMeasX - bTrueX);
1235 i++;
1236
1237 a.setElementAt(i, 1, 1.0);
1238 a.setElementAt(i, 4, bTrueY);
1239 a.setElementAt(i, 8, bTrueX);
1240 a.setElementAt(i, 9, bTrueZ);
1241
1242 b.setElementAtIndex(i, bMeasY - bTrueY);
1243 i++;
1244
1245 a.setElementAt(i, 2, 1.0);
1246 a.setElementAt(i, 5, bTrueZ);
1247 a.setElementAt(i, 10, bTrueX);
1248 a.setElementAt(i, 11, bTrueY);
1249
1250 b.setElementAtIndex(i, bMeasZ - bTrueZ);
1251 i++;
1252 }
1253
1254 final var unknowns = Utils.solve(a, b);
1255
1256 final var bx = unknowns.getElementAtIndex(0);
1257 final var by = unknowns.getElementAtIndex(1);
1258 final var bz = unknowns.getElementAtIndex(2);
1259 final var sx = unknowns.getElementAtIndex(3);
1260 final var sy = unknowns.getElementAtIndex(4);
1261 final var sz = unknowns.getElementAtIndex(5);
1262 final var mxy = unknowns.getElementAtIndex(6);
1263 final var mxz = unknowns.getElementAtIndex(7);
1264 final var myx = unknowns.getElementAtIndex(8);
1265 final var myz = unknowns.getElementAtIndex(9);
1266 final var mzx = unknowns.getElementAtIndex(10);
1267 final var mzy = unknowns.getElementAtIndex(11);
1268
1269 fillHardIronBiases(bx, by, bz);
1270 fillMm(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
1271 }
1272
1273 /**
1274 * Fills estimated biases array with estimated values.
1275 *
1276 * @param bx x coordinate of bias.
1277 * @param by y coordinate of bias.
1278 * @param bz z coordinate of bias.
1279 */
1280 private void fillHardIronBiases(final double bx, final double by, final double bz) {
1281 if (estimatedHardIron == null) {
1282 estimatedHardIron = new double[BodyMagneticFluxDensity.COMPONENTS];
1283 }
1284
1285 estimatedHardIron[0] = bx;
1286 estimatedHardIron[1] = by;
1287 estimatedHardIron[2] = bz;
1288 }
1289
1290 /**
1291 * Fills scale factor and cross coupling error matrix with estimated values.
1292 *
1293 * @param sx x scale factor
1294 * @param sy y scale factor
1295 * @param sz z scale factor
1296 * @param mxy x-y cross coupling
1297 * @param mxz x-z cross coupling
1298 * @param myx y-x cross coupling
1299 * @param myz y-z cross coupling
1300 * @param mzx z-x cross coupling
1301 * @param mzy z-y cross coupling
1302 * @throws WrongSizeException never happens.
1303 */
1304 private void fillMm(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) throws WrongSizeException {
1307 if (estimatedMm == null) {
1308 estimatedMm = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
1309 }
1310
1311 estimatedMm.setElementAt(0, 0, sx);
1312 estimatedMm.setElementAt(1, 0, myx);
1313 estimatedMm.setElementAt(2, 0, mzx);
1314
1315 estimatedMm.setElementAt(0, 1, mxy);
1316 estimatedMm.setElementAt(1, 1, sy);
1317 estimatedMm.setElementAt(2, 1, mzy);
1318
1319 estimatedMm.setElementAt(0, 2, mxz);
1320 estimatedMm.setElementAt(1, 2, myz);
1321 estimatedMm.setElementAt(2, 2, sz);
1322 }
1323 }