/*
    * Copyright (C) 2016 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.ar.slam;
  
  import com.irurueta.algebra.ArrayUtils;
  import com.irurueta.algebra.Matrix;
  import com.irurueta.algebra.WrongSizeException;
  import com.irurueta.numerical.signal.processing.MeasurementNoiseCovarianceEstimator;
  import com.irurueta.numerical.signal.processing.SignalProcessingException;
  import com.irurueta.statistics.InvalidCovarianceMatrixException;
  import com.irurueta.statistics.MultivariateNormalDist;
  
  import java.util.Arrays;
  
  /**
   * Base class for estimating mean and covariance of noise in control values
   * when the system state is held constant (only noise is provided as control
   * input).
   *
   * @param <D> type of calibration data.
   */
  @SuppressWarnings("DuplicatedCode")
  public abstract class BaseSlamCalibrator<D extends BaseCalibrationData> {
  
      /**
       * Minimum allowed sample length.
       */
      public static final int MIN_SAMPLE_LENGTH = 1;
  
      /**
       * Default minimum number of samples to take into account.
       */
      public static final int DEFAULT_MIN_NUM_SAMPLES = 20;
  
      /**
       * Default maximum number of samples to take into account.
       */
      public static final int DEFAULT_MAX_NUM_SAMPLES = 100;
  
      /**
       * Value to consider that mean and covariance have converged.
       */
      public static final double DEFAULT_CONVERGENCE_THRESHOLD = 1e-5;
  
      /**
       * Indicates whether sample accumulation must be enabled or not.
       */
      protected static final boolean DEFAULT_ENABLE_SAMPLE_ACCUMULATION = true;
  
      /**
       * Number of components in 3D.
       */
      protected static final int N_COMPONENTS_3D = 3;
  
      /**
       * Conversion of nanoseconds to milliseconds.
       */
      protected static final double NANOS_TO_SECONDS = 1e-9;
  
      /**
       * Sample length of control values used during prediction stage in SLAM
       * estimator.
       */
      private final int sampleLength;
  
      /**
       * Array containing a control sample used during SLAM prediction stage.
       */
      protected double[] sample;
  
      /**
       * Mean and covariance estimator.
       */
      protected MeasurementNoiseCovarianceEstimator estimator;
  
      /**
       * Contains previous mean value.
       */
      protected double[] previousMean;
  
      /**
       * Contains mean value of covariance.
       */
      protected Matrix previousCovariance;
  
      /**
      * Indicates whether this calibrator converged.
      */
     protected boolean converged;
 
     /**
      * Indicates whether this calibrator failed.
      */
     protected boolean failed;
 
     /**
      * Indicates whether calibrator has finished taking samples.
      */
     protected boolean finished;
 
     /**
      * Number of obtained samples.
      */
     protected int sampleCount;
 
     /**
      * Array to store the difference between average values to determine whether
      * the result has converged or not.
      */
     protected double[] meanDiff;
 
     /**
      * Matrix to store the difference between covariance matrices to determine
      * whether the result has converged or not.
      */
     protected Matrix covDiff;
 
     /**
      * Minimum number of samples to take into account.
      */
     protected int minNumSamples = DEFAULT_MIN_NUM_SAMPLES;
 
     /**
      * Maximum number of samples to take into account.
      */
     protected int maxNumSamples = DEFAULT_MAX_NUM_SAMPLES;
 
     /**
      * Threshold to consider whether calibration has converged or not.
      */
     protected double convergenceThreshold = DEFAULT_CONVERGENCE_THRESHOLD;
 
     /**
      * Indicates whether accumulation of samples is enabled or not.
      */
     protected boolean accumulationEnabled = DEFAULT_ENABLE_SAMPLE_ACCUMULATION;
 
     /**
      * Timestamp expressed in nanoseconds since the epoch time of the last
      * accelerometer sample.
      */
     protected long accelerometerTimestampNanos = -1;
 
     /**
      * Timestamp expressed in nanoseconds since the epoch time of the last
      * gyroscope sample.
      */
     protected long gyroscopeTimestampNanos = -1;
 
     /**
      * Number of accelerometer samples accumulated since last full sample.
      */
     protected int accumulatedAccelerometerSamples = 0;
 
     /**
      * Number of gyroscope samples accumulated since last full sample.
      */
     protected int accumulatedGyroscopeSamples = 0;
 
     /**
      * Average of acceleration along x-axis accumulated since last full sample.
      * Expressed in meters per squared second (m/s^2).
      */
     protected double accumulatedAccelerationSampleX;
 
     /**
      * Average of acceleration along y-axis accumulated since last full sample.
      * Expressed in meters per squared second (m/s^2).
      */
     protected double accumulatedAccelerationSampleY;
 
     /**
      * Average of acceleration along z-axis accumulated since last full sample.
      * Expressed in meters per squared second (m/s^2).
      */
     protected double accumulatedAccelerationSampleZ;
 
     /**
      * Average of angular speed along x-axis accumulated since last full sample.
      * Expressed in meters per squared second (m/s^2).
      */
     protected double accumulatedAngularSpeedSampleX;
 
     /**
      * Average of angular speed along y-axis accumulated since last full sample.
      * Expressed in meters per squared second (m/s^2).
      */
     protected double accumulatedAngularSpeedSampleY;
 
     /**
      * Average of angular speed along z-axis accumulated since last full sample.
      * Expressed in meters per squared second (m/s^2).
      */
     protected double accumulatedAngularSpeedSampleZ;
 
     /**
      * Listener in charge of handling events raised by instances of this class.
      */
     protected BaseSlamCalibratorListener<D> listener;
 
     /**
      * Constructor.
      *
      * @param sampleLength sample length of control values used during
      *                     prediction stage in SLAM estimator.
      * @throws IllegalArgumentException if sample length is less than 1.
      */
     protected BaseSlamCalibrator(final int sampleLength) {
         if (sampleLength < MIN_SAMPLE_LENGTH) {
             throw new IllegalArgumentException("length must be greater than 0");
         }
 
         this.sampleLength = sampleLength;
         sample = new double[sampleLength];
         previousMean = new double[sampleLength];
         meanDiff = new double[sampleLength];
         try {
             previousCovariance = new Matrix(sampleLength, sampleLength);
             covDiff = new Matrix(sampleLength, sampleLength);
             estimator = new MeasurementNoiseCovarianceEstimator(sampleLength);
         } catch (final Exception ignore) {
             // never thrown
         }
     }
 
     /**
      * Gets sample length of control values used during prediction stage in SLAM
      * estimator.
      *
      * @return sample length of control values used during prediction stage in
      * SLAM estimator.
      */
     public int getSampleLength() {
         return sampleLength;
     }
 
     /**
      * Indicates whether calibrator converged or not.
      *
      * @return true if calibrator converged, false otherwise.
      */
     public boolean isConverged() {
         return converged;
     }
 
     /**
      * Indicates whether this calibrator failed or not.
      *
      * @return true if calibrator failed, false otherwise.
      */
     public boolean isFailed() {
         return failed;
     }
 
     /**
      * Indicates whether calibrator has finished taking samples or not.
      *
      * @return true if calibrator has finished taking samples, false otherwise.
      */
     public boolean isFinished() {
         return finished;
     }
 
     /**
      * Gets number of obtained samples.
      *
      * @return number of obtained samples.
      */
     public int getSampleCount() {
         return sampleCount;
     }
 
     /**
      * Obtains the minimum number of samples to use before taking convergence
      * into account.
      *
      * @return minimum number of samples to use before taking convergence into
      * account.
      */
     public int getMinNumSamples() {
         return minNumSamples;
     }
 
     /**
      * Specifies the minimum number of samples to take before taking convergence
      * into account.
      *
      * @param minNumSamples minimum number of samples to take before taking
      *                      convergence into account.
      * @throws IllegalArgumentException if provided value is negative.
      */
     public void setMinNumSamples(final int minNumSamples) {
         if (minNumSamples < 0) {
             throw new IllegalArgumentException("minNumSamples must be positive");
         }
         this.minNumSamples = minNumSamples;
     }
 
     /**
      * Gets maximum number of samples to take into account.
      *
      * @return maximum number of samples to take into account.
      */
     public int getMaxNumSamples() {
         return maxNumSamples;
     }
 
     /**
      * Specifies the maximum number of samples to take.
      *
      * @param maxNumSamples maximum number of samples to take.
      * @throws IllegalArgumentException if provided value is negative or zero.
      */
     public void setMaxNumSamples(final int maxNumSamples) {
         if (maxNumSamples <= 0) {
             throw new IllegalArgumentException("maxNumSamples must be positive");
         }
         this.maxNumSamples = maxNumSamples;
     }
 
     /**
      * Gets threshold to consider that calibration has converged.
      *
      * @return threshold to consider that calibration has converged.
      */
     public double getConvergenceThreshold() {
         return convergenceThreshold;
     }
 
     /**
      * Specifies threshold to determine that calibration has converged.
      *
      * @param convergenceThreshold threshold to determine that calibration has
      *                             converged.
      * @throws IllegalArgumentException if threshold is negative.
      */
     public void setConvergenceThreshold(final double convergenceThreshold) {
         if (convergenceThreshold < 0.0) {
             throw new IllegalArgumentException("convergenceThreshold must be positive");
         }
         this.convergenceThreshold = convergenceThreshold;
     }
 
     /**
      * Resets calibrator.
      */
     public void reset() {
         converged = failed = false;
         sampleCount = 0;
         finished = false;
         Arrays.fill(sample, 0.0);
         Arrays.fill(previousMean, 0.0);
         previousCovariance.initialize(0.0);
         try {
             estimator = new MeasurementNoiseCovarianceEstimator(sample.length);
         } catch (final SignalProcessingException e) {
             // never thrown
         }
         sampleCount = 0;
         Arrays.fill(meanDiff, 0.0);
         covDiff.initialize(0.0);
         accelerometerTimestampNanos = gyroscopeTimestampNanos = -1;
         accumulatedAccelerometerSamples = accumulatedGyroscopeSamples = 0;
         accumulatedAccelerationSampleX = accumulatedAccelerationSampleY = accumulatedAccelerationSampleZ = 0.0;
         accumulatedAngularSpeedSampleX = accumulatedAngularSpeedSampleY = accumulatedAngularSpeedSampleZ = 0.0;
     }
 
     /**
      * Indicates whether accumulation of samples is enabled or not.
      *
      * @return true if accumulation of samples is enabled, false otherwise.
      */
     public boolean isAccumulationEnabled() {
         return accumulationEnabled;
     }
 
     /**
      * Specifies whether accumulation of samples is enabled or not.
      *
      * @param accumulationEnabled true if accumulation of samples is enabled,
      *                            false otherwise.
      */
     public void setAccumulationEnabled(final boolean accumulationEnabled) {
         this.accumulationEnabled = accumulationEnabled;
     }
 
     /**
      * Gets timestamp expressed in nanoseconds since the epoch time of the last
      * accelerometer sample, or -1 if no sample has been set yet.
      *
      * @return timestamp expressed in nanoseconds since the epoch time of the
      * last accelerometer sample, or -1.
      */
     public long getAccelerometerTimestampNanos() {
         return accelerometerTimestampNanos;
     }
 
     /**
      * Gets timestamp expressed in nanoseconds since the epoch time of the last
      * gyroscope sample, or -1 if no sample has been set yet.
      *
      * @return timestamp expressed in nanoseconds since the epoch time of the
      * last gyroscope sample, or -1.
      */
     public long getGyroscopeTimestampNanos() {
         return gyroscopeTimestampNanos;
     }
 
     /**
      * Gets number of accelerometer samples accumulated since last full sample.
      *
      * @return number of accelerometer samples accumulated since last full
      * sample.
      */
     public int getAccumulatedAccelerometerSamples() {
         return accumulatedAccelerometerSamples;
     }
 
     /**
      * Gets number of gyroscope samples accumulated since last full sample.
      *
      * @return number of gyroscope samples accumulated since last full sample.
      */
     public int getAccumulatedGyroscopeSamples() {
         return accumulatedGyroscopeSamples;
     }
 
     /**
      * Indicates whether the accelerometer sample has been received since the
      * last full sample (accelerometer + gyroscope).
      *
      * @return true if accelerometer sample has been received, false otherwise.
      */
     public boolean isAccelerometerSampleReceived() {
         return accumulatedAccelerometerSamples > 0;
     }
 
     /**
      * Indicates whether the gyroscope sample has been received since the last
      * full sample (accelerometer + gyroscope).
      *
      * @return true if gyroscope sample has been received, false otherwise.
      */
     public boolean isGyroscopeSampleReceived() {
         return accumulatedGyroscopeSamples > 0;
     }
 
     /**
      * Indicates whether a full sample (accelerometer + gyroscope) has been
      * received or not.
      *
      * @return true if full sample has been received, false otherwise.
      */
     public boolean isFullSampleAvailable() {
         return isAccelerometerSampleReceived() && isGyroscopeSampleReceived();
     }
 
     /**
      * Gets average acceleration along x-axis accumulated since last full
      * sample. Expressed in meters per squared second (m/s^2).
      *
      * @return average acceleration along x-axis accumulated since last full
      * sample.
      */
     public double getAccumulatedAccelerationSampleX() {
         return accumulatedAccelerationSampleX;
     }
 
     /**
      * Gets average acceleration along y-axis accumulated since last full
      * sample. Expressed in meters per squared second (m/s^2).
      *
      * @return average acceleration along y-axis accumulated since last full
      * sample.
      */
     public double getAccumulatedAccelerationSampleY() {
         return accumulatedAccelerationSampleY;
     }
 
     /**
      * Gets average acceleration along z-axis accumulated since last full
      * sample. Expressed in meters per squared second (m/s^2).
      *
      * @return average acceleration along z-axis accumulated since last full
      * sample.
      */
     public double getAccumulatedAccelerationSampleZ() {
         return accumulatedAccelerationSampleZ;
     }
 
     /**
      * Gets average acceleration along x,y,z axes accumulated since last full
      * sample. Expressed in meters per squared second (m/s^2).
      *
      * @return average acceleration along x,y,z axes expressed in meters per
      * squared second (m/s^2).
      */
     public double[] getAccumulatedAccelerationSample() {
         return new double[]{
                 accumulatedAccelerationSampleX,
                 accumulatedAccelerationSampleY,
                 accumulatedAccelerationSampleZ
         };
     }
 
     /**
      * Gets average acceleration along x,yz axes accumulated since last full
      * sample. Expressed in meters per squared second (m/s^2).
      *
      * @param result array where average acceleration along x,y,z axes will be
      *               stored.
      * @throws IllegalArgumentException if provided array does not have length
      *                                  3.
      */
     public void getAccumulatedAccelerationSample(final double[] result) {
         if (result.length != N_COMPONENTS_3D) {
             throw new IllegalArgumentException("result must have length 3");
         }
         result[0] = accumulatedAccelerationSampleX;
         result[1] = accumulatedAccelerationSampleY;
         result[2] = accumulatedAccelerationSampleZ;
     }
 
     /**
      * Gets average angular speed along x-axis accumulated since last full
      * sample. Expressed in radians per second (rad/s).
      *
      * @return average angular speed along x-axis expressed in radians per
      * second (rad/s).
      */
     public double getAccumulatedAngularSpeedSampleX() {
         return accumulatedAngularSpeedSampleX;
     }
 
     /**
      * Gets average angular speed along y-axis accumulated since last full
      * sample. Expressed in radians per second (rad/s).
      *
      * @return average angular speed along y-axis expressed in radians per
      * second (rad/s).
      */
     public double getAccumulatedAngularSpeedSampleY() {
         return accumulatedAngularSpeedSampleY;
     }
 
     /**
      * Gets average angular speed along z-axis accumulated since last full
      * sample. Expressed in radians per second (rad/s).
      *
      * @return average angular speed along z-axis expressed in radians per
      * second (rad/s).
      */
     public double getAccumulatedAngularSpeedSampleZ() {
         return accumulatedAngularSpeedSampleZ;
     }
 
     /**
      * Gets average angular speed along x,y,z axes accumulated since last full
      * sample. Expressed in radians per second (rad/s).
      *
      * @return average angular speed along x,y,z axes expressed in radians per
      * second.
      */
     public double[] getAccumulatedAngularSpeedSample() {
         return new double[]{
                 accumulatedAngularSpeedSampleX,
                 accumulatedAngularSpeedSampleY,
                 accumulatedAngularSpeedSampleZ
         };
     }
 
     /**
      * Gets average angular speed along x,y,z axes accumulated since last full
      * sample. Expressed in radians per second (rad/s).
      *
      * @param result array where average angular speed along x,y,z axes will be
      *               stored.
      * @throws IllegalArgumentException if provided array does not have length
      *                                  3.
      */
     public void getAccumulatedAngularSpeedSample(final double[] result) {
         if (result.length != N_COMPONENTS_3D) {
             throw new IllegalArgumentException("result must have length 3");
         }
         result[0] = accumulatedAngularSpeedSampleX;
         result[1] = accumulatedAngularSpeedSampleY;
         result[2] = accumulatedAngularSpeedSampleZ;
     }
 
     /**
      * Provides a new accelerometer sample.
      * If accumulation is enabled, samples are averaged until a full sample is
      * received.
      * When a full sample (accelerometer + gyroscope) is received, internal
      * state gets also updated.
      *
      * @param timestamp     timestamp of accelerometer sample since epoch time and
      * @param accelerationX linear acceleration along x-axis expressed in meters
      *                      per squared second (m/s^2).
      * @param accelerationY linear acceleration along y-axis expressed in meters
      *                      per squared second (m/s^2).
      * @param accelerationZ linear acceleration along z-axis expressed in meters
      *                      per squared second (m/s^2).
      */
     public void updateAccelerometerSample(
             final long timestamp, final float accelerationX, final float accelerationY, final float accelerationZ) {
         if (!isFullSampleAvailable()) {
             accelerometerTimestampNanos = timestamp;
             if (isAccumulationEnabled() && isAccelerometerSampleReceived()) {
                 // accumulation enabled
                 final var nextSamples = accumulatedAccelerometerSamples + 1;
                 accumulatedAccelerationSampleX = (accumulatedAccelerationSampleX * accumulatedAccelerometerSamples
                         + accelerationX) / nextSamples;
                 accumulatedAccelerationSampleY = (accumulatedAccelerationSampleY * accumulatedAccelerometerSamples
                         + accelerationY) / nextSamples;
                 accumulatedAccelerationSampleZ = (accumulatedAccelerationSampleZ * accumulatedAccelerometerSamples
                         + accelerationZ) / nextSamples;
                 accumulatedAccelerometerSamples = nextSamples;
             } else {
                 // accumulation disabled
                 accumulatedAccelerationSampleX = accelerationX;
                 accumulatedAccelerationSampleY = accelerationY;
                 accumulatedAccelerationSampleZ = accelerationZ;
                 accumulatedAccelerometerSamples++;
             }
             notifyFullSampleAndResetSampleReceive();
         }
     }
 
     /**
      * Provides a new accelerometer sample.
      * If accumulation is enabled, samples are averaged until a full sample is
      * received.
      * When a full sample (accelerometer + gyroscope) is received, internal
      * state gets also updated.
      *
      * @param timestamp timestamp of accelerometer sample since epoch time and
      *                  expressed in nanoseconds.
      * @param data      array containing x,y,z components of linear acceleration
      *                  expressed in meters per squared second (m/s^2).
      * @throws IllegalArgumentException if provided array does not have length
      *                                  3.
      */
     public void updateAccelerometerSample(final long timestamp, final float[] data) {
         if (data.length != N_COMPONENTS_3D) {
             throw new IllegalArgumentException("acceleration must have length 3");
         }
         updateAccelerometerSample(timestamp, data[0], data[1], data[2]);
     }
 
     /**
      * Provides a new gyroscope sample.
      * If accumulation is enabled, samples are averaged until a full sample is
      * received.
      * When a full sample (accelerometer + gyroscope) is received, internal
      * state gets also updated.
      *
      * @param timestamp     timestamp of accelerometer sample since epoch time and
      *                      expressed in nanoseconds.
      * @param angularSpeedX angular speed of rotation along x-axis expressed in
      *                      radians per second (rad/s).
      * @param angularSpeedY angular speed of rotation along y-axis expressed in
      *                      radians per second (rad/s).
      * @param angularSpeedZ angular speed of rotation along z-axis expressed in
      *                      radians per second (rad/s).
      */
     public void updateGyroscopeSample(
             final long timestamp, final float angularSpeedX, final float angularSpeedY, final float angularSpeedZ) {
         if (!isFullSampleAvailable()) {
             gyroscopeTimestampNanos = timestamp;
             if (isAccumulationEnabled() && isGyroscopeSampleReceived()) {
                 // accumulation enabled
                 final var nextSamples = accumulatedGyroscopeSamples + 1;
                 accumulatedAngularSpeedSampleX = (accumulatedAngularSpeedSampleX * accumulatedGyroscopeSamples
                         + angularSpeedX) / nextSamples;
                 accumulatedAngularSpeedSampleY = (accumulatedAngularSpeedSampleY * accumulatedGyroscopeSamples
                         + angularSpeedY) / nextSamples;
                 accumulatedAngularSpeedSampleZ = (accumulatedAngularSpeedSampleZ * accumulatedGyroscopeSamples
                         + angularSpeedZ) / nextSamples;
                 accumulatedGyroscopeSamples = nextSamples;
             } else {
                 // accumulation disabled
                 accumulatedAngularSpeedSampleX = angularSpeedX;
                 accumulatedAngularSpeedSampleY = angularSpeedY;
                 accumulatedAngularSpeedSampleZ = angularSpeedZ;
                 accumulatedGyroscopeSamples++;
             }
             notifyFullSampleAndResetSampleReceive();
         }
     }
 
     /**
      * Provides a new gyroscope sample.
      * If accumulation is enabled, samples are averaged until a full sample is
      * received.
      * When a full sample (accelerometer + gyroscope) is received, internal
      * state gets also updated.
      *
      * @param timestamp timestamp of gyroscope sample since epoch time and
      *                  expressed in nanoseconds.
      * @param data      angular speed of rotation along x,y,z axes expressed in
      *                  radians per second (rad/s).
      * @throws IllegalArgumentException if provided array does not have length
      *                                  3.
      */
     public void updateGyroscopeSample(final long timestamp, final float[] data) {
         if (data.length != N_COMPONENTS_3D) {
             throw new IllegalArgumentException("angular speed must have length 3");
         }
         updateGyroscopeSample(timestamp, data[0], data[1], data[2]);
     }
 
     /**
      * Gets most recent timestamp of received partial samples (accelerometer or
      * gyroscope).
      *
      * @return most recent timestamp of received partial sample.
      */
     public long getMostRecentTimestampNanos() {
         return Math.max(accelerometerTimestampNanos, gyroscopeTimestampNanos);
     }
 
     /**
      * Gets listener in charge of handling events raised by instances of this
      * class.
      *
      * @return listener in charge of handling events raised by instances of this
      * class.
      */
     public BaseSlamCalibratorListener<D> getListener() {
         return listener;
     }
 
     /**
      * Sets listener in charge of handling events raised by instances of this
      * class.
      *
      * @param listener listener in charge of handling events raised by instances
      *                 of this class.
      */
     public void setListener(final BaseSlamCalibratorListener<D> listener) {
         this.listener = listener;
     }
 
     /**
      * Obtains mean values of control signal used for SLAM estimation during
      * prediction stage of Kalman filter in order to correct possible biases
      * and offsets.
      *
      * @return mean values of control signal.
      */
     public double[] getControlMean() {
         return estimator.getSampleAverage();
     }
 
     /**
      * Obtains mean values of control signal used for SLAM estimation during
      * prediction stage of Kalman filter in order to correct possible biases
      * and offsets.
      *
      * @param result array where mean values of control signal will be stored.
      *               Array must have the same length as the control signal.
      * @throws IllegalArgumentException if provided length is invalid.
      */
     public void getControlMean(final double[] result) {
         final var src = getControlMean();
         if (result.length != src.length) {
             throw new IllegalArgumentException("wrong length");
         }
 
         System.arraycopy(src, 0, result, 0, src.length);
     }
 
     /**
      * Gets covariance of control signal used for SLAM estimation during
      * prediction stage of Kalman filter in order to correct possible biases
      * and offsets.
      *
      * @return covariance matrix of control signal.
      */
     public Matrix getControlCovariance() {
         return estimator.getMeasurementNoiseCov();
     }
 
     /**
      * Gets covariance of control signal used for SLAM estimation during
      * prediction stage of Kalman filter in order to correct possible biases
      * and offsets.
      *
      * @param result matrix where covariance will be stored.
      */
     public void getControlCovariance(final Matrix result) {
         final var src = getControlCovariance();
         src.copyTo(result);
     }
 
     /**
      * Gets a multivariate normal distribution containing control signal mean
      * and covariance used for SLAM estimation during prediction stage of Kalman
      * filter in order to correct possible biases and offsets.
      *
      * @return a multivariate normal distribution.
      * @throws InvalidCovarianceMatrixException if estimated covariance is not
      *                                          valid.
      */
     public MultivariateNormalDist getControlDistribution() throws InvalidCovarianceMatrixException {
         final var cov = getControlCovariance();
         try {
             cov.symmetrize();
         } catch (final WrongSizeException ignore) {
             // never thrown
         }
         return new MultivariateNormalDist(getControlMean(), cov, false);
     }
 
     /**
      * Gets a multivariate normal distribution containing control signal mean
      * and covariance used for SLAM estimation during prediction stage of Kalman
      * filter in order to correct possible biases and offsets.
      *
      * @param dist a multivariate normal distribution.
      * @throws InvalidCovarianceMatrixException if estimated covariance is not
      *                                          valid.
      */
     public void getControlDistribution(final MultivariateNormalDist dist) throws InvalidCovarianceMatrixException {
         final var cov = getControlCovariance();
         try {
             cov.symmetrize();
         } catch (final WrongSizeException ignore) {
             // never thrown
         }
         dist.setMeanAndCovariance(getControlMean(), cov, false);
     }
 
     /**
      * Gets a new instance containing calibration data estimated by this
      * calibrator.
      *
      * @return a new calibration data instance.
      */
     public abstract D getCalibrationData();
 
     /**
      * Gets calibration data estimated by this calibrator.
      *
      * @param result instance where calibration data will be stored.
      */
     public void getCalibrationData(final D result) {
         result.setControlMeanAndCovariance(getControlMean(), getControlCovariance());
     }
 
     /**
      * Propagates calibrated control signal covariance using current control
      * jacobian matrix.
      * The propagated distribution can be used during prediction stage in Kalman
      * filtering.
      *
      * @param controlJacobian current control jacobian matrix.
      * @return propagated distribution.
      * @throws InvalidCovarianceMatrixException if estimated covariance is not
      *                                          valid.
      */
     public MultivariateNormalDist propagateWithControlJacobian(final Matrix controlJacobian)
             throws InvalidCovarianceMatrixException {
         return getCalibrationData().propagateWithControlJacobian(controlJacobian);
     }
 
     /**
      * Propagates calibrated control signal covariance using current control
      * jacobian matrix.
      * The propagated distribution can be used during prediction stage in Kalman
      * filtering.
      *
      * @param controlJacobian current control jacobian matrix.
      * @param result          instance where propagated distribution is stored.
      * @throws InvalidCovarianceMatrixException if estimated covariance is not
      *                                          valid.
      */
     public void propagateWithControlJacobian(final Matrix controlJacobian, final MultivariateNormalDist result)
             throws InvalidCovarianceMatrixException {
         getCalibrationData().propagateWithControlJacobian(controlJacobian, result);
     }
 
     /**
      * Notifies that a full sample has been received and resets flags indicating
      * whether partial samples have been received.
      */
     protected void notifyFullSampleAndResetSampleReceive() {
         if (isFullSampleAvailable()) {
             processFullSample();
             accumulatedAccelerometerSamples = accumulatedGyroscopeSamples = 0;
         }
     }
 
     /**
      * Obtains the number of state parameters in associated SLAM estimator.
      *
      * @return number of state parameters.
      */
     protected abstract int getEstimatorStateLength();
 
     /**
      * Method to be implemented in subclasses to process a full sample.
      */
     protected abstract void processFullSample();
 
     /**
      * Updates internal mean and covariance values and checks whether
      * convergence has been reached and calibrator has finished or failed.
      */
     protected void updateSample() {
         if (finished) {
             return;
         }
 
         try {
             estimator.update(sample);
         } catch (final SignalProcessingException e) {
             failed = finished = true;
 
             if (listener != null) {
                 listener.onCalibratorFinished(this, converged, true);
             }
             return;
         }
 
         sampleCount++;
 
         final var mean = estimator.getSampleAverage();
         final var cov = estimator.getMeasurementNoiseCov();
 
         // check if minimum number of samples has been reached
         if (sampleCount >= maxNumSamples) {
             finished = true;
 
             if (listener != null) {
                 listener.onCalibratorFinished(this, converged, failed);
             }
             return;
         }
 
         // check if estimator has converged
         if (sampleCount >= minNumSamples) {
             ArrayUtils.subtract(mean, previousMean, meanDiff);
             try {
                 cov.subtract(previousCovariance, covDiff);
             } catch (final WrongSizeException ignore) {
                 // never thrown
             }
             final var meanDiffNorm = com.irurueta.algebra.Utils.normF(meanDiff);
             final var covDiffNorm = com.irurueta.algebra.Utils.normF(covDiff);
             if (meanDiffNorm <= convergenceThreshold && covDiffNorm <= convergenceThreshold) {
                 converged = finished = true;
 
                 if (listener != null) {
                     listener.onCalibratorFinished(this, true, failed);
                 }
                 return;
             }
         }
 
         // copy current value for next iteration
         System.arraycopy(mean, 0, previousMean, 0, mean.length);
         cov.copyTo(previousCovariance);
 
         finished = false;
     }
 
     /**
      * Listener for implementations of this class.
      */
     public interface BaseSlamCalibratorListener<D extends BaseCalibrationData> {
         /**
          * Called when a full sample (accelerometer + gyroscope, etc.) has been
          * received.
          *
          * @param calibrator SLAM calibrator.
          */
         void onFullSampleReceived(final BaseSlamCalibrator<D> calibrator);
 
         /**
          * Called when a full sample (accelerometer + gyroscope, etc.) has been
          * received and has already been processed.
          *
          * @param calibrator SLAM calibrator.
          */
         void onFullSampleProcessed(final BaseSlamCalibrator<D> calibrator);
 
         /**
          * Called when calibration finishes.
          *
          * @param calibrator SLAM calibrator.
          * @param converged  true if calibration converged, false otherwise.
          * @param failed     true if calibration failed, false otherwise.
          */
         void onCalibratorFinished(
                 final BaseSlamCalibrator<D> calibrator, final boolean converged, final boolean failed);
     }
 }