KnownHardIronPositionAndInstantMagnetometerCalibrator.java
/*
* Copyright (C) 2020 Alberto Irurueta Carro (alberto@irurueta.com)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package com.irurueta.navigation.inertial.calibration.magnetometer;
import com.irurueta.algebra.Matrix;
import com.irurueta.navigation.LockedException;
import com.irurueta.navigation.frames.ECEFPosition;
import com.irurueta.navigation.frames.ECEFVelocity;
import com.irurueta.navigation.frames.NEDPosition;
import com.irurueta.navigation.frames.NEDVelocity;
import com.irurueta.navigation.frames.converters.ECEFtoNEDPositionVelocityConverter;
import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
import com.irurueta.navigation.inertial.calibration.CalibrationException;
import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyMagneticFluxDensity;
import com.irurueta.navigation.inertial.wmm.WMMEarthMagneticFluxDensityEstimator;
import com.irurueta.navigation.inertial.wmm.WorldMagneticModel;
import java.io.IOException;
import java.util.Collection;
import java.util.Date;
import java.util.GregorianCalendar;
import java.util.List;
/**
* Estimates magnetometer cross couplings and scaling factors.
* This calibrator uses Levenberg-Marquardt to find a minimum least squared
* error solution.
* <p>
* To use this calibrator at least 7 measurements taken at a single known
* position and instant must be taken at 7 different unknown orientations and
* zero velocity when common z-axis is assumed, otherwise at least 10
* measurements are required.
* <p>
* Measured magnetic flux density is assumed to follow the model shown below:
* <pre>
* mBmeas = bm + (I + Mm) * mBtrue + w
* </pre>
* Where:
* - mBmeas is the measured magnetic flux density. This is a 3x1 vector.
* - bm is magnetometer hard-iron bias. Ideally, on a perfect magnetometer,
* this should be a 3x1 zero vector.
* - I is the 3x3 identity matrix.
* - Mm is the 3x3 soft-iron matrix containing cross-couplings and scaling
* factors. Ideally, on a perfect magnetometer, this should be a 3x3 zero
* matrix.
* - mBtrue is ground-truth magnetic flux density. This is a 3x1 vector.
* - w is measurement noise. This is a 3x1 vector.
* Notice that this calibrator assumes that all measurements are taken in
* a short span of time where Earth magnetic field can be assumed to be
* constant at provided location and instant.
*/
public class KnownHardIronPositionAndInstantMagnetometerCalibrator extends
BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator<
KnownHardIronPositionAndInstantMagnetometerCalibrator,
KnownHardIronPositionAndInstantMagnetometerCalibratorListener> {
/**
* Position where body magnetic flux density measurements have been
* taken.
*/
private NEDPosition position;
/**
* Timestamp expressed as decimal year where magnetic flux density
* measurements have been measured.
*/
private Double year = convertTime(System.currentTimeMillis());
/**
* Contains Earth's magnetic model.
*/
private WorldMagneticModel magneticModel;
/**
* Constructor.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator() {
super();
}
/**
* Constructor.
*
* @param listener listener to handle events raised by this calibrator.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
super(listener);
}
/**
* Constructor.
*
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final Collection<StandardDeviationBodyMagneticFluxDensity> measurements) {
super(measurements);
}
/**
* Constructor.
*
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(final boolean commonAxisUsed) {
super(commonAxisUsed);
}
/**
* Constructor.
*
* @param magneticModel Earth's magnetic model. If null, a default model
* will be used instead.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(final WorldMagneticModel magneticModel) {
super();
this.magneticModel = magneticModel;
}
/**
* Constructor.
*
* @param hardIron known hard-iron.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(final double[] hardIron) {
super(hardIron);
}
/**
* Constructor.
*
* @param hardIron known hard-iron.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(final Matrix hardIron) {
super(hardIron);
}
/**
* Constructor.
*
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(final Matrix hardIron, final Matrix initialMm) {
super(hardIron, initialMm);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(final NEDPosition position) {
super();
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements) {
super(measurements);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final Collection<StandardDeviationBodyMagneticFluxDensity> measurements,
final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
super(measurements, listener);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed) {
super(measurements, commonAxisUsed);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed,
final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
super(measurements, commonAxisUsed, listener);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final double[] hardIron) {
super(measurements, hardIron);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final double[] hardIron, final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
super(measurements, hardIron, listener);
this.position = position;
}
/**
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] hardIron) {
super(measurements, commonAxisUsed, hardIron);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] hardIron,
final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
super(measurements, commonAxisUsed, hardIron, listener);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron) {
super(measurements, hardIron);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron, final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
super(measurements, hardIron, listener);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix hardIron) {
super(measurements, commonAxisUsed, hardIron);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix hardIron,
final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
super(measurements, commonAxisUsed, hardIron, listener);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron, final Matrix initialMm) {
super(measurements, hardIron, initialMm);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param listener listener to handle events raised by this calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron, final Matrix initialMm,
final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
super(measurements, hardIron, initialMm, listener);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix hardIron, final Matrix initialMm) {
super(measurements, commonAxisUsed, hardIron, initialMm);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param listener listener to handle events raised by this calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix hardIron, final Matrix initialMm,
final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
super(measurements, commonAxisUsed, hardIron, initialMm, listener);
this.position = position;
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(final ECEFPosition position) {
this(convertPosition(position));
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements) {
this(convertPosition(position), measurements);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
this(convertPosition(position), measurements, listener);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed) {
this(convertPosition(position), measurements, commonAxisUsed);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed,
final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
this(convertPosition(position), measurements, commonAxisUsed, listener);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final double[] hardIron) {
this(convertPosition(position), measurements, hardIron);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final double[] hardIron, final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
this(convertPosition(position), measurements, hardIron, listener);
}
/**
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] hardIron) {
this(convertPosition(position), measurements, commonAxisUsed, hardIron);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] hardIron,
final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
this(convertPosition(position), measurements, commonAxisUsed, hardIron, listener);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron) {
this(convertPosition(position), measurements, hardIron);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron, final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
this(convertPosition(position), measurements, hardIron, listener);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix hardIron) {
this(convertPosition(position), measurements, commonAxisUsed, hardIron);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix hardIron,
final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
this(convertPosition(position), measurements, commonAxisUsed, hardIron, listener);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron, final Matrix initialMm) {
this(convertPosition(position), measurements, hardIron, initialMm);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param listener listener to handle events raised by this calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron, final Matrix initialMm,
final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
this(convertPosition(position), measurements, hardIron, initialMm, listener);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix hardIron, final Matrix initialMm) {
this(convertPosition(position), measurements, commonAxisUsed, hardIron, initialMm);
}
/**
* Constructor.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param listener listener to handle events raised by this calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public KnownHardIronPositionAndInstantMagnetometerCalibrator(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix hardIron, final Matrix initialMm,
final KnownHardIronPositionAndInstantMagnetometerCalibratorListener listener) {
this(convertPosition(position), measurements, commonAxisUsed, hardIron, initialMm, listener);
}
/**
* Gets position where body magnetic flux density measurements have been
* taken.
*
* @return position where body magnetic flux density measurements have
* been taken.
*/
public NEDPosition getNedPosition() {
return position;
}
/**
* Sets position where body magnetic flux density measurements have been
* taken.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final NEDPosition position) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
this.position = position;
}
/**
* Gets position where body magnetic flux density measurements have been
* taken expressed in ECEF coordinates.
*
* @return position where body magnetic flux density measurements have
* been taken or null if not available.
*/
public ECEFPosition getEcefPosition() {
if (position != null) {
final var result = new ECEFPosition();
getEcefPosition(result);
return result;
} else {
return null;
}
}
/**
* Gets position where body magnetic flux density measurements have been
* taken expressed in ECEF coordinates.
*
* @param result instance where result will be stored.
* @return true if ECEF position could be computed, false otherwise.
*/
public boolean getEcefPosition(final ECEFPosition result) {
if (position != null) {
final var velocity = new ECEFVelocity();
NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
position.getLatitude(), position.getLongitude(), position.getHeight(),
0.0, 0.0, 0.0, result, velocity);
return true;
} else {
return false;
}
}
/**
* Sets position where body magnetic flux density measurements have been
* taken expressed in ECEF coordinates.
*
* @param position position where body magnetic flux density have been
* taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final ECEFPosition position) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
this.position = convertPosition(position);
}
/**
* Gets timestamp expressed as decimal year where magnetic flux density
* measurements have been measured.
*
* @return timestamp expressed as decimal year or null if not defined.
*/
public Double getYear() {
return year;
}
/**
* Sets timestamp expressed as decimal year where magnetic flux density
* measurements have been measured.
*
* @param year timestamp expressed as decimal year.
* @throws LockedException if calibrator is currently running.
*/
public void setYear(final Double year) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
this.year = year;
}
/**
* Sets timestamp when magnetic flux density measurements have been
* measured.
*
* @param timestampMillis a timestamp expressed in milliseconds since
* epoch time (January 1st, 1970 at midnight).
* @throws LockedException if calibrator is currently running.
*/
public void setTime(final Long timestampMillis) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
year = convertTime(timestampMillis);
}
/**
* Sets timestamp when magnetic flux density measurements have been
* measured.
*
* @param date a date instance containing a timestamp.
* @throws LockedException if calibrator is currently running.
*/
public void setTime(final Date date) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
year = convertTime(date);
}
/**
* Sets timestamp when magnetic flux density measurements have been
* measured.
*
* @param calendar a calendar instance containing a timestamp.
* @throws LockedException if calibrator is currently running.
*/
public void setTime(final GregorianCalendar calendar) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
year = convertTime(calendar);
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && position != null && year != null;
}
/**
* Gets Earth's magnetic model.
*
* @return Earth's magnetic model or null if not provided.
*/
public WorldMagneticModel getMagneticModel() {
return magneticModel;
}
/**
* Sets Earth's magnetic model.
*
* @param magneticModel Earth's magnetic model to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
this.magneticModel = magneticModel;
}
/**
* Called before calibration occurs.
* This can be overridden by subclasses.
*
* @throws CalibrationException if anything fails.
*/
@Override
protected void onBeforeCalibrate() throws CalibrationException {
computeGroundTruthMagneticFluxDensityNorm();
}
/**
* Computes expected ground truth magnetic flux density norm based on World Magnetic Model.
* Notice that actual magnetic field measured by devices might differ from this value, since it might be attenuated
* or other devices might interfere.
*
* @throws CalibrationException if world magnetic model cannot be loaded.
*/
private void computeGroundTruthMagneticFluxDensityNorm() throws CalibrationException {
final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
if (magneticModel != null) {
wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
} else {
try {
wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
} catch (final IOException e) {
throw new CalibrationException(e);
}
}
final var pos = getNedPosition();
final var earthB = wmmEstimator.estimate(pos, year);
groundTruthMagneticFluxDensityNorm = earthB.getNorm();
}
/**
* Converts a time instance expressed in milliseconds since epoch time
* (January 1st, 1970 at midnight) to a decimal year.
*
* @param timestampMillis milliseconds value to be converted.
* @return converted value expressed in decimal years.
*/
private static Double convertTime(final Long timestampMillis) {
if (timestampMillis == null) {
return null;
}
final var calendar = new GregorianCalendar();
calendar.setTimeInMillis(timestampMillis);
return convertTime(calendar);
}
/**
* Converts a time instant contained ina date object to a
* decimal year.
*
* @param date a time instance to be converted.
* @return converted value expressed in decimal years.
*/
private static Double convertTime(final Date date) {
if (date == null) {
return null;
}
final var calendar = new GregorianCalendar();
calendar.setTime(date);
return convertTime(calendar);
}
/**
* Converts a time instant contained in a gregorian calendar to a
* decimal year.
*
* @param calendar calendar containing a specific instant to be
* converted.
* @return converted value expressed in decimal years.
*/
private static Double convertTime(final GregorianCalendar calendar) {
if (calendar == null) {
return null;
}
return WMMEarthMagneticFluxDensityEstimator.convertTime(calendar);
}
/**
* Converts provided ECEF position to position expressed in NED
* coordinates.
*
* @param position ECEF position to be converted.
* @return converted position expressed in NED coordinates.
*/
private static NEDPosition convertPosition(final ECEFPosition position) {
final var velocity = new NEDVelocity();
final var result = new NEDPosition();
ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
position.getX(), position.getY(), position.getZ(),
0.0, 0.0, 0.0, result, velocity);
return result;
}
}