Package com.irurueta.navigation.inertial.calibration.intervals.thresholdfactor

Class MagnetometerIntervalDetectorThresholdFactorOptimizer

java.lang.Object
com.irurueta.navigation.inertial.calibration.intervals.thresholdfactor.IntervalDetectorThresholdFactorOptimizer<com.irurueta.navigation.inertial.BodyKinematicsAndMagneticFluxDensity,MagnetometerIntervalDetectorThresholdFactorOptimizerDataSource>
com.irurueta.navigation.inertial.calibration.intervals.thresholdfactor.MagnetometerIntervalDetectorThresholdFactorOptimizer
All Implemented Interfaces:
com.irurueta.navigation.inertial.calibration.AccelerometerNoiseRootPsdSource
Direct Known Subclasses:
BracketedMagnetometerIntervalDetectorThresholdFactorOptimizer, ExhaustiveMagnetometerIntervalDetectorThresholdFactorOptimizer
public abstract class MagnetometerIntervalDetectorThresholdFactorOptimizer extends IntervalDetectorThresholdFactorOptimizer<com.irurueta.navigation.inertial.BodyKinematicsAndMagneticFluxDensity,MagnetometerIntervalDetectorThresholdFactorOptimizerDataSource> implements com.irurueta.navigation.inertial.calibration.AccelerometerNoiseRootPsdSource
Optimizes the threshold factor for interval detection of magnetometer data based on results of calibration. Implementations of this class will attempt to find the best threshold factor between the provided range of values. Only magnetometer calibrators based on unknown orientation are supported, in other terms, calibrators must be MagnetometerNonLinearCalibrator and must support MagnetometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_MAGNETIC_FLUX_DENSITY.

  • Field Details

    • DEFAULT_MIN_THRESHOLD_FACTOR

      public static final double DEFAULT_MIN_THRESHOLD_FACTOR
      Default minimum threshold factor.
      See Also:

    • DEFAULT_MAX_THRESHOLD_FACTOR

      public static final double DEFAULT_MAX_THRESHOLD_FACTOR
      Default maximum threshold factor.
      See Also:

    • minThresholdFactor

      protected double minThresholdFactor
      Minimum threshold factor.

    • maxThresholdFactor

      protected double maxThresholdFactor
      Maximum threshold factor.

    • calibrator

      private com.irurueta.navigation.inertial.calibration.magnetometer.MagnetometerNonLinearCalibrator calibrator
      Magnetometer calibrator.

    • generator

      private com.irurueta.navigation.inertial.calibration.generators.MagnetometerMeasurementsGenerator generator
      A measurement generator for magnetometer calibrators.

    • measurements

      private List<com.irurueta.navigation.inertial.calibration.StandardDeviationBodyMagneticFluxDensity> measurements
      Generated measurements to be used for magnetometer calibration.

    • qualityScoreMapper

      private QualityScoreMapper<com.irurueta.navigation.inertial.calibration.StandardDeviationBodyMagneticFluxDensity> qualityScoreMapper
      Mapper to convert StandardDeviationBodyMagneticFluxDensity measurements into quality scores.

    • baseNoiseLevel

      private double baseNoiseLevel
      Accelerometer base noise level that has been detected during initialization of the best solution that has been found expressed in meters per squared second (m/s^2). This is equal to the standard deviation of the accelerometer measurements during the initialization phase.

    • threshold

      private double threshold
      Threshold to determine static/dynamic period changes expressed in meters per squared second (m/s^2) for the best calibration solution that has been found.

    • estimatedCovariance

      private com.irurueta.algebra.Matrix estimatedCovariance
      Estimated covariance matrix for estimated parameters.

    • estimatedMm

      private com.irurueta.algebra.Matrix estimatedMm
      Estimated magnetometer soft-iron matrix containing scale factors and cross-coupling errors. This is the product of matrix Tm containing cross-coupling errors and Km containing scaling factors. So that: 
           Mm = [sx    mxy  mxz] = Tm*Km
          [myx   sy   myz]
          [mzx   mzy  sz ]
      Where: 
           Km = [sx 0   0 ]
          [0  sy  0 ]
          [0  0   sz]
      and 
           Tm = [1          -alphaXy    alphaXz ]
          [alphaYx    1           -alphaYz]
          [-alphaZx   alphaZy     1       ]
      Hence: 
           Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
      This instance allows any 3x3 matrix. However, typically alphaYx, alphaZx and alphaZy are considered to be zero if the accelerometer z-axis is assumed to be the same as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix becomes upper diagonal: 
           Mm = [sx    mxy  mxz]
          [0     sy   myz]
          [0     0    sz ]
      Values of this matrix are unit-less.

    • estimatedHardIron

      private double[] estimatedHardIron
      Estimated magnetometer hard-iron biases for each magnetometer axis expressed in Teslas (T).

  • Constructor Details

    • MagnetometerIntervalDetectorThresholdFactorOptimizer

      protected MagnetometerIntervalDetectorThresholdFactorOptimizer()
      Constructor.

    • MagnetometerIntervalDetectorThresholdFactorOptimizer

      protected MagnetometerIntervalDetectorThresholdFactorOptimizer(MagnetometerIntervalDetectorThresholdFactorOptimizerDataSource dataSource)
      Constructor.
      Parameters:
      dataSource - instance in charge of retrieving data for this optimizer.

    • MagnetometerIntervalDetectorThresholdFactorOptimizer

      protected MagnetometerIntervalDetectorThresholdFactorOptimizer(com.irurueta.navigation.inertial.calibration.magnetometer.MagnetometerNonLinearCalibrator calibrator)
      Constructor.
      Parameters:
      calibrator - a magnetometer calibrator to be used to optimize its Mean Square Error (MSE).
      Throws:
      IllegalArgumentException - if magnetometer calibrator does not use StandardDeviationBodyMagneticFluxDensity measurements.

    • MagnetometerIntervalDetectorThresholdFactorOptimizer

      protected MagnetometerIntervalDetectorThresholdFactorOptimizer(MagnetometerIntervalDetectorThresholdFactorOptimizerDataSource dataSource, com.irurueta.navigation.inertial.calibration.magnetometer.MagnetometerNonLinearCalibrator calibrator)
      Constructor.
      Parameters:
      dataSource - instance in charge of retrieving data for this optimizer.
      calibrator - a magnetometer calibrator to be used to optimize its Mean Square Error (MSE).
      Throws:
      IllegalArgumentException - if magnetometer calibrator does not use StandardDeviationBodyMagneticFluxDensity measurements.

  • Method Details

    • getCalibrator

      public com.irurueta.navigation.inertial.calibration.magnetometer.MagnetometerNonLinearCalibrator getCalibrator()
      Gets provided magnetometer calibrator to be used to optimize its Mean Square Error (MSE).
      Returns:
      magnetometer calibrator to be used to optimize its MSE.

    • setCalibrator

      public void setCalibrator(com.irurueta.navigation.inertial.calibration.magnetometer.MagnetometerNonLinearCalibrator calibrator) throws com.irurueta.navigation.LockedException
      Sets a magnetometer calibrator to be used to optimize its Mean Square Error.
      Parameters:
      calibrator - magnetometer calibrator to be used to optimize its MSE.
      Throws:
      com.irurueta.navigation.LockedException - if optimizer is already running.
      IllegalArgumentException - if magnetometer calibrator does not use StandardDeviationBodyMagneticFluxDensity measurements.

    • getQualityScoreMapper

      public QualityScoreMapper<com.irurueta.navigation.inertial.calibration.StandardDeviationBodyMagneticFluxDensity> getQualityScoreMapper()
      Gets mapper to convert StandardDeviationBodyMagneticFluxDensity measurements into quality scores.
      Returns:
      mapper to convert measurements into quality scores.

    • setQualityScoreMapper

      public void setQualityScoreMapper(QualityScoreMapper<com.irurueta.navigation.inertial.calibration.StandardDeviationBodyMagneticFluxDensity> qualityScoreMapper) throws com.irurueta.navigation.LockedException
      Sets mapper to convert StandardDeviationBodyMagneticFluxDensity measurements into quality scores.
      Parameters:
      qualityScoreMapper - mapper to convert measurements into quality scores.
      Throws:
      com.irurueta.navigation.LockedException - if optimizer is already running.

    • getMinThresholdFactor

      public double getMinThresholdFactor()
      Gets the minimum threshold factor.
      Returns:
      minimum threshold factor.

    • getMaxThresholdFactor

      public double getMaxThresholdFactor()
      Gets the maximum threshold factor.
      Returns:
      maximum threshold factor.

    • setThresholdFactorRange

      public void setThresholdFactorRange(double minThresholdFactor, double maxThresholdFactor) throws com.irurueta.navigation.LockedException
      Sets a range of threshold factor values to get an optimized threshold factor value.
      Parameters:
      minThresholdFactor - minimum threshold.
      maxThresholdFactor - maximum threshold.
      Throws:
      com.irurueta.navigation.LockedException - if optimizer is already running.
      IllegalArgumentException - if either minimum or maximum values are negative, or if the minimum value is larger than the maximum one.

    • isReady

      public boolean isReady()
      Indicates whether this optimizer is ready to start optimization.
      Overrides:
      isReady in class IntervalDetectorThresholdFactorOptimizer<com.irurueta.navigation.inertial.BodyKinematicsAndMagneticFluxDensity,MagnetometerIntervalDetectorThresholdFactorOptimizerDataSource>
      Returns:
      true if this optimizer is ready, false otherwise.

    • getTimeInterval

      public double getTimeInterval()
      Gets the time interval between input measurements provided to the IntervalDetectorThresholdFactorOptimizer.getDataSource() expressed in seconds (s).
      Returns:
      time interval between input measurements.

    • setTimeInterval

      public void setTimeInterval(double timeInterval) throws com.irurueta.navigation.LockedException
      Sets time interval between input measurements provided to the IntervalDetectorThresholdFactorOptimizer.getDataSource() expressed in seconds (s).
      Parameters:
      timeInterval - time interval between input measurements.
      Throws:
      com.irurueta.navigation.LockedException - if optimizer is already running.
      IllegalArgumentException - if provided value is negative.

    • getTimeIntervalAsTime

      public com.irurueta.units.Time getTimeIntervalAsTime()
      Gets the time interval between input measurements provided to the IntervalDetectorThresholdFactorOptimizer.getDataSource().
      Returns:
      time interval between input measurements.

    • getTimeIntervalAsTime

      public void getTimeIntervalAsTime(com.irurueta.units.Time result)
      Gets the time interval between input measurements provided to the IntervalDetectorThresholdFactorOptimizer.getDataSource().
      Parameters:
      result - instance where the time interval will be stored.

    • setTimeInterval

      public void setTimeInterval(com.irurueta.units.Time timeInterval) throws com.irurueta.navigation.LockedException
      Sets time interval between input measurements provided to the IntervalDetectorThresholdFactorOptimizer.getDataSource().
      Parameters:
      timeInterval - time interval between input measurements.
      Throws:
      com.irurueta.navigation.LockedException - if optimizer is already running.
      IllegalArgumentException - if provided value is negative.

    • getMinStaticSamples

      public int getMinStaticSamples()
      Gets minimum number of input measurements provided to the IntervalDetectorThresholdFactorOptimizer.getDataSource() required in a static interval to be taken into account. Smaller static intervals will be discarded.
      Returns:
      a minimum number of input measurements required in a static interval to be taken into account.

    • setMinStaticSamples

      public void setMinStaticSamples(int minStaticSamples) throws com.irurueta.navigation.LockedException
      Sets minimum number of input measurements provided to the IntervalDetectorThresholdFactorOptimizer.getDataSource() required in a static interval to be taken into account. Smaller static intervals will be discarded.
      Parameters:
      minStaticSamples - a minimum number of input measurements required in a static interval to be taken into account.
      Throws:
      com.irurueta.navigation.LockedException - if optimizer is already running.
      IllegalArgumentException - if provided value is less than 2.

    • getMaxDynamicSamples

      public int getMaxDynamicSamples()
      Gets maximum number of input measurements provided to the IntervalDetectorThresholdFactorOptimizer.getDataSource() allowed in dynamic intervals.
      Returns:
      maximum number of input measurements allowed in dynamic intervals.

    • setMaxDynamicSamples

      public void setMaxDynamicSamples(int maxDynamicSamples) throws com.irurueta.navigation.LockedException
      Sets maximum number of input measurements provided to the IntervalDetectorThresholdFactorOptimizer.getDataSource() allowed in dynamic intervals.
      Parameters:
      maxDynamicSamples - maximum number of input measurements allowed in dynamic intervals.
      Throws:
      com.irurueta.navigation.LockedException - if optimizer is already running.
      IllegalArgumentException - if provided value is less than 2.

    • getWindowSize

      public int getWindowSize()
      Gets length of number of input measurements provided to the IntervalDetectorThresholdFactorOptimizer.getDataSource() to keep within the window being processed to determine instantaneous accelerometer noise level.
      Returns:
      length of input measurements to keep within the window.

    • setWindowSize

      public void setWindowSize(int windowSize) throws com.irurueta.navigation.LockedException
      Sets length of number of input measurements provided to the IntervalDetectorThresholdFactorOptimizer.getDataSource() to keep within the window being processed to determine instantaneous noise level. Window size must always be larger than the allowed minimum value, which is 2 and must have an odd value.
      Parameters:
      windowSize - length of number of samples to keep within the window.
      Throws:
      com.irurueta.navigation.LockedException - if optimizer is already running.
      IllegalArgumentException - if provided value is not valid.

    • getInitialStaticSamples

      public int getInitialStaticSamples()
      Gets number of input measurements provided to the IntervalDetectorThresholdFactorOptimizer.getDataSource() to be processed initially while keeping the sensor static to find the base noise level when the device is static.
      Returns:
      number of samples to be processed initially.

    • setInitialStaticSamples

      public void setInitialStaticSamples(int initialStaticSamples) throws com.irurueta.navigation.LockedException
      Sets number of input parameters provided to the IntervalDetectorThresholdFactorOptimizer.getDataSource() to be processed initially while keeping the sensor static to find the base noise level when the device is static.
      Parameters:
      initialStaticSamples - number of samples ot be processed initially.
      Throws:
      com.irurueta.navigation.LockedException - if optimizer is already running.
      IllegalArgumentException - if provided value is less than TriadStaticIntervalDetector.MINIMUM_INITIAL_STATIC_SAMPLES.

    • getInstantaneousNoiseLevelFactor

      public double getInstantaneousNoiseLevelFactor()
      Gets factor to determine that a sudden movement has occurred during initialization if instantaneous noise level exceeds accumulated noise level by this factor amount. This factor is unit-less.
      Returns:
      factor to determine that a sudden movement has occurred.

    • setInstantaneousNoiseLevelFactor

      public void setInstantaneousNoiseLevelFactor(double instantaneousNoiseLevelFactor) throws com.irurueta.navigation.LockedException
      Sets factor to determine that a sudden movement has occurred during initialization if instantaneous noise level exceeds accumulated noise level by this factor amount. This factor is unit-less.
      Parameters:
      instantaneousNoiseLevelFactor - factor to determine that a sudden movement has occurred during initialization.
      Throws:
      com.irurueta.navigation.LockedException - if optimizer is already running.
      IllegalArgumentException - if provided value is zero or negative.

    • getBaseNoiseLevelAbsoluteThreshold

      public double getBaseNoiseLevelAbsoluteThreshold()
      Gets the overall absolute threshold to determine whether there has been excessive motion during the whole initialization phase. This threshold is expressed in meters per squared second (m/s^2).
      Returns:
      overall absolute threshold to determine whether there has been excessive motion.

    • setBaseNoiseLevelAbsoluteThreshold

      public void setBaseNoiseLevelAbsoluteThreshold(double baseNoiseLevelAbsoluteThreshold) throws com.irurueta.navigation.LockedException
      Sets the overall absolute threshold to determine whether there has been excessive motion during the whole initialization phase. This threshold is expressed in meters per squared second (m/s^2).
      Parameters:
      baseNoiseLevelAbsoluteThreshold - overall absolute threshold to determine whether there has been excessive motion.
      Throws:
      com.irurueta.navigation.LockedException - if optimizer is already running.
      IllegalArgumentException - if provided value is zero or negative.

    • getBaseNoiseLevelAbsoluteThresholdAsMeasurement

      public com.irurueta.units.Acceleration getBaseNoiseLevelAbsoluteThresholdAsMeasurement()
      Gets the overall absolute threshold to determine whether there has been excessive motion during the whole initialization phase.
      Returns:
      overall absolute threshold to determine whether there has been excessive motion.

    • getBaseNoiseLevelAbsoluteThresholdAsMeasurement

      public void getBaseNoiseLevelAbsoluteThresholdAsMeasurement(com.irurueta.units.Acceleration result)
      Gets the overall absolute threshold to determine whether there has been excessive motion during the whole initialization phase.
      Parameters:
      result - instance where the result will be stored.

    • setBaseNoiseLevelAbsoluteThreshold

      public void setBaseNoiseLevelAbsoluteThreshold(com.irurueta.units.Acceleration baseNoiseLevelAbsoluteThreshold) throws com.irurueta.navigation.LockedException
      Sets the overall absolute threshold to determine whether there has been excessive motion during the whole initialization phase.
      Parameters:
      baseNoiseLevelAbsoluteThreshold - overall absolute threshold to determine whether there has been excessive motion.
      Throws:
      com.irurueta.navigation.LockedException - if optimizer is already running.
      IllegalArgumentException - if provided value is zero or negative.

    • getAccelerometerBaseNoiseLevel

      public double getAccelerometerBaseNoiseLevel()
      Gets accelerometer base noise level that has been detected during initialization of the best solution that has been found expressed in meters per squared second (m/s^2). This is equal to the standard deviation of the accelerometer measurements during the initialization phase.
      Returns:
      accelerometer base noise level of the best solution that has been found.

    • getAccelerometerBaseNoiseLevelAsMeasurement

      public com.irurueta.units.Acceleration getAccelerometerBaseNoiseLevelAsMeasurement()
      Gets accelerometer base noise level that has been detected during initialization of the best solution that has been found. This is equal to the standard deviation of the accelerometer measurements during the initialization phase.
      Returns:
      accelerometer base noise level of the best solution that has been found.

    • getAccelerometerBaseNoiseLevelAsMeasurement

      public void getAccelerometerBaseNoiseLevelAsMeasurement(com.irurueta.units.Acceleration result)
      Gets accelerometer base noise level that has been detected during initialization of the best solution that has been found. This is equal to the standard deviation of the accelerometer measurements during the initialization phase.
      Parameters:
      result - instance where the result will be stored.

    • getAccelerometerBaseNoiseLevelPsd

      public double getAccelerometerBaseNoiseLevelPsd()
      Gets accelerometer base noise level PSD (Power Spectral Density) expressed in (m^2 * s^-3) of the best solution that has been found.
      Returns:
      accelerometer base noise level PSD of the best solution that has been found.

    • getAccelerometerBaseNoiseLevelRootPsd

      public double getAccelerometerBaseNoiseLevelRootPsd()
      Gets accelerometer base noise level root PSD (Power Spectral Density) expressed in (m * s^-1.5) of the best solution that has been found.
      Specified by:
      getAccelerometerBaseNoiseLevelRootPsd in interface com.irurueta.navigation.inertial.calibration.AccelerometerNoiseRootPsdSource
      Returns:
      accelerometer base noise level root PSD of the best solution that has been found.

    • getThreshold

      public double getThreshold()
      Gets the threshold to determine static/dynamic period changes expressed in meters per squared second (m/s^2) for the best calibration solution that has been found.
      Returns:
      threshold to determine static/dynamic period changes for the best solution.

    • getThresholdAsMeasurement

      public com.irurueta.units.Acceleration getThresholdAsMeasurement()
      Gets the threshold to determine static/dynamic period changes for the best calibration solution that has been found.
      Returns:
      threshold to determine static/dynamic period changes for the best solution.

    • getThresholdAsMeasurement

      public void getThresholdAsMeasurement(com.irurueta.units.Acceleration result)
      Get the threshold to determine static/dynamic period changes for the best calibration solution that has been found.
      Parameters:
      result - instance where the result will be stored.

    • getEstimatedHardIron

      public double[] getEstimatedHardIron()
      Gets the array containing x,y,z components of estimated magnetometer hard-iron biases expressed in Teslas (T).
      Returns:
      array containing x,y,z components of estimated magnetometer hard-iron biases.

    • getEstimatedMm

      public com.irurueta.algebra.Matrix getEstimatedMm()
      Gets estimated magnetometer soft-iron matrix containing scale factors and cross-coupling errors. This is the product of matrix Tm containing cross-coupling errors and Km containing scaling factors. So that: 
           Mm = [sx    mxy  mxz] = Tm*Km
          [myx   sy   myz]
          [mzx   mzy  sz ]
      Where: 
           Km = [sx 0   0 ]
          [0  sy  0 ]
          [0  0   sz]
      and 
           Tm = [1          -alphaXy    alphaXz ]
          [alphaYx    1           -alphaYz]
          [-alphaZx   alphaZy     1       ]
      Hence: 
           Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
      This instance allows any 3x3 matrix. However, typically alphaYx, alphaZx and alphaZy are considered to be zero if the accelerometer z-axis is assumed to be the same as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix becomes upper diagonal: 
           Mm = [sx    mxy  mxz]
          [0     sy   myz]
          [0     0    sz ]
      Values of this matrix are unit-less.
      Returns:
      estimated magnetometer soft-iron scale factors and cross-coupling errors, or null if not available.

    • evaluateForThresholdFactor

      protected double evaluateForThresholdFactor(double thresholdFactor) throws com.irurueta.navigation.LockedException, com.irurueta.navigation.inertial.calibration.CalibrationException, com.irurueta.navigation.NotReadyException, IntervalDetectorThresholdFactorOptimizerException
      Evaluates calibration Mean Square Error (MSE) for the provided threshold factor.
      Parameters:
      thresholdFactor - threshold factor to be used for interval detection and measurement generation to be used for calibration.
      Returns:
      calibration MSE.
      Throws:
      com.irurueta.navigation.LockedException - if the generator is busy.
      com.irurueta.navigation.inertial.calibration.CalibrationException - if calibration fails.
      com.irurueta.navigation.NotReadyException - if the calibrator is not ready.
      IntervalDetectorThresholdFactorOptimizerException - interval detection failed.

    • initialize

      private void initialize()
      Initializes magnetometer measurement generator to convert body kinematics and magnetic flux measurements after interval detection into measurements used for magnetometer calibration.

    • keepBestResult

      private void keepBestResult(double mse, double thresholdFactor)
      Keeps the best calibration solution found so far.
      Parameters:
      mse - Estimated Mean Square Error during calibration.
      thresholdFactor - threshold factor to be kept.

    • createMeasurement

      private com.irurueta.units.Acceleration createMeasurement(double value, com.irurueta.units.AccelerationUnit unit)
      Creates an acceleration instance using the provided value and unit.
      Parameters:
      value - value of measurement.
      unit - unit of value.
      Returns:
      created acceleration.

    • getDefaultUnit

      private com.irurueta.units.AccelerationUnit getDefaultUnit()
      Gets the default unit for acceleration, which is meters per squared second (m/s^2).
      Returns:
      default unit for acceleration.