/*
    * 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.noise;
  
  import com.irurueta.navigation.LockedException;
  import com.irurueta.navigation.inertial.BodyKinematics;
  import com.irurueta.navigation.inertial.calibration.AccelerationTriad;
  import com.irurueta.navigation.inertial.calibration.AccelerometerNoiseRootPsdSource;
  import com.irurueta.navigation.inertial.calibration.AngularSpeedTriad;
  import com.irurueta.navigation.inertial.calibration.GyroscopeNoiseRootPsdSource;
  import com.irurueta.navigation.inertial.calibration.TimeIntervalEstimator;
  import com.irurueta.units.Acceleration;
  import com.irurueta.units.AccelerationUnit;
  import com.irurueta.units.AngularSpeed;
  import com.irurueta.units.AngularSpeedUnit;
  import com.irurueta.units.Time;
  import com.irurueta.units.TimeConverter;
  import com.irurueta.units.TimeUnit;
  
  import java.util.LinkedList;
  
  /**
   * Estimates accelerometer and angular speed noise variances and PSD's
   * (Power Spectral Densities) along with their average values for a windowed
   * amount of samples.
   * This estimator must be used when the body where the accelerometer and
   * gyroscope are attached remains static on the same position with zero
   * velocity and constant (or zero) angular speed while capturing data.
   * To compute PSD's, this estimator assumes that measurement samples are obtained
   * at a constant provided rate equal to {@link #getTimeInterval()} seconds.
   * If not available, sampling rate average can be estimated using
   * {@link TimeIntervalEstimator}.
   * This estimator does NOT require the knowledge of current location and body
   * orientation.
   * Because body location and orientation is not known, estimated average values
   * cannot be used to determine biases. Only norm of noise estimations
   * (variance or standard deviation) can be safely used.
   */
  @SuppressWarnings("DuplicatedCode")
  public class WindowedBodyKinematicsNoiseEstimator implements AccelerometerNoiseRootPsdSource,
          GyroscopeNoiseRootPsdSource {
      /**
       * Number of samples to keep within the window by default.
       * For an accelerometer generating 100 samples/second, this is equivalent to
       * 1 second.
       * For an accelerometer generating 50 samples/second, this is equivalent to
       * 2 seconds.
       */
      public static final int DEFAULT_WINDOW_SIZE = WindowedTriadNoiseEstimator.DEFAULT_WINDOW_SIZE;
  
      /**
       * Minimum allowed window size.
       */
      public static final int MIN_WINDOW_SIZE = WindowedTriadNoiseEstimator.MIN_WINDOW_SIZE;
  
      /**
       * Default time interval between accelerometer samples expressed in seconds
       * (s).
       */
      public static final double DEFAULT_TIME_INTERVAL_SECONDS =
              WindowedTriadNoiseEstimator.DEFAULT_TIME_INTERVAL_SECONDS;
  
      /**
       * Length of number of samples to keep within the window being processed.
       * Window size must always be larger than allowed minimum value.
       */
      private int windowSize = DEFAULT_WINDOW_SIZE;
  
      /**
       * Time interval expressed in seconds (s) between consecutive accelerometer
       * samples.
       */
      private double timeInterval = DEFAULT_TIME_INTERVAL_SECONDS;
  
      /**
       * Keeps the list of body kinematics samples that remain within the window.
       */
      private final LinkedList<BodyKinematics> windowedSamples = new LinkedList<>();
  
      /**
       * Listener to handle events raised by this estimator.
       */
      private WindowedBodyKinematicsNoiseEstimatorListener listener;
  
      /**
       * Estimated average of x coordinate of specific force expressed in
      * meters per squared second (m/s^2).
      */
     private double avgSpecificForceX;
 
     /**
      * Estimated average of y coordinate of specific force expressed in
      * meters per squared second (m/s^2).
      */
     private double avgSpecificForceY;
 
     /**
      * Estimated average of z coordinate of specific force expressed in
      * meters per squared second (m/s^2).
      */
     private double avgSpecificForceZ;
 
     /**
      * Estimated average of x coordinate of angular rate expressed in
      * radians per second (rad/s).
      */
     private double avgAngularRateX;
 
     /**
      * Estimated average of y coordinate of angular rate expressed in
      * radians per second (rad/s).
      */
     private double avgAngularRateY;
 
     /**
      * Estimated average of z coordinate of angular rate expressed in
      * radians per second (rad/s).
      */
     private double avgAngularRateZ;
 
     /**
      * Estimated variance of x coordinate of specific force expressed
      * in (m^2/s^4).
      */
     private double varianceSpecificForceX;
 
     /**
      * Estimated variance of y coordinate of specific force expressed
      * in (m^2/s^4).
      */
     private double varianceSpecificForceY;
 
     /**
      * Estimated variance of z coordinate of specific force expressed
      * in (m^2/s^4).
      */
     private double varianceSpecificForceZ;
 
     /**
      * Estimated variance of x coordinate of angular rate expressed
      * in (rad^2/s^2).
      */
     private double varianceAngularRateX;
 
     /**
      * Estimated variance of y coordinate of angular rate expressed
      * in (rad^2/s^2).
      */
     private double varianceAngularRateY;
 
     /**
      * Estimated variance of z coordinate of angular rate expressed
      * in (rad^2/s^2).
      */
     private double varianceAngularRateZ;
 
     /**
      * Number of processed acceleration triad samples.
      */
     private int numberOfProcessedSamples;
 
     /**
      * Indicates whether estimator is running or not.
      */
     private boolean running;
 
     /**
      * Constructor.
      */
     public WindowedBodyKinematicsNoiseEstimator() {
     }
 
     /**
      * Constructor.
      *
      * @param listener listener to handle events raised by this estimator.
      */
     public WindowedBodyKinematicsNoiseEstimator(final WindowedBodyKinematicsNoiseEstimatorListener listener) {
         this.listener = listener;
     }
 
     /**
      * Gets length of number of samples to keep within the window being processed.
      * Window size must always be larger than allowed minimum value.
      *
      * @return length of number of samples to keep within the window.
      */
     public int getWindowSize() {
         return windowSize;
     }
 
     /**
      * Sets length of number of samples to keep within the window being processed.
      * Window size must always be larger than allowed minimum value.
      * When window size is modified, instance state is reset.
      *
      * @param windowSize length of number of samples to keep within the window.
      * @throws IllegalArgumentException if provided value is not valid.
      * @throws LockedException          if estimator is currently running.
      */
     public void setWindowSize(final int windowSize) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         // check that window is larger than minimum allowed value
         if (windowSize < MIN_WINDOW_SIZE) {
             throw new IllegalArgumentException();
         }
 
         this.windowSize = windowSize;
         reset();
     }
 
     /**
      * Gets time interval between body kinematics samples expressed in
      * seconds (s).
      *
      * @return time interval between accelerometer triad samples.
      */
     public double getTimeInterval() {
         return timeInterval;
     }
 
     /**
      * Sets time interval between body kinematics samples expressed in
      * seconds (s).
      *
      * @param timeInterval time interval between accelerometer triad samples.
      * @throws IllegalArgumentException if provided value is negative.
      * @throws LockedException          if estimator is currently running.
      */
     public void setTimeInterval(final double timeInterval) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         if (timeInterval < 0.0) {
             throw new IllegalArgumentException();
         }
 
         this.timeInterval = timeInterval;
     }
 
     /**
      * Gets time interval between body kinematics samples.
      *
      * @return time interval between accelerometer triad samples.
      */
     public Time getTimeIntervalAsTime() {
         return new Time(timeInterval, TimeUnit.SECOND);
     }
 
     /**
      * Gets time interval between body kinematics samples.
      *
      * @param result instance where time interval will be stored.
      */
     public void getTimeIntervalAsTime(final Time result) {
         result.setValue(timeInterval);
         result.setUnit(TimeUnit.SECOND);
     }
 
     /**
      * Sets time interval between body kinematics samples.
      *
      * @param timeInterval time interval between accelerometer triad samples.
      * @throws LockedException if estimator is currently running.
      */
     public void setTimeInterval(final Time timeInterval) throws LockedException {
         setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(),
                 TimeUnit.SECOND));
     }
 
     /**
      * Gets listener to handle events raised by this estimator.
      *
      * @return listener to handle events raised by this estimator.
      */
     public WindowedBodyKinematicsNoiseEstimatorListener getListener() {
         return listener;
     }
 
     /**
      * Sets listener to handle events raised by this estimator.
      *
      * @param listener listener to handle events raised by this estimator.
      * @throws LockedException if this estimator is running.
      */
     public void setListener(final WindowedBodyKinematicsNoiseEstimatorListener listener) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         this.listener = listener;
     }
 
     /**
      * Gets first provided body kinematics within the window.
      *
      * @return first provided body kinematics within the window or null if not
      * available.
      */
     public BodyKinematics getFirstWindowedBodyKinematics() {
         return windowedSamples.isEmpty() ? null : windowedSamples.getFirst();
     }
 
     /**
      * Gets first provided body kinematics within the window.
      *
      * @param result instance where first provided body kinematics will be stored.
      * @return true if result instance was updated, false otherwise.
      */
     public boolean getFirstWindowedBodyKinematics(final BodyKinematics result) {
         if (windowedSamples.isEmpty()) {
             return false;
         } else {
             result.copyFrom(windowedSamples.getFirst());
             return true;
         }
     }
 
     /**
      * Gets last provided body kinematics within the window.
      *
      * @return last provided body kinematics within the window or null if not
      * available.
      */
     public BodyKinematics getLastWindowedBodyKinematics() {
         return windowedSamples.isEmpty() ? null : windowedSamples.getLast();
     }
 
     /**
      * Gets last provided body kinematics within the window.
      *
      * @param result instance where last provided body kinematics will be stored.
      * @return true if result instance was updated, false otherwise.
      */
     public boolean getLastWindowedBodyKinematics(final BodyKinematics result) {
         if (windowedSamples.isEmpty()) {
             return false;
         } else {
             result.copyFrom(windowedSamples.getLast());
             return true;
         }
     }
 
     /**
      * Gets estimated average of x coordinate of accelerometer sensed specific force
      * expressed in meters per squared second (m/s^2).
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of x coordinate of sensed specific force.
      */
     public double getAvgSpecificForceX() {
         return avgSpecificForceX;
     }
 
     /**
      * Gets estimated average of x coordinate of accelerometer sensed specific force.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of x coordinate of sensed specific force.
      */
     public Acceleration getAvgSpecificForceXAsMeasurement() {
         return new Acceleration(avgSpecificForceX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated average of x coordinate of accelerometer sensed specific force.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @param result instance where average of x coordinate of sensed specific force
      *               will be stored.
      */
     public void getAvgSpecificForceXAsMeasurement(final Acceleration result) {
         result.setValue(avgSpecificForceX);
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated average of y coordinate of accelerometer sensed specific force
      * expressed in meters per squared second (m/s^2).
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of y coordinate of sensed specific force.
      */
     public double getAvgSpecificForceY() {
         return avgSpecificForceY;
     }
 
     /**
      * Gets estimated average of y coordinate of accelerometer sensed specific force.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of y coordinate of sensed specific force.
      */
     public Acceleration getAvgSpecificForceYAsMeasurement() {
         return new Acceleration(avgSpecificForceY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated average of y coordinate of accelerometer sensed specific force.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @param result instance where average of y coordinate of sensed specific force
      *               will be stored.
      */
     public void getAvgSpecificForceYAsMeasurement(final Acceleration result) {
         result.setValue(avgSpecificForceY);
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated average of z coordinate of accelerometer sensed specific force
      * expressed in meters per squared second (m/s^2).
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of z coordinate of sensed specific force.
      */
     public double getAvgSpecificForceZ() {
         return avgSpecificForceZ;
     }
 
     /**
      * Gets estimated average of z coordinate of accelerometer sensed specific force.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of z coordinate of sensed specific force.
      */
     public Acceleration getAvgSpecificForceZAsMeasurement() {
         return new Acceleration(avgSpecificForceZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated average of z coordinate of accelerometer sensed specific force.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @param result instance where average of z coordinate of sensed specific force
      *               will be stored.
      */
     public void getAvgSpecificForceZAsMeasurement(final Acceleration result) {
         result.setValue(avgSpecificForceZ);
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated average of accelerometer sensed specific force as a measurement
      * triad.
      *
      * @return average accelerometer triad.
      */
     public AccelerationTriad getAvgSpecificForceAsTriad() {
         return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                 avgSpecificForceX, avgSpecificForceY, avgSpecificForceZ);
     }
 
     /**
      * Gets estimated average of accelerometer sensed specific force as a measurement
      * triad.
      *
      * @param result instance where average accelerometer triad will be stored.
      */
     public void getAvgSpecificForceAsTriad(final AccelerationTriad result) {
         result.setValueCoordinatesAndUnit(avgSpecificForceX, avgSpecificForceY, avgSpecificForceZ,
                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets norm of estimated average acceleration expressed in meters per squared
      * second (m/s^2). This value is independent of body orientation.
      *
      * @return norm of estimated average acceleration.
      */
     public double getAvgSpecificForceNorm() {
         return Math.sqrt(avgSpecificForceX * avgSpecificForceX + avgSpecificForceY * avgSpecificForceY
                 + avgSpecificForceZ * avgSpecificForceZ);
     }
 
     /**
      * Gets norm of estimated average acceleration within current window.
      *
      * @return norm of estimated average acceleration.
      */
     public Acceleration getAvgSpecificForceNormAsMeasurement() {
         return new Acceleration(getAvgSpecificForceNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets norm of estimated average acceleration.
      *
      * @param result instance where norm of estimated average acceleration will be stored.
      */
     public void getAvgSpecificForceNormAsMeasurement(final Acceleration result) {
         result.setValue(getAvgSpecificForceNorm());
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated average of x coordinate of gyroscope sensed angular rate
      * expressed in radians per second (rad/s).
      * This value will depend of body location and orientation, hence it should
      * never be used as a calibration bias.
      *
      * @return average of x coordinate of sensed angular rate.
      */
     public double getAvgAngularRateX() {
         return avgAngularRateX;
     }
 
     /**
      * Gets estimated average of x coordinate of gyroscope sensed angular rate.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of x coordinate of sensed angular rate.
      */
     public AngularSpeed getAvgAngularRateXAsMeasurement() {
         return new AngularSpeed(avgAngularRateX, AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated average of x coordinate of gyroscope sensed angular rate.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @param result instance where average of x coordinate of sensed angular rate
      *               will be stored.
      */
     public void getAvgAngularRateXAsMeasurement(final AngularSpeed result) {
         result.setValue(avgAngularRateX);
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated average of y coordinate of gyroscope sensed angular rate
      * expressed in radians per second (rad/s).
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of y coordinate of sensed angular rate.
      */
     public double getAvgAngularRateY() {
         return avgAngularRateY;
     }
 
     /**
      * Gets estimated average of y coordinate of gyroscope sensed angular rate.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of y coordinate of sensed angular rate.
      */
     public AngularSpeed getAvgAngularRateYAsMeasurement() {
         return new AngularSpeed(avgAngularRateY, AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated average of y coordinate of gyroscope sensed angular rate.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @param result instance where average of y coordinate of sensed angular rate
      *               will be stored.
      */
     public void getAvgAngularRateYAsMeasurement(final AngularSpeed result) {
         result.setValue(avgAngularRateY);
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated average of z coordinate of gyroscope sensed angular rate
      * expressed in radians per second (rad/s).
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of z coordinate of sensed angular rate.
      */
     public double getAvgAngularRateZ() {
         return avgAngularRateZ;
     }
 
     /**
      * Gets estimated average of z coordinate of gyroscope sensed angular rate.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of z coordinate of sensed angular rate.
      */
     public AngularSpeed getAvgAngularRateZAsMeasurement() {
         return new AngularSpeed(avgAngularRateZ, AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated average of z coordinate of gyroscope sensed angular rate.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @param result instance where average of z coordinate of sensed angular rate
      *               will be stored.
      */
     public void getAvgAngularRateZAsMeasurement(final AngularSpeed result) {
         result.setValue(avgAngularRateZ);
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated average of gyroscope sensed angular speed as a measurement
      * triad.
      *
      * @return average angular speed triad.
      */
     public AngularSpeedTriad getAvgAngularRateTriad() {
         return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                 avgAngularRateX, avgAngularRateY, avgAngularRateZ);
     }
 
     /**
      * Gets estimated average of gyroscope sensed angular speed as a measurement
      * triad.
      *
      * @param result instance where average angular speed triad will be stored.
      */
     public void getAvgAngularRateTriad(final AngularSpeedTriad result) {
         result.setValueCoordinatesAndUnit(avgAngularRateX, avgAngularRateY, avgAngularRateZ,
                 AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets norm of estimated average angular speed expressed in radians per
      * second (rad/s). This value is independent of body orientation.
      *
      * @return norm of estimated average angular speed.
      */
     public double getAvgAngularRateNorm() {
         return Math.sqrt(avgAngularRateX * avgAngularRateX
                 + avgAngularRateY * avgAngularRateY
                 + avgAngularRateZ * avgAngularRateZ);
     }
 
     /**
      * Gets norm of estimated average angular speed.
      * This value is independent of body orientation.
      *
      * @return norm of estimated average angular speed.
      */
     public AngularSpeed getAvgAngularRateNormAsMeasurement() {
         return new AngularSpeed(getAvgAngularRateNorm(), AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets norm of estimated average angular speed.
      * This value is independent of body orientation.
      *
      * @param result instance where norm of estimated average angular speed will be stored.
      */
     public void getAvgAngularRateNormAsMeasurement(final AngularSpeed result) {
         result.setValue(getAvgAngularRateNorm());
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated average of body kinematics.
      *
      * @return estimated average of body kinematics.
      */
     public BodyKinematics getAvgBodyKinematics() {
         final var result = new BodyKinematics();
         getAvgBodyKinematics(result);
         return result;
     }
 
     /**
      * Gets estimated average of body kinematics.
      *
      * @param result instance where estimated average of body kinematics will be stored.
      */
     public void getAvgBodyKinematics(final BodyKinematics result) {
         result.setSpecificForceCoordinates(getAvgSpecificForceX(), getAvgSpecificForceY(), getAvgSpecificForceZ());
         result.setAngularRateCoordinates(getAvgAngularRateX(), getAvgAngularRateY(), getAvgAngularRateZ());
     }
 
     /**
      * Gets estimated variance of x coordinate of accelerometer sensed specific force
      * expressed in (m^2/s^4).
      *
      * @return estimated variance of x coordinate of sensed specific force.
      */
     public double getVarianceSpecificForceX() {
         return varianceSpecificForceX;
     }
 
     /**
      * Gets estimated variance of y coordinate of accelerometer sensed specific
      * force expressed in (m^2/s^4).
      *
      * @return estimated variance of y coordinate of sensed specific force.
      */
     public double getVarianceSpecificForceY() {
         return varianceSpecificForceY;
     }
 
     /**
      * Gets estimated variance of z coordinate of accelerometer sensed specific
      * force expressed in (m^2/s^4).
      *
      * @return estimated variance of z coordinate of sensed specific force.
      */
     public double getVarianceSpecificForceZ() {
         return varianceSpecificForceZ;
     }
 
     /**
      * Gets estimated variance of x coordinate of gyroscope sensed angular rate
      * expressed in (rad^2/s^2).
      *
      * @return estimated variance of x coordinate of sensed angular rate.
      */
     public double getVarianceAngularRateX() {
         return varianceAngularRateX;
     }
 
     /**
      * Gets estimated variance of y coordinate of gyroscope sensed angular rate
      * expressed in (rad^2/s^2).
      *
      * @return estimated variance of y coordinate of sensed angular rate.
      */
     public double getVarianceAngularRateY() {
         return varianceAngularRateY;
     }
 
     /**
      * Gets estimated variance of z coordinate of gyroscope sensed angular rate
      * expressed in (rad^2/s^2).
      *
      * @return estimated variance of z coordinate of sensed angular rate.
      */
     public double getVarianceAngularRateZ() {
         return varianceAngularRateZ;
     }
 
     /**
      * Gets estimated standard deviation of x coordinate of accelerometer
      * sensed specific force expressed in meters per squared second (m/s^2).
      *
      * @return estimated standard deviation of x coordinate of sensed specific
      * force.
      */
     public double getStandardDeviationSpecificForceX() {
         return Math.sqrt(varianceSpecificForceX);
     }
 
     /**
      * Gets estimated standard deviation of x coordinate of accelerometer
      * sensed specific force.
      *
      * @return estimated standard deviation of x coordinate of sensed specific
      * force.
      */
     public Acceleration getStandardDeviationSpecificForceXAsMeasurement() {
         return new Acceleration(getStandardDeviationSpecificForceX(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of x coordinate of accelerometer
      * sensed specific force.
      *
      * @param result instance where estimated standard deviation of x
      *               coordinate of sensed specific force will be stored.
      */
     public void getStandardDeviationSpecificForceXAsMeasurement(final Acceleration result) {
         result.setValue(getStandardDeviationSpecificForceX());
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of y coordinate of accelerometer
      * sensed specific force expressed in meters per squared second (m/s^2).
      *
      * @return estimated standard deviation of y coordinate of sensed specific
      * force.
      */
     public double getStandardDeviationSpecificForceY() {
         return Math.sqrt(varianceSpecificForceY);
     }
 
     /**
      * Gets estimated standard deviation of y coordinate of accelerometer
      * sensed specific force.
      *
      * @return estimated standard deviation of y coordinate of sensed specific
      * force.
      */
     public Acceleration getStandardDeviationSpecificForceYAsMeasurement() {
         return new Acceleration(getStandardDeviationSpecificForceY(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of y coordinate of accelerometer
      * sensed specific force.
      *
      * @param result instance where estimated standard deviation of y
      *               coordinate of sensed specific force will be stored.
      */
     public void getStandardDeviationSpecificForceYAsMeasurement(final Acceleration result) {
         result.setValue(getStandardDeviationSpecificForceY());
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of z coordinate of accelerometer
      * sensed specific force expressed in meters per squared second (m/s^2).
      *
      * @return estimated standard deviation of z coordinate of sensed specific
      * force.
      */
     public double getStandardDeviationSpecificForceZ() {
         return Math.sqrt(varianceSpecificForceZ);
     }
 
     /**
      * Gets estimated standard deviation of z coordinate of accelerometer
      * sensed specific force.
      *
      * @return estimated standard deviation of z coordinate of sensed specific
      * force.
      */
     public Acceleration getStandardDeviationSpecificForceZAsMeasurement() {
         return new Acceleration(getStandardDeviationSpecificForceZ(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of z coordinate of accelerometer
      * sensed specific force.
      *
      * @param result instance where estimated standard deviation of z
      *               coordinate of sensed specific force will be stored.
      */
     public void getStandardDeviationSpecificForceZAsMeasurement(final Acceleration result) {
         result.setValue(getStandardDeviationSpecificForceZ());
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated standard deviation triad of accelerometer measurements.
      *
      * @return estimated standard deviation triad of accelerometer measurements.
      */
     public AccelerationTriad getStandardDeviationSpecificForceTriad() {
         return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                 getStandardDeviationSpecificForceX(),
                 getStandardDeviationSpecificForceY(),
                 getStandardDeviationSpecificForceZ());
     }
 
     /**
      * Gets estimated standard deviation triad of accelerometer measurements.
      *
      * @param result instance where estimated standard deviation triad of
      *               accelerometer measurements will be stored.
      */
     public void getStandardDeviationSpecificForceTriad(final AccelerationTriad result) {
         result.setValueCoordinatesAndUnit(getStandardDeviationSpecificForceX(),
                 getStandardDeviationSpecificForceY(),
                 getStandardDeviationSpecificForceZ(),
                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets norm of estimated standard deviation of accelerometer measurements
      * expressed in meters per squared second (m/s^2).
      *
      * @return norm of estimated standard deviation of accelerometer
      * measurements.
      */
     public double getStandardDeviationSpecificForceNorm() {
         final var fx = getStandardDeviationSpecificForceX();
         final var fy = getStandardDeviationSpecificForceY();
         final var fz = getStandardDeviationSpecificForceZ();
         return Math.sqrt(fx * fx + fy * fy + fz * fz);
     }
 
     /**
      * Gets norm of estimated standard deviation of accelerometer measurements.
      *
      * @return norm of estimated standard deviation of measurements.
      */
     public Acceleration getStandardDeviationSpecificForceNormAsMeasurement() {
         return new Acceleration(getStandardDeviationSpecificForceNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets norm of estimated standard deviation of accelerometer measurements.
      *
      * @param result instance where norm of estimated standard deviation will be
      *               stored.
      */
     public void getStandardDeviationSpecificForceNormAsMeasurement(final Acceleration result) {
         result.setValue(getStandardDeviationSpecificForceNorm());
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets average of estimated standard deviation coordinates of accelerometer
      * measurements expressed in meters per squared second (m/s^2).
      *
      * @return average of estimated standard deviation coordinates.
      */
     public double getAverageStandardDeviationSpecificForce() {
         final var fx = getStandardDeviationSpecificForceX();
         final var fy = getStandardDeviationSpecificForceY();
         final var fz = getStandardDeviationSpecificForceZ();
         return (fx + fy + fz) / 3.0;
     }
 
     /**
      * Gets average of estimated standard deviation coordinates of accelerometer
      * measurements.
      *
      * @return average of estimated standard deviation coordinates.
      */
     public Acceleration getAverageStandardDeviationSpecificForceAsMeasurement() {
         return new Acceleration(getAverageStandardDeviationSpecificForce(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets average of estimated standard deviation coordinates of accelerometer
      * measurements.
      *
      * @param result instance where average of estimated standard deviation coordinates
      *               will be stored.
      */
     public void getAverageStandardDeviationSpecificForceAsMeasurement(final Acceleration result) {
         result.setValue(getAverageStandardDeviationSpecificForce());
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of x coordinate of gyroscope
      * expressed in radians per second (rad/s).
      *
      * @return estimated standard deviation of x coordinate of gyroscope.
      */
     public double getStandardDeviationAngularRateX() {
         return Math.sqrt(varianceAngularRateX);
     }
 
     /**
      * Gets estimated standard deviation of x coordinate of gyroscope.
      *
      * @return estimated standard deviation of x coordinate of gyroscope.
      */
     public AngularSpeed getStandardDeviationAngularRateXAsMeasurement() {
         return new AngularSpeed(getStandardDeviationAngularRateX(), AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of x coordinate of gyroscope.
      *
      * @param result estimated standard deviation of x coordinate of gyroscope.
      */
     public void getStandardDeviationAngularRateXAsMeasurement(final AngularSpeed result) {
         result.setValue(getStandardDeviationAngularRateX());
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of y coordinate of gyroscope
      * expressed in radians per second (rad/s).
      *
      * @return estimated standard deviation of y coordinate of gyroscope.
      */
     public double getStandardDeviationAngularRateY() {
         return Math.sqrt(varianceAngularRateY);
     }
 
     /**
      * Gets estimated standard deviation of y coordinate of gyroscope.
      *
      * @return estimated standard deviation of y coordinate of gyroscope.
      */
     public AngularSpeed getStandardDeviationAngularRateYAsMeasurement() {
         return new AngularSpeed(getStandardDeviationAngularRateY(), AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of y coordinate of gyroscope.
      *
      * @param result estimated standard deviation of y coordinate of gyroscope.
      */
     public void getStandardDeviationAngularRateYAsMeasurement(final AngularSpeed result) {
         result.setValue(getStandardDeviationAngularRateY());
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of z coordinate of gyroscope
      * expressed in radians per second (rad/s).
      *
      * @return estimated standard deviation of z coordinate of gyroscope.
      */
     public double getStandardDeviationAngularRateZ() {
         return Math.sqrt(varianceAngularRateZ);
     }
 
     /**
      * Gets estimated standard deviation of z coordinate of gyroscope.
      *
      * @return estimated standard deviation of z coordinate of gyroscope.
      */
     public AngularSpeed getStandardDeviationAngularRateZAsMeasurement() {
         return new AngularSpeed(getStandardDeviationAngularRateZ(), AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of z coordinate of gyroscope.
      *
      * @param result estimated standard deviation of z coordinate of gyroscope.
      */
     public void getStandardDeviationAngularRateZAsMeasurement(final AngularSpeed result) {
         result.setValue(getStandardDeviationAngularRateZ());
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated standard deviation triad of angular speed measurements.
      *
      * @return estimated standard deviation triad of angular speed measurements.
      */
     public AngularSpeedTriad getStandardDeviationAngularSpeedTriad() {
         return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                 getStandardDeviationAngularRateX(),
                 getStandardDeviationAngularRateY(),
                 getStandardDeviationAngularRateZ());
     }
 
     /**
      * Gets estimated standard deviation triad of angular speed measurements.
      *
      * @param result instance where estimated standard deviation triad of
      *               gyroscope measurements will be stored.
      */
     public void getStandardDeviationAngularSpeedTriad(final AngularSpeedTriad result) {
         result.setValueCoordinatesAndUnit(getStandardDeviationAngularRateX(),
                 getStandardDeviationAngularRateY(),
                 getStandardDeviationAngularRateZ(),
                 AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets norm of estimated standard deviation of gyroscope measurements
      * expressed in radians per second (rad/s).
      *
      * @return norm of estimated standard deviation of gyroscope
      * measurements.
      */
     public double getStandardDeviationAngularSpeedNorm() {
         final var wx = getStandardDeviationAngularRateX();
         final var wy = getStandardDeviationAngularRateY();
         final var wz = getStandardDeviationAngularRateZ();
         return Math.sqrt(wx * wx + wy * wy + wz * wz);
     }
 
     /**
      * Gets norm of estimated standard deviation of gyroscope measurements.
      *
      * @return norm of estimated standard deviation of measurements.
      */
     public AngularSpeed getStandardDeviationAngularSpeedNormAsMeasurement() {
         return new AngularSpeed(getStandardDeviationAngularSpeedNorm(), AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets norm of estimated standard deviation of gyroscope measurements.
      *
      * @param result instance where norm of estimated standard deviation will be
      *               stored.
      */
     public void getStandardDeviationAngularSpeedNormAsMeasurement(final AngularSpeed result) {
         result.setValue(getStandardDeviationAngularSpeedNorm());
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets average of estimated standard deviation coordinates of gyroscope
      * measurements expressed in radians per second (rad/s).
      *
      * @return average of estimated standard deviation coordinates.
      */
     public double getAverageStandardDeviationAngularSpeed() {
         final var wx = getStandardDeviationAngularRateX();
         final var wy = getStandardDeviationAngularRateY();
         final var wz = getStandardDeviationAngularRateZ();
         return (wx + wy + wz) / 3.0;
     }
 
     /**
      * Gets average of estimated standard deviation coordinates of gyroscope
      * measurements.
      *
      * @return average of estimated standard deviation coordinates.
      */
     public AngularSpeed getAverageStandardDeviationAngularSpeedAsMeasurement() {
         return new AngularSpeed(getAverageStandardDeviationAngularSpeed(), AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets average of estimated standard deviation coordinates of gyroscope
      * measurements.
      *
      * @param result instance where average of estimated standard deviation coordinates
      *               will be stored.
      */
     public void getAverageStandardDeviationAngularSpeedAsMeasurement(final AngularSpeed result) {
         result.setValue(getAverageStandardDeviationAngularSpeed());
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated standard deviations of accelerometer and gyroscope components
      * as a body kinematics instance.
      *
      * @return a body kinematics instance containing standard deviation values.
      */
     public BodyKinematics getStandardDeviationAsBodyKinematics() {
         return new BodyKinematics(getStandardDeviationSpecificForceX(),
                 getStandardDeviationSpecificForceY(),
                 getStandardDeviationSpecificForceZ(),
                 getStandardDeviationAngularRateX(),
                 getStandardDeviationAngularRateY(),
                 getStandardDeviationAngularRateZ());
     }
 
     /**
      * Gets estimated standard deviations of accelerometer and gyroscope components
      * as a body kinematics instance.
      *
      * @param result instance where data will be stored.
      */
     public void getStandardDeviationAsBodyKinematics(final BodyKinematics result) {
         result.setSpecificForceCoordinates(getStandardDeviationSpecificForceX(),
                 getStandardDeviationSpecificForceY(),
                 getStandardDeviationSpecificForceZ());
         result.setAngularRateCoordinates(getStandardDeviationAngularRateX(),
                 getStandardDeviationAngularRateY(),
                 getStandardDeviationAngularRateZ());
     }
 
     /**
      * Gets accelerometer noise PSD (Power Spectral Density) on x axis expressed
      * in (m^2 * s^-3).
      *
      * @return accelerometer noise PSD on x axis.
      */
     public double getSpecificForcePsdX() {
         return varianceSpecificForceX * timeInterval;
     }
 
     /**
      * Gets accelerometer noise PSD (Power Spectral Density) on y axis expressed
      * in (m^2 * s^-3).
      *
      * @return accelerometer noise PSD on y axis.
      */
     public double getSpecificForcePsdY() {
         return varianceSpecificForceY * timeInterval;
     }
 
     /**
      * Gets accelerometer noise PSD (Power Spectral Density) on z axis expressed
      * in (m^2 * s^-3).
      *
      * @return accelerometer noise PSD on z axis.
      */
     public double getSpecificForcePsdZ() {
         return varianceSpecificForceZ * timeInterval;
     }
 
     /**
      * Gets gyroscope noise PSD (Power Spectral Density) on x axis expressed
      * in (rad^2/s).
      *
      * @return gyroscope noise PSD on x axis.
      */
     public double getAngularRatePsdX() {
         return varianceAngularRateX * timeInterval;
     }
 
     /**
      * Gets gyroscope noise PSD (Power Spectral Density) on y axis expressed
      * in (rad^2/s).
      *
      * @return gyroscope noise PSD on y axis.
      */
     public double getAngularRatePsdY() {
         return varianceAngularRateY * timeInterval;
     }
 
     /**
      * Gets gyroscope noise PSD (Power Spectral Density) on z axis expressed
      * in (rad^2/s).
      *
      * @return gyroscope noise PSD on z axis.
      */
     public double getAngularRatePsdZ() {
         return varianceAngularRateZ * timeInterval;
     }
 
     /**
      * Gets accelerometer noise root PSD (Power Spectral Density) on x axis
      * expressed in (m * s^-1.5).
      *
      * @return accelerometer noise root PSD on x axis.
      */
     public double getSpecificForceRootPsdX() {
         return Math.sqrt(getSpecificForcePsdX());
     }
 
     /**
      * Gets accelerometer noise root PSD (Power Spectral Density) on y axis
      * expressed in (m * s^-1.5).
      *
      * @return accelerometer noise root PSD on y axis.
      */
     public double getSpecificForceRootPsdY() {
         return Math.sqrt(getSpecificForcePsdY());
     }
 
     /**
      * Gets accelerometer noise root PSD (Power Spectral Density) on z axis
      * expressed in (m * s^-1.5).
      *
      * @return accelerometer noise root PSD on z axis.
      */
     public double getSpecificForceRootPsdZ() {
         return Math.sqrt(getSpecificForcePsdZ());
     }
 
     /**
      * Gets gyroscope noise root PSD (Power Spectral Density) on x axis
      * expressed in (rad * s^-0.5).
      *
      * @return gyroscope noise root PSD on x axis.
      */
     public double getAngularRateRootPsdX() {
         return Math.sqrt(getAngularRatePsdX());
     }
 
     /**
      * Gets gyroscope noise root PSD (Power Spectral Density) on y axis
      * expressed in (rad * s^-0.5).
      *
      * @return gyroscope noise root PSD on y axis.
      */
     public double getAngularRateRootPsdY() {
         return Math.sqrt(getAngularRatePsdY());
     }
 
     /**
      * Gets gyroscope noise root PSD (Power Spectral Density) on z axis
      * expressed in (rad * s^-0.5).
      *
      * @return gyroscope noise root PSD on z axis.
      */
     public double getAngularRateRootPsdZ() {
         return Math.sqrt(getAngularRatePsdZ());
     }
 
     /**
      * Gets average accelerometer noise PSD (Power Spectral Density) among
      * x,y,z components expressed as (m^2/s^-3).
      *
      * @return average accelerometer noise PSD.
      */
     public double getAvgSpecificForceNoisePsd() {
         return (getSpecificForcePsdX() + getSpecificForcePsdY() + getSpecificForcePsdZ()) / 3.0;
     }
 
     /**
      * Gets norm of noise root PSD (Power Spectral Density) among x,y,z
      * components expressed as (m * s^-1.5).
      *
      * @return norm of noise root PSD.
      */
     public double getSpecificForceNoiseRootPsdNorm() {
         return Math.sqrt(getSpecificForcePsdX() + getSpecificForcePsdY() + getSpecificForcePsdZ());
     }
 
     /**
      * Gets average gyroscope noise PSD (Power Spectral Density) among
      * x,y,z components expressed in (rad^2/s).
      *
      * @return average gyroscope noise PSD.
      */
     public double getAvgAngularRateNoisePsd() {
         return (getAngularRatePsdX() + getAngularRatePsdY() + getAngularRatePsdZ()) / 3.0;
     }
 
     /**
      * Gets norm of noise root PSD (Power Spectral Density) among x,y,z
      * components expressed as (rad * s^-0.5).
      *
      * @return norm of noise root PSD.
      */
     public double getAngularRateNoiseRootPsdNorm() {
         return Math.sqrt(getAngularRatePsdX() + getAngularRatePsdY() + getAngularRatePsdZ());
     }
 
     /**
      * Gets number of samples that have been processed so far.
      *
      * @return number of samples that have been processed so far.
      */
     public int getNumberOfProcessedSamples() {
         return numberOfProcessedSamples;
     }
 
     /**
      * Gets number of currently windowed samples.
      *
      * @return number of samples within the window.
      */
     public int getNumberOfSamplesInWindow() {
         return windowedSamples.size();
     }
 
     /**
      * Indicates whether estimator is currently running or not.
      *
      * @return true if estimator is running, false otherwise.
      */
     public boolean isRunning() {
         return running;
     }
 
     /**
      * Indicates whether window of samples is filled or not.
      *
      * @return true if window is filled, false otherwise.
      */
     public boolean isWindowFilled() {
         return getNumberOfSamplesInWindow() == windowSize;
     }
 
     /**
      * Adds a body kinematics measurement and processes current window.
      *
      * @param specificForceX x coordinate of specific force expressed in meters per squared second (m/s^2).
      * @param specificForceY y coordinate of specific force expressed in meters per squared second (m/s^2).
      * @param specificForceZ z coordinate of specific force expressed in meters per squared second (m/s^2).
      * @param angularRateX   x coordinate of angular rate expressed in radians per second (rad/s).
      * @param angularRateY   y coordinate of angular rate expressed in radians per second (rad/s).
      * @param angularRateZ   z coordinate of angular rate expressed in radians per second (rad/s).
      * @return true if provided kinematics instance has been processed, false if it has
      * been ignored.
      * @throws LockedException if estimator is currently running.
      */
     public boolean addBodyKinematicsAndProcess(
             final double specificForceX, final double specificForceY, final double specificForceZ,
             final double angularRateX, final double angularRateY, final double angularRateZ) throws LockedException {
         return addBodyKinematicsAndProcess(new BodyKinematics(specificForceX, specificForceY, specificForceZ,
                 angularRateX, angularRateY, angularRateZ));
     }
 
     /**
      * Adds a body kinematics measurement and processes current window.
      *
      * @param specificForceX x coordinate of specific force.
      * @param specificForceY y coordinate of specific force.
      * @param specificForceZ z coordinate of specific force.
      * @param angularRateX   x coordinate of angular rate.
      * @param angularRateY   y coordinate of angular rate.
      * @param angularRateZ   z coordinate of angular rate.
      * @return true if provided kinematics instance has been processed, false if it has
      * been ignored.
      * @throws LockedException if estimator is currently running.
      */
     public boolean addBodyKinematicsAndProcess(
             final Acceleration specificForceX, final Acceleration specificForceY, final Acceleration specificForceZ,
             final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
             throws LockedException {
         return addBodyKinematicsAndProcess(new BodyKinematics(specificForceX, specificForceY, specificForceZ,
                 angularRateX, angularRateY, angularRateZ));
     }
 
     /**
      * Adds a body kinematics measurement and processes current window.
      *
      * @param specificForce specific force triad.
      * @param angularSpeed  angular speed triad.
      * @return true if provided kinematics instance has been processed, false if it has
      * been ignored.
      * @throws LockedException if estimator is currently running.
      */
     public boolean addBodyKinematicsAndProcess(
             final AccelerationTriad specificForce, final AngularSpeedTriad angularSpeed) throws LockedException {
         return addBodyKinematicsAndProcess(new BodyKinematics(specificForce, angularSpeed));
     }
 
     /**
      * Adds a body kinematics measurement and processes current window.
      *
      * @param kinematics body kinematics to be added and processed.
      * @return true if provided kinematics instance has been processed, false if it has
      * been ignored.
      * @throws LockedException if estimator is currently running.
      */
     public boolean addBodyKinematicsAndProcess(final BodyKinematics kinematics) throws LockedException {
         return internalAdd(kinematics, true);
     }
 
     /**
      * Adds a body kinematics measurement.
      *
      * @param specificForceX x coordinate of specific force expressed in meters per squared second (m/s^2).
      * @param specificForceY y coordinate of specific force expressed in meters per squared second (m/s^2).
      * @param specificForceZ z coordinate of specific force expressed in meters per squared second (m/s^2).
      * @param angularRateX   x coordinate of angular rate expressed in radians per second (rad/s).
      * @param angularRateY   y coordinate of angular rate expressed in radians per second (rad/s).
      * @param angularRateZ   z coordinate of angular rate expressed in radians per second (rad/s).
      * @throws LockedException if estimator is currently running.
      */
     public void addBodyKinematics(
             final double specificForceX, final double specificForceY, final double specificForceZ,
             final double angularRateX, final double angularRateY, final double angularRateZ) throws LockedException {
         addBodyKinematics(new BodyKinematics(specificForceX, specificForceY, specificForceZ,
                 angularRateX, angularRateY, angularRateZ));
     }
 
     /**
      * Adds a body kinematics measurement.
      *
      * @param specificForceX x coordinate of specific force.
      * @param specificForceY y coordinate of specific force.
      * @param specificForceZ z coordinate of specific force.
      * @param angularRateX   x coordinate of angular rate.
      * @param angularRateY   y coordinate of angular rate.
      * @param angularRateZ   z coordinate of angular rate.
      * @throws LockedException if estimator is currently running.
      */
     public void addBodyKinematics(
             final Acceleration specificForceX, final Acceleration specificForceY, final Acceleration specificForceZ,
             final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
             throws LockedException {
         addBodyKinematics(new BodyKinematics(specificForceX, specificForceY, specificForceZ,
                 angularRateX, angularRateY, angularRateZ));
     }
 
     /**
      * Adds a body kinematics measurement.
      *
      * @param specificForce specific force triad.
      * @param angularSpeed  angular speed triad.
      * @throws LockedException if estimator is currently running.
      */
     public void addBodyKinematics(
             final AccelerationTriad specificForce, final AngularSpeedTriad angularSpeed) throws LockedException {
         addBodyKinematics(new BodyKinematics(specificForce, angularSpeed));
     }
 
     /**
      * Adds a body kinematics measurement.
      *
      * @param kinematics body kinematics to be added.
      * @throws LockedException if estimator is currently running.
      */
     public void addBodyKinematics(final BodyKinematics kinematics) throws LockedException {
         internalAdd(kinematics, false);
     }
 
     /**
      * Resets current estimator.
      *
      * @return true if estimator was successfully reset, false if no reset was needed.
      * @throws LockedException if estimator is currently running.
      */
     public boolean reset() throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         if (numberOfProcessedSamples == 0) {
             return false;
         }
 
         windowedSamples.clear();
         avgSpecificForceX = 0.0;
         avgSpecificForceY = 0.0;
         avgSpecificForceZ = 0.0;
         avgAngularRateX = 0.0;
         avgAngularRateY = 0.0;
         avgAngularRateZ = 0.0;
         varianceSpecificForceX = 0.0;
         varianceSpecificForceY = 0.0;
         varianceSpecificForceZ = 0.0;
         varianceAngularRateX = 0.0;
         varianceAngularRateY = 0.0;
         varianceAngularRateZ = 0.0;
         numberOfProcessedSamples = 0;
 
         if (listener != null) {
             listener.onReset(this);
         }
 
         return true;
     }
 
     /**
      * Internally adds a body kinematics measurement and processes current window if indicated.
      *
      * @param kinematics body kinematics to be added.
      * @param process    true if window of samples must also be processed, false otherwise.
      * @return true if result values were updated, false if not enough samples are available yet
      * and no average or variance values have been computed yet.
      * @throws LockedException if estimator is currently running.
      */
     private boolean internalAdd(final BodyKinematics kinematics, boolean process) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         running = true;
 
         if (windowedSamples.isEmpty() && listener != null) {
             listener.onStart(this);
         }
 
         final var wasFilled = isWindowFilled();
         if (wasFilled) {
             // remove first sample
             windowedSamples.removeFirst();
         }
 
         windowedSamples.addLast(new BodyKinematics(kinematics));
 
         // process window
         final var result = process && processWindow();
 
         running = false;
 
         if (listener != null) {
             listener.onBodyKinematicsAdded(this);
 
             if (!wasFilled && isWindowFilled()) {
                 listener.onWindowFilled(this);
             }
         }
 
         return result;
     }
 
     /**
      * Processes current windowed samples.
      *
      * @return true if sample was processed, false it there are not enough samples to
      * process current window.
      */
     private boolean processWindow() {
         numberOfProcessedSamples++;
 
         final var n = windowedSamples.size();
         if (n <= 1) {
             return false;
         }
 
         // compute averages
         var avgFx = 0.0;
         var avgFy = 0.0;
         var avgFz = 0.0;
         var avgWx = 0.0;
         var avgWy = 0.0;
         var avgWz = 0.0;
         for (final var kinematics : windowedSamples) {
             final var fx = kinematics.getFx();
             final var fy = kinematics.getFy();
             final var fz = kinematics.getFz();
             final var wx = kinematics.getAngularRateX();
             final var wy = kinematics.getAngularRateY();
             final var wz = kinematics.getAngularRateZ();
 
             avgFx += fx;
             avgFy += fy;
             avgFz += fz;
             avgWx += wx;
             avgWy += wy;
             avgWz += wz;
         }
 
         avgFx /= n;
         avgFy /= n;
         avgFz /= n;
         avgWx /= n;
         avgWy /= n;
         avgWz /= n;
 
         // compute variances
         var varFx = 0.0;
         var varFy = 0.0;
         var varFz = 0.0;
         var varWx = 0.0;
         var varWy = 0.0;
         var varWz = 0.0;
         for (final var kinematics : windowedSamples) {
             final var fx = kinematics.getFx();
             final var fy = kinematics.getFy();
             final var fz = kinematics.getFz();
             final var wx = kinematics.getAngularRateX();
             final var wy = kinematics.getAngularRateY();
             final var wz = kinematics.getAngularRateZ();
 
             final var diffFx = fx - avgFx;
             final var diffFy = fy - avgFy;
             final var diffFz = fz - avgFz;
             final var diffWx = wx - avgWx;
             final var diffWy = wy - avgWy;
             final var diffWz = wz - avgWz;
 
             final var diffFx2 = diffFx * diffFx;
             final var diffFy2 = diffFy * diffFy;
             final var diffFz2 = diffFz * diffFz;
             final var diffWx2 = diffWx * diffWx;
             final var diffWy2 = diffWy * diffWy;
             final var diffWz2 = diffWz * diffWz;
 
             varFx += diffFx2;
             varFy += diffFy2;
             varFz += diffFz2;
             varWx += diffWx2;
             varWy += diffWy2;
             varWz += diffWz2;
         }
 
         final var nMinusOne = n - 1;
 
         varFx /= nMinusOne;
         varFy /= nMinusOne;
         varFz /= nMinusOne;
         varWx /= nMinusOne;
         varWy /= nMinusOne;
         varWz /= nMinusOne;
 
         avgSpecificForceX = avgFx;
         avgSpecificForceY = avgFy;
         avgSpecificForceZ = avgFz;
         avgAngularRateX = avgWx;
         avgAngularRateY = avgWy;
         avgAngularRateZ = avgWz;
 
         varianceSpecificForceX = varFx;
         varianceSpecificForceY = varFy;
         varianceSpecificForceZ = varFz;
         varianceAngularRateX = varWx;
         varianceAngularRateY = varWy;
         varianceAngularRateZ = varWz;
 
         return true;
     }
 
     /**
      * Gets accelerometer base noise level root PSD (Power Spectral Density)
      * expressed in (m * s^-1.5).
      *
      * @return accelerometer base noise level root PSD.
      */
     @Override
     public double getAccelerometerBaseNoiseLevelRootPsd() {
         return getSpecificForceNoiseRootPsdNorm();
     }
 
     /**
      * Gets gyroscope base noise level root PSD (Power Spectral Density)
      * expressed in (rad * s^-0.5)
      *
      * @return gyroscope base noise level root PSD.
      */
     @Override
     public double getGyroscopeBaseNoiseLevelRootPsd() {
         return getAngularRateNoiseRootPsdNorm();
     }
 }