/*
    * 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.AccelerationConverter;
  import com.irurueta.units.AccelerationUnit;
  import com.irurueta.units.AngularSpeed;
  import com.irurueta.units.AngularSpeedConverter;
  import com.irurueta.units.AngularSpeedUnit;
  import com.irurueta.units.Time;
  
  /**
   * Estimates accumulated acceleration and angular speed noise variances and PSD's
   * (Power Spectral Densities) along with their average values.
   * 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.
   */
  public class AccumulatedBodyKinematicsNoiseEstimator implements AccelerometerNoiseRootPsdSource,
          GyroscopeNoiseRootPsdSource {
  
      /**
       * Default time interval between accelerometer samples expressed in seconds
       * (s).
       */
      public static final double DEFAULT_TIME_INTERVAL_SECONDS =
              AccumulatedTriadNoiseEstimator.DEFAULT_TIME_INTERVAL_SECONDS;
  
      /**
       * Listener to handle events raised by this estimator.
       */
      private AccumulatedBodyKinematicsNoiseEstimatorListener listener;
  
      /**
       * Last provided body kinematics.
       */
      private BodyKinematics lastBodyKinematics;
  
      /**
       * Accumulated acceleration estimator.
       */
      private final AccumulatedAccelerationTriadNoiseEstimator accelerationEstimator =
              new AccumulatedAccelerationTriadNoiseEstimator();
  
      /**
       * Accumulated angular speed estimator.
       */
      private final AccumulatedAngularSpeedTriadNoiseEstimator angularSpeedEstimator =
              new AccumulatedAngularSpeedTriadNoiseEstimator();
  
      /**
       * Indicates that estimator is running.
       */
      private boolean running;
  
      /**
       * Constructor.
       */
      public AccumulatedBodyKinematicsNoiseEstimator() {
      }
  
      /**
       * Constructor.
       *
       * @param listener listener to handle events raised by this estimator.
       */
      public AccumulatedBodyKinematicsNoiseEstimator(final AccumulatedBodyKinematicsNoiseEstimatorListener listener) {
          this.listener = listener;
      }
 
     /**
      * Gets time interval between body kinematics samples expressed in
      * seconds (s).
      *
      * @return time interval between body kinematics samples.
      */
     public double getTimeInterval() {
         return accelerationEstimator.getTimeInterval();
     }
 
     /**
      * Sets time interval between body kinematics samples expressed in
      * seconds (s).
      *
      * @param timeInterval time interval between body kinematic 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();
         }
 
         accelerationEstimator.setTimeInterval(timeInterval);
         angularSpeedEstimator.setTimeInterval(timeInterval);
     }
 
     /**
      * Gets time interval between body kinematics samples.
      *
      * @return time interval between body kinematics samples.
      */
     public Time getTimeIntervalAsTime() {
         return accelerationEstimator.getTimeIntervalAsTime();
     }
 
     /**
      * Gets time interval between body kinematics samples.
      *
      * @param result instance where body kinematics will be stored.
      */
     public void getTimeIntervalAsTime(final Time result) {
         accelerationEstimator.getTimeIntervalAsTime(result);
     }
 
     /**
      * Sets time interval between body kinematics samples.
      *
      * @param timeInterval time interval between body kinematics samples.
      * @throws LockedException if estimator is currently running.
      */
     public void setTimeInterval(final Time timeInterval) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         accelerationEstimator.setTimeInterval(timeInterval);
         angularSpeedEstimator.setTimeInterval(timeInterval);
     }
 
     /**
      * Gets listener to handle events raised by this estimator.
      *
      * @return listener to handle events raised by this estimator.
      */
     public AccumulatedBodyKinematicsNoiseEstimatorListener 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 AccumulatedBodyKinematicsNoiseEstimatorListener listener) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         this.listener = listener;
     }
 
     /**
      * Gets last provided body kinematics or null if not available.
      *
      * @return last provided body kinematics or null.
      */
     public BodyKinematics getLastBodyKinematics() {
         return lastBodyKinematics;
     }
 
     /**
      * Gets last provided body kinematics.
      *
      * @param result instance where last provided body kinematics will be stored.
      * @return true if result instance was updated, false otherwise.
      */
     public boolean getLastBodyKinematics(final BodyKinematics result) {
         if (lastBodyKinematics != null) {
             lastBodyKinematics.copyTo(result);
             return true;
         } else {
             return false;
         }
     }
 
     /**
      * 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 accelerationEstimator.getAvgX();
     }
 
     /**
      * 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 accelerationEstimator.getAvgXAsMeasurement();
     }
 
     /**
      * 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) {
         accelerationEstimator.getAvgXAsMeasurement(result);
     }
 
     /**
      * 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 accelerationEstimator.getAvgY();
     }
 
     /**
      * 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 accelerationEstimator.getAvgYAsMeasurement();
     }
 
     /**
      * 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) {
         accelerationEstimator.getAvgYAsMeasurement(result);
     }
 
     /**
      * 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 accelerationEstimator.getAvgZ();
     }
 
     /**
      * 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 accelerationEstimator.getAvgZAsMeasurement();
     }
 
     /**
      * 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) {
         accelerationEstimator.getAvgZAsMeasurement(result);
     }
 
     /**
      * Gets estimated average of accelerometer sensed specific force as a measurement
      * triad.
      *
      * @return average accelerometer triad.
      */
     public AccelerationTriad getAvgSpecificForceAsTriad() {
         return accelerationEstimator.getAvgTriad();
     }
 
     /**
      * 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) {
         accelerationEstimator.getAvgTriad(result);
     }
 
     /**
      * 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 accelerationEstimator.getAvgNorm();
     }
 
     /**
      * Gets norm of estimated average acceleration within current window.
      *
      * @return norm of estimated average acceleration.
      */
     public Acceleration getAvgSpecificForceNormAsMeasurement() {
         return accelerationEstimator.getAvgNormAsMeasurement();
     }
 
     /**
      * Gets norm of estimated average acceleration.
      *
      * @param result instance where norm of estimated average acceleration will be stored.
      */
     public void getAvgSpecificForceNormAsMeasurement(final Acceleration result) {
         accelerationEstimator.getAvgNormAsMeasurement(result);
     }
 
     /**
      * 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 angularSpeedEstimator.getAvgX();
     }
 
     /**
      * 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 angularSpeedEstimator.getAvgXAsMeasurement();
     }
 
     /**
      * 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) {
         angularSpeedEstimator.getAvgXAsMeasurement(result);
     }
 
     /**
      * 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 angularSpeedEstimator.getAvgY();
     }
 
     /**
      * 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 angularSpeedEstimator.getAvgYAsMeasurement();
     }
 
     /**
      * 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) {
         angularSpeedEstimator.getAvgYAsMeasurement(result);
     }
 
     /**
      * 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 angularSpeedEstimator.getAvgZ();
     }
 
     /**
      * 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 angularSpeedEstimator.getAvgZAsMeasurement();
     }
 
     /**
      * 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) {
         angularSpeedEstimator.getAvgZAsMeasurement(result);
     }
 
     /**
      * Gets estimated average of gyroscope sensed angular speed as a measurement
      * triad.
      *
      * @return average angular speed triad.
      */
     public AngularSpeedTriad getAvgAngularRateTriad() {
         return angularSpeedEstimator.getAvgTriad();
     }
 
     /**
      * 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) {
         angularSpeedEstimator.getAvgTriad(result);
     }
 
     /**
      * 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 angularSpeedEstimator.getAvgNorm();
     }
 
     /**
      * 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 angularSpeedEstimator.getAvgNormAsMeasurement();
     }
 
     /**
      * 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) {
         angularSpeedEstimator.getAvgNormAsMeasurement(result);
     }
 
     /**
      * 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) {
         final var avgFx = accelerationEstimator.getAvgX();
         final var avgFy = accelerationEstimator.getAvgY();
         final var avgFz = accelerationEstimator.getAvgZ();
 
         final var avgWx = angularSpeedEstimator.getAvgX();
         final var avgWy = angularSpeedEstimator.getAvgY();
         final var avgWz = angularSpeedEstimator.getAvgZ();
 
         result.setSpecificForceCoordinates(avgFx, avgFy, avgFz);
         result.setAngularRateCoordinates(avgWx, avgWy, avgWz);
     }
 
     /**
      * 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 accelerationEstimator.getVarianceX();
     }
 
     /**
      * 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 accelerationEstimator.getVarianceY();
     }
 
     /**
      * 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 accelerationEstimator.getVarianceZ();
     }
 
     /**
      * 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 angularSpeedEstimator.getVarianceX();
     }
 
     /**
      * 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 angularSpeedEstimator.getVarianceY();
     }
 
     /**
      * 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 angularSpeedEstimator.getVarianceZ();
     }
 
     /**
      * 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 accelerationEstimator.getStandardDeviationX();
     }
 
     /**
      * 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 accelerationEstimator.getStandardDeviationXAsMeasurement();
     }
 
     /**
      * 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) {
         accelerationEstimator.getStandardDeviationXAsMeasurement(result);
     }
 
     /**
      * 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 accelerationEstimator.getStandardDeviationY();
     }
 
     /**
      * 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 accelerationEstimator.getStandardDeviationYAsMeasurement();
     }
 
     /**
      * 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) {
         accelerationEstimator.getStandardDeviationYAsMeasurement(result);
     }
 
     /**
      * 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 accelerationEstimator.getStandardDeviationZ();
     }
 
     /**
      * 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 accelerationEstimator.getStandardDeviationZAsMeasurement();
     }
 
     /**
      * 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) {
         accelerationEstimator.getStandardDeviationZAsMeasurement(result);
     }
 
     /**
      * Gets estimated standard deviation triad of accelerometer measurements.
      *
      * @return estimated standard deviation triad of accelerometer measurements.
      */
     public AccelerationTriad getStandardDeviationSpecificForceTriad() {
         return accelerationEstimator.getStandardDeviationTriad();
     }
 
     /**
      * 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) {
         accelerationEstimator.getStandardDeviationTriad(result);
     }
 
     /**
      * 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() {
         return accelerationEstimator.getStandardDeviationNorm();
     }
 
     /**
      * Gets norm of estimated standard deviation of accelerometer measurements.
      *
      * @return norm of estimated standard deviation of measurements.
      */
     public Acceleration getStandardDeviationSpecificForceNormAsMeasurement() {
         return accelerationEstimator.getStandardDeviationNormAsMeasurement();
     }
 
     /**
      * 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) {
         accelerationEstimator.getStandardDeviationNormAsMeasurement(result);
     }
 
     /**
      * 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() {
         return accelerationEstimator.getAverageStandardDeviation();
     }
 
     /**
      * Gets average of estimated standard deviation coordinates of accelerometer
      * measurements.
      *
      * @return average of estimated standard deviation coordinates.
      */
     public Acceleration getAverageStandardDeviationSpecificForceAsMeasurement() {
         return accelerationEstimator.getAverageStandardDeviationAsMeasurement();
     }
 
     /**
      * 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) {
         accelerationEstimator.getAverageStandardDeviationAsMeasurement(result);
     }
 
     /**
      * 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 angularSpeedEstimator.getStandardDeviationX();
     }
 
     /**
      * Gets estimated standard deviation of x coordinate of gyroscope.
      *
      * @return estimated standard deviation of x coordinate of gyroscope.
      */
     public AngularSpeed getStandardDeviationAngularRateXAsMeasurement() {
         return angularSpeedEstimator.getStandardDeviationXAsMeasurement();
     }
 
     /**
      * 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) {
         angularSpeedEstimator.getStandardDeviationXAsMeasurement(result);
     }
 
     /**
      * 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 angularSpeedEstimator.getStandardDeviationY();
     }
 
     /**
      * Gets estimated standard deviation of y coordinate of gyroscope.
      *
      * @return estimated standard deviation of y coordinate of gyroscope.
      */
     public AngularSpeed getStandardDeviationAngularRateYAsMeasurement() {
         return angularSpeedEstimator.getStandardDeviationYAsMeasurement();
     }
 
     /**
      * 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) {
         angularSpeedEstimator.getStandardDeviationYAsMeasurement(result);
     }
 
     /**
      * 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 angularSpeedEstimator.getStandardDeviationZ();
     }
 
     /**
      * Gets estimated standard deviation of z coordinate of gyroscope.
      *
      * @return estimated standard deviation of z coordinate of gyroscope.
      */
     public AngularSpeed getStandardDeviationAngularRateZAsMeasurement() {
         return angularSpeedEstimator.getStandardDeviationZAsMeasurement();
     }
 
     /**
      * 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) {
         angularSpeedEstimator.getStandardDeviationZAsMeasurement(result);
     }
 
     /**
      * Gets estimated standard deviation triad of angular speed measurements.
      *
      * @return estimated standard deviation triad of angular speed measurements.
      */
     public AngularSpeedTriad getStandardDeviationAngularSpeedTriad() {
         return angularSpeedEstimator.getStandardDeviationTriad();
     }
 
     /**
      * 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) {
         angularSpeedEstimator.getStandardDeviationTriad(result);
     }
 
     /**
      * 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() {
         return angularSpeedEstimator.getStandardDeviationNorm();
     }
 
     /**
      * Gets norm of estimated standard deviation of gyroscope measurements.
      *
      * @return norm of estimated standard deviation of measurements.
      */
     public AngularSpeed getStandardDeviationAngularSpeedNormAsMeasurement() {
         return angularSpeedEstimator.getStandardDeviationNormAsMeasurement();
     }
 
     /**
      * 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) {
         angularSpeedEstimator.getStandardDeviationNormAsMeasurement(result);
     }
 
     /**
      * 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() {
         return angularSpeedEstimator.getAverageStandardDeviation();
     }
 
     /**
      * Gets average of estimated standard deviation coordinates of gyroscope
      * measurements.
      *
      * @return average of estimated standard deviation coordinates.
      */
     public AngularSpeed getAverageStandardDeviationAngularSpeedAsMeasurement() {
         return angularSpeedEstimator.getAverageStandardDeviationAsMeasurement();
     }
 
     /**
      * 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) {
         angularSpeedEstimator.getAverageStandardDeviationAsMeasurement(result);
     }
 
     /**
      * 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 accelerationEstimator.getPsdX();
     }
 
     /**
      * 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 accelerationEstimator.getPsdY();
     }
 
     /**
      * 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 accelerationEstimator.getPsdZ();
     }
 
     /**
      * 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 angularSpeedEstimator.getPsdX();
     }
 
     /**
      * 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 angularSpeedEstimator.getPsdY();
     }
 
     /**
      * 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 angularSpeedEstimator.getPsdZ();
     }
 
     /**
      * 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 accelerationEstimator.getRootPsdX();
     }
 
     /**
      * 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 accelerationEstimator.getRootPsdY();
     }
 
     /**
      * 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 accelerationEstimator.getRootPsdZ();
     }
 
     /**
      * 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 angularSpeedEstimator.getRootPsdX();
     }
 
     /**
      * 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 angularSpeedEstimator.getRootPsdY();
     }
 
     /**
      * 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 angularSpeedEstimator.getRootPsdZ();
     }
 
     /**
      * 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 accelerationEstimator.getAvgNoisePsd();
     }
 
     /**
      * 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 accelerationEstimator.getNoiseRootPsdNorm();
     }
 
     /**
      * 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 angularSpeedEstimator.getAvgNoisePsd();
     }
 
     /**
      * 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 angularSpeedEstimator.getNoiseRootPsdNorm();
     }
 
     /**
      * Gets number of samples that have been processed so far.
      *
      * @return number of samples that have been processed so far.
      */
     public int getNumberOfProcessedSamples() {
         return accelerationEstimator.getNumberOfProcessedSamples();
     }
 
     /**
      * Indicates whether estimator is currently running or not.
      *
      * @return true if estimator is running, false otherwise.
      */
     public boolean isRunning() {
         return running;
     }
 
     /**
      * Adds body kinematics measurement samples.
      *
      * @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 {
 
         if (running) {
             throw new LockedException();
         }
 
         running = true;
 
         if (lastBodyKinematics == null && listener != null) {
             listener.onStart(this);
         }
 
         if (lastBodyKinematics == null) {
             lastBodyKinematics = new BodyKinematics();
         }
         lastBodyKinematics.setSpecificForceCoordinates(specificForceX, specificForceY, specificForceZ);
         lastBodyKinematics.setAngularRateCoordinates(angularRateX, angularRateY, angularRateZ);
 
         accelerationEstimator.addTriad(specificForceX, specificForceY, specificForceZ);
         angularSpeedEstimator.addTriad(angularRateX, angularRateY, angularRateZ);
 
         if (listener != null) {
             listener.onBodyKinematicsAdded(this);
         }
 
         running = false;
     }
 
     /**
      * Adds body kinematics measurement samples.
      *
      * @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(convertAcceleration(specificForceX),
                 convertAcceleration(specificForceY),
                 convertAcceleration(specificForceZ),
                 convertAngularSpeed(angularRateX),
                 convertAngularSpeed(angularRateY),
                 convertAngularSpeed(angularRateZ));
     }
 
     /**
      * Adds body kinematics measurement samples.
      *
      * @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(convertAcceleration(specificForce.getValueX(), specificForce.getUnit()),
                 convertAcceleration(specificForce.getValueY(), specificForce.getUnit()),
                 convertAcceleration(specificForce.getValueZ(), specificForce.getUnit()),
                 convertAngularSpeed(angularSpeed.getValueX(), angularSpeed.getUnit()),
                 convertAngularSpeed(angularSpeed.getValueY(), angularSpeed.getUnit()),
                 convertAngularSpeed(angularSpeed.getValueZ(), angularSpeed.getUnit()));
     }
 
     /**
      * Adds body kinematics measurement.
      *
      * @param bodyKinematics body kinematics.
      * @throws LockedException if estimator is currently running.
      */
     public void addBodyKinematics(final BodyKinematics bodyKinematics) throws LockedException {
         addBodyKinematics(bodyKinematics.getFx(), bodyKinematics.getFy(), bodyKinematics.getFz(),
                 bodyKinematics.getAngularRateX(), bodyKinematics.getAngularRateY(), bodyKinematics.getAngularRateZ());
     }
 
     /**
      * 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();
         }
 
         running = true;
 
         lastBodyKinematics = null;
         final var result = accelerationEstimator.reset() && angularSpeedEstimator.reset();
 
         if (listener != null) {
             listener.onReset(this);
         }
 
         running = false;
 
         return result;
     }
 
     /**
      * Converts an acceleration instance to meters per squared seconds (m/s^2).
      *
      * @param value value to be converted.
      * @return converted value.
      */
     private double convertAcceleration(final Acceleration value) {
         return AccelerationConverter.convert(value.getValue().doubleValue(), value.getUnit(),
                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Converts an acceleration value and unit to meters per squared seconds (m/s^2).
      *
      * @param value value to be converted.
      * @param unit  unit of value to be converted.
      * @return converted value.
      */
     private double convertAcceleration(final double value, final AccelerationUnit unit) {
         return AccelerationConverter.convert(value, unit, AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Converts an angular speed instance to radians per second (rad/s).
      *
      * @param value value to be converted.
      * @return converted value.
      */
     private double convertAngularSpeed(final AngularSpeed value) {
         return AngularSpeedConverter.convert(value.getValue().doubleValue(), value.getUnit(),
                 AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Converts an angular speed value and unit to radians per second (rad/s).
      *
      * @param value value to be converted.
      * @param unit  unit of value to be converted.
      * @return converted value.
      */
     private double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
         return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * 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();
     }
 }