/*
    * 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.bias;
  
  import com.irurueta.algebra.Matrix;
  import com.irurueta.algebra.WrongSizeException;
  import com.irurueta.geometry.Point3D;
  import com.irurueta.navigation.LockedException;
  import com.irurueta.navigation.frames.CoordinateTransformation;
  import com.irurueta.navigation.frames.ECEFFrame;
  import com.irurueta.navigation.frames.ECEFPosition;
  import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
  import com.irurueta.navigation.frames.NEDFrame;
  import com.irurueta.navigation.frames.NEDPosition;
  import com.irurueta.navigation.frames.converters.ECEFtoNEDFrameConverter;
  import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
  import com.irurueta.navigation.inertial.BodyKinematics;
  import com.irurueta.navigation.inertial.calibration.AccelerationTriad;
  import com.irurueta.navigation.inertial.calibration.AngularSpeedTriad;
  import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
  import com.irurueta.units.Acceleration;
  import com.irurueta.units.AccelerationUnit;
  import com.irurueta.units.Angle;
  import com.irurueta.units.AngularSpeed;
  import com.irurueta.units.AngularSpeedUnit;
  import com.irurueta.units.Distance;
  import com.irurueta.units.Time;
  import com.irurueta.units.TimeConverter;
  import com.irurueta.units.TimeUnit;
  
  /**
   * Approximately estimates accelerometer and gyroscope biases and noise PSD's
   * by averaging all provided samples when body position and orientation is known
   * while assuming that any cross coupling errors can be neglected.
   * <p>
   * This estimator must be used when the body where the accelerometer and gyroscope
   * is attached remains static on the same position with zero velocity and no rotation
   * speed while capturing data.
   * <p>
   * To compute PSD's this estimator assumes that accelerometer samples are obtained
   * at a constant provided rate equal to {@link #getTimeInterval()} seconds.
   * If not available, accelerometer and gyroscope sampling rate average can be
   * estimated using {@link com.irurueta.navigation.inertial.calibration.TimeIntervalEstimator}.
   * <p>
   * Notice that in order to compute accelerometer and gyroscope biases, body position
   * and orientation must be known to account for gravity and Earth rotation effects.
   * <p>
   * Even though this estimator obtains approximate bias values, the obtained
   * result can be used to initialize some non-linear calibrators to obtain
   * more accurate results. Such calibrators are:
   * - com.irurueta.navigation.inertial.calibration.accelerometer.KnownFrameAccelerometerNonLinearLeastSquaresCalibrator
   * - com.irurueta.navigation.inertial.calibration.accelerometer.KnownGravityNormAccelerometerCalibrator
   * - com.irurueta.navigation.inertial.calibration.accelerometer.KnownPositionAccelerometerCalibrator}
   * - com.irurueta.navigation.inertial.calibration.accelerometer.RobustKnownFrameAccelerometerCalibrator and
   * any of its subclasses.
   * - com.irurueta.navigation.inertial.calibration.accelerometer.RobustKnownGravityNormAccelerometerCalibrator
   * and any of its subclasses.
   * - com.irurueta.navigation.inertial.calibration.accelerometer.RobustKnownPositionAccelerometerCalibrator and
   * any of its subclasses.
   * - com.irurueta.navigation.inertial.calibration.gyroscope.KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator
   * - com.irurueta.navigation.inertial.calibration.gyroscope.KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator
   * - com.irurueta.navigation.inertial.calibration.gyroscope.KnownBiasEasyGyroscopeCalibrator
   * - com.irurueta.navigation.inertial.calibration.gyroscope.KnownBiasTurntableGyroscopeCalibrator
   * - com.irurueta.navigation.inertial.calibration.gyroscope.RobustKnownBiasAndFrameGyroscopeCalibrator and any
   * of its subclasses.
   * - com.irurueta.navigation.inertial.calibration.gyroscope.RobustKnownBiasEasyGyroscopeCalibrator and any of
   * its subclasses.
   * - com.irurueta.navigation.inertial.calibration.gyroscope.RobustKnownBiasTurntableGyroscopeCalibrator and any
   * of its subclasses.
   * <p>
   * Even though this estimator can compute noise PSD's, if only noise PSD's levels
   * are required, estimators in {@link com.irurueta.navigation.inertial.calibration.noise} package should
   * be used instead.
   * <p>
   * This estimator does NOT compute average bias values over a period of time, it only
   * computes accumulated averages.
   */
  public class BodyKinematicsBiasEstimator {
  
      /**
       * Default time interval between kinematics samples expressed in seconds (s).
       */
      public static final double DEFAULT_TIME_INTERVAL_SECONDS = 0.02;
  
      /**
       * Time interval expressed in seconds (s) between body kinematics samples.
      */
     private double timeInterval = DEFAULT_TIME_INTERVAL_SECONDS;
 
     /**
      * Contains body position, velocity (which will always be zero) and orientation
      * resolved around ECEF axes.
      * By default it is assumed that body is located at zero NED coordinates (latitude,
      * longitude and height) and with zero Euler angles representing rotation (roll = 0,
      * pith = 0, yaw = 0), which for Android devices it means that the device is flat
      * on a horizontal surface with the screen facing down.
      */
     private final ECEFFrame frame;
 
     /**
      * Listener to handle events raised by this estimator.
      */
     private BodyKinematicsBiasEstimatorListener listener;
 
     /**
      * Last provided body kinematics values.
      */
     private BodyKinematics lastBodyKinematics;
 
     /**
      * Contains estimated bias of x coordinate of accelerometer sensed specific force
      * expressed in meters per squared second (m/s^2).
      */
     private double biasFx;
 
     /**
      * Contains estimated bias of y coordinate of accelerometer sensed specific force
      * expressed in meters per squared second (m/s^2).
      */
     private double biasFy;
 
     /**
      * Contains estimated bias of z coordinate of accelerometer sensed specific force
      * expressed in meters per squared second (m/s^2).
      */
     private double biasFz;
 
     /**
      * Contains estimated bias of x coordinate of gyroscope sensed angular rate
      * expressed in radians per second (rad/s).
      */
     private double biasAngularRateX;
 
     /**
      * Contains estimated bias of y coordinate of gyroscope sensed angular rate
      * expressed in radians per second (rad/s).
      */
     private double biasAngularRateY;
 
     /**
      * Contains estimated bias of z coordinate of gyroscope sensed angular rate
      * expressed in radians per second (rad/s).
      */
     private double biasAngularRateZ;
 
     /**
      * Contains estimated variance of x coordinate of accelerometer sensed specific
      * force expressed in (m^2/s^4).
      */
     private double varianceFx;
 
     /**
      * Contains estimated variance of y coordinate of accelerometer sensed specific
      * force expressed in (m^2/s^4).
      */
     private double varianceFy;
 
     /**
      * Contains estimated variance of z coordinate of accelerometer sensed specific
      * force expressed in (m^2/s4).
      */
     private double varianceFz;
 
     /**
      * Contains estimated variance of x coordinate of gyroscope sensed angular rate
      * expressed in (rad^2/s^2).
      */
     private double varianceAngularRateX;
 
     /**
      * Contains estimated variance of y coordinate of gyroscope sensed angular rate
      * expressed in (rad^2/s^2).
      */
     private double varianceAngularRateY;
 
     /**
      * Contains estimated variance of z coordinate of gyroscope sensed angular rate
      * expressed in (rad^2/s^2).
      */
     private double varianceAngularRateZ;
 
     /**
      * Number of processed body kinematics samples.
      */
     private int numberOfProcessedSamples;
 
     /**
      * Number of processed body kinematics samples plus one.
      */
     private int numberOfProcessedSamplesPlusOne = 1;
 
     /**
      * Indicates that estimator is running.
      */
     private boolean running;
 
     /**
      * Theoretical expected body kinematics for provided body position and orientation,
      * and provided time interval, assuming that body remains at the same position
      * (zero velocity).
      * When body remains static, sensed specific force and angular rates will remain
      * constant due to gravity and Earth rotation.
      */
     private BodyKinematics expectedKinematics;
 
     /**
      * Constructor.
      * It is assumed that body is located at zero NED coordinates (latitude = 0,
      * longitude = 0 and height = 0) and with zero Euler angles representing rotation
      * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
      * device is flat on a horizontal surface with the screen facing down.
      */
     public BodyKinematicsBiasEstimator() {
         frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(new NEDFrame());
         rebuildExpectedKinematics();
     }
 
     /**
      * Constructor.
      * It is assumed that body is located at zero NED coordinates (latitude = 0,
      * longitude = 0, and height = 0) with provided orientation.
      *
      * @param nedC coordinate transformation from body to local navigation
      *             (NED) coordinates. This contains orientation respect the horizon
      *             at current body location.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      */
     public BodyKinematicsBiasEstimator(final CoordinateTransformation nedC)
             throws InvalidSourceAndDestinationFrameTypeException {
         frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(new NEDFrame(nedC));
         rebuildExpectedKinematics();
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude  latitude expressed in radians (rad).
      * @param longitude longitude expressed in radians (rad).
      * @param height    height expressed in meters (m).
      */
     public BodyKinematicsBiasEstimator(final double latitude, final double longitude, final double height) {
         frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(new NEDFrame(latitude, longitude, height));
         rebuildExpectedKinematics();
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude  latitude.
      * @param longitude longitude.
      * @param height    height expressed in meters (m).
      */
     public BodyKinematicsBiasEstimator(final Angle latitude, final Angle longitude, final double height) {
         frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(new NEDFrame(latitude, longitude, height));
         rebuildExpectedKinematics();
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude  latitude.
      * @param longitude longitude.
      * @param height    height.
      */
     public BodyKinematicsBiasEstimator(final Angle latitude, final Angle longitude, final Distance height) {
         frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(new NEDFrame(latitude, longitude, height));
         rebuildExpectedKinematics();
     }
 
     /**
      * Constructor.
      *
      * @param position body position expressed in NED coordinates.
      * @param nedC     coordinate transformation from body to local navigation
      *                 (NED) coordinates. This contains orientation respect the
      *                 horizon at current body location.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      */
     public BodyKinematicsBiasEstimator(final NEDPosition position, final CoordinateTransformation nedC)
             throws InvalidSourceAndDestinationFrameTypeException {
         frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(new NEDFrame(position, nedC));
         rebuildExpectedKinematics();
     }
 
     /**
      * Constructor.
      *
      * @param position body position expressed in ECEF coordinates.
      * @param nedC     coordinate transformation from body to local navigation
      *                 (NED) coordinates. This contains orientation respect the
      *                 horizon at current body location.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      */
     public BodyKinematicsBiasEstimator(final ECEFPosition position, final CoordinateTransformation nedC)
             throws InvalidSourceAndDestinationFrameTypeException {
         frame = new ECEFFrame(position);
         final var nedFrame = ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(frame);
         nedFrame.setCoordinateTransformation(nedC);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Constructor.
      * It is assumed that body is located at zero NED coordinates (latitude = 0,
      * longitude = 0 and height = 0) and with zero Euler angles representing rotation
      * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
      * device is flat on a horizontal surface with the screen facing down.
      *
      * @param listener listener to handle events raised by this estimator.
      */
     public BodyKinematicsBiasEstimator(final BodyKinematicsBiasEstimatorListener listener) {
         this();
         this.listener = listener;
     }
 
     /**
      * Constructor.
      * It is assumed that body is located at zero NED coordinates (latitude = 0,
      * longitude = 0, and height = 0) with provided orientation.
      *
      * @param nedC     coordinate transformation from body to local navigation
      *                 (NED) coordinates. This contains orientation respect the
      *                 horizon at current body location.
      * @param listener listener to handle events raised by this estimator.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      */
     public BodyKinematicsBiasEstimator(
             final CoordinateTransformation nedC, final BodyKinematicsBiasEstimatorListener listener)
             throws InvalidSourceAndDestinationFrameTypeException {
         this(nedC);
         this.listener = listener;
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude  latitude expressed in radians (rad).
      * @param longitude longitude expressed in radians (rad).
      * @param height    height expressed in meters (m).
      * @param listener  listener to handle events raised by this estimator.
      */
     public BodyKinematicsBiasEstimator(
             final double latitude, final double longitude, final double height,
             final BodyKinematicsBiasEstimatorListener listener) {
         this(latitude, longitude, height);
         this.listener = listener;
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude  latitude.
      * @param longitude longitude.
      * @param height    height expressed in meters (m).
      * @param listener  listener to handle events raised by this estimator.
      */
     public BodyKinematicsBiasEstimator(
             final Angle latitude, final Angle longitude, final double height,
             final BodyKinematicsBiasEstimatorListener listener) {
         this(latitude, longitude, height);
         this.listener = listener;
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude  latitude.
      * @param longitude longitude.
      * @param height    height.
      * @param listener  listener to handle events raised by this estimator.
      */
     public BodyKinematicsBiasEstimator(
             final Angle latitude, final Angle longitude, final Distance height,
             final BodyKinematicsBiasEstimatorListener listener) {
         this(latitude, longitude, height);
         this.listener = listener;
     }
 
     /**
      * Constructor.
      *
      * @param position body position expressed in NED coordinates.
      * @param nedC     coordinate transformation from body to local navigation
      *                 (NED) coordinates. This contains orientation respect the
      *                 horizon at current body location.
      * @param listener listener to handle events raised by this estimator.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      */
     public BodyKinematicsBiasEstimator(
             final NEDPosition position, final CoordinateTransformation nedC,
             final BodyKinematicsBiasEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException {
         this(position, nedC);
         this.listener = listener;
     }
 
     /**
      * Constructor.
      *
      * @param position body position expressed in ECEF coordinates.
      * @param nedC     coordinate transformation from body to local navigation
      *                 (NED) coordinates. This contains orientation respect the
      *                 horizon at current body location.
      * @param listener listener to handle events raised by this estimator.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      */
     public BodyKinematicsBiasEstimator(
             final ECEFPosition position, final CoordinateTransformation nedC,
             final BodyKinematicsBiasEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException {
         this(position, nedC);
         this.listener = listener;
     }
 
     /**
      * Constructor.
      * It is assumed that body is located at zero NED coordinates (latitude = 0,
      * longitude = 0 and height = 0) and with zero Euler angles representing rotation
      * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
      * device is flat on a horizontal surface with the screen facing down.
      *
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(final double timeInterval) {
         this();
         try {
             setTimeInterval(timeInterval);
         } catch (final LockedException ignore) {
             // never happens
         }
     }
 
     /**
      * Constructor.
      * It is assumed that body is located at zero NED coordinates (latitude = 0,
      * longitude = 0, and height = 0) with provided orientation.
      *
      * @param nedC         coordinate transformation from body to local navigation
      *                     (NED) coordinates. This contains orientation respect the
      *                     horizon at current body location.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      * @throws IllegalArgumentException                      if provided time interval
      *                                                       is negative.
      */
     public BodyKinematicsBiasEstimator(
             final CoordinateTransformation nedC, final double timeInterval)
             throws InvalidSourceAndDestinationFrameTypeException {
         this(nedC);
         try {
             setTimeInterval(timeInterval);
         } catch (final LockedException ignore) {
             // never happens
         }
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude     latitude expressed in radians (rad).
      * @param longitude    longitude expressed in radians (rad).
      * @param height       height expressed in meters (m).
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(
             final double latitude, final double longitude, final double height, final double timeInterval) {
         this(latitude, longitude, height);
         try {
             setTimeInterval(timeInterval);
         } catch (final LockedException ignore) {
             // never happens
         }
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude     latitude.
      * @param longitude    longitude.
      * @param height       height expressed in meters (m).
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(
             final Angle latitude, final Angle longitude, final double height, final double timeInterval) {
         this(latitude, longitude, height);
         try {
             setTimeInterval(timeInterval);
         } catch (final LockedException ignore) {
             // never happens
         }
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude     latitude.
      * @param longitude    longitude.
      * @param height       height.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(
             final Angle latitude, final Angle longitude, final Distance height, final double timeInterval) {
         this(latitude, longitude, height);
         try {
             setTimeInterval(timeInterval);
         } catch (final LockedException ignore) {
             // never happens
         }
     }
 
     /**
      * Constructor.
      *
      * @param position     body position expressed in NED coordinates.
      * @param nedC         coordinate transformation from body to local navigation
      *                     (NED) coordinates. This contains orientation respect the
      *                     horizon at current body location.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      * @throws IllegalArgumentException                      if provided time interval
      *                                                       is negative.
      */
     public BodyKinematicsBiasEstimator(
             final NEDPosition position, final CoordinateTransformation nedC, final double timeInterval)
             throws InvalidSourceAndDestinationFrameTypeException {
         this(position, nedC);
         try {
             setTimeInterval(timeInterval);
         } catch (final LockedException ignore) {
             // never happens
         }
     }
 
     /**
      * Constructor.
      *
      * @param position     body position expressed in ECEF coordinates.
      * @param nedC         coordinate transformation from body to local navigation
      *                     (NED) coordinates. This contains orientation respect the
      *                     horizon at current body location.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      * @throws IllegalArgumentException                      if provided time interval
      *                                                       is negative.
      */
     public BodyKinematicsBiasEstimator(
             final ECEFPosition position, final CoordinateTransformation nedC, final double timeInterval)
             throws InvalidSourceAndDestinationFrameTypeException {
         this(position, nedC);
         try {
             setTimeInterval(timeInterval);
         } catch (final LockedException ignore) {
             // never happens
         }
     }
 
     /**
      * Constructor.
      * It is assumed that body is located at zero NED coordinates (latitude = 0,
      * longitude = 0 and height = 0) and with zero Euler angles representing rotation
      * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
      * device is flat on a horizontal surface with the screen facing down.
      *
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @param listener     listener to handle events raised by this estimator.
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(final double timeInterval, final BodyKinematicsBiasEstimatorListener listener) {
         this(timeInterval);
         this.listener = listener;
     }
 
     /**
      * Constructor.
      * It is assumed that body is located at zero NED coordinates (latitude = 0,
      * longitude = 0, and height = 0) with provided orientation.
      *
      * @param nedC         coordinate transformation from body to local navigation
      *                     (NED) coordinates. This contains orientation respect the
      *                     horizon at current body location.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @param listener     listener to handle events raised by this estimator.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      * @throws IllegalArgumentException                      if provided time interval
      *                                                       is negative.
      */
     public BodyKinematicsBiasEstimator(
             final CoordinateTransformation nedC, final double timeInterval,
             final BodyKinematicsBiasEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException {
         this(nedC, timeInterval);
         this.listener = listener;
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude     latitude expressed in radians (rad).
      * @param longitude    longitude expressed in radians (rad).
      * @param height       height expressed in meters (m).
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @param listener     listener to handle events raised by this estimator.
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(
             final double latitude, final double longitude, final double height,
             final double timeInterval, final BodyKinematicsBiasEstimatorListener listener) {
         this(latitude, longitude, height, timeInterval);
         this.listener = listener;
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude     latitude.
      * @param longitude    longitude.
      * @param height       height expressed in meters (m).
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @param listener     listener to handle events raised by this estimator.
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(
             final Angle latitude, final Angle longitude, final double height,
             final double timeInterval, final BodyKinematicsBiasEstimatorListener listener) {
         this(latitude, longitude, height, timeInterval);
         this.listener = listener;
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude     latitude.
      * @param longitude    longitude.
      * @param height       height.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @param listener     listener to handle events raised by this estimator.
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(
             final Angle latitude, final Angle longitude, final Distance height,
             final double timeInterval, final BodyKinematicsBiasEstimatorListener listener) {
         this(latitude, longitude, height, timeInterval);
         this.listener = listener;
     }
 
     /**
      * Constructor.
      *
      * @param position     body position expressed in NED coordinates.
      * @param nedC         coordinate transformation from body to local navigation
      *                     (NED) coordinates. This contains orientation respect the
      *                     horizon at current body location.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @param listener     listener to handle events raised by this estimator.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      * @throws IllegalArgumentException                      if provided time interval
      *                                                       is negative.
      */
     public BodyKinematicsBiasEstimator(
             final NEDPosition position, final CoordinateTransformation nedC,
             final double timeInterval, final BodyKinematicsBiasEstimatorListener listener)
             throws InvalidSourceAndDestinationFrameTypeException {
         this(position, nedC, timeInterval);
         this.listener = listener;
     }
 
     /**
      * Constructor.
      *
      * @param position     body position expressed in ECEF coordinates.
      * @param nedC         coordinate transformation from body to local navigation
      *                     (NED) coordinates. This contains orientation respect the
      *                     horizon at current body location.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples
      *                     expressed in seconds (s).
      * @param listener     listener to handle events raised by this estimator.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      * @throws IllegalArgumentException                      if provided time interval
      *                                                       is negative.
      */
     public BodyKinematicsBiasEstimator(
             final ECEFPosition position, final CoordinateTransformation nedC,
             final double timeInterval, final BodyKinematicsBiasEstimatorListener listener)
             throws InvalidSourceAndDestinationFrameTypeException {
         this(position, nedC, timeInterval);
         this.listener = listener;
     }
 
     /**
      * Constructor.
      * It is assumed that body is located at zero NED coordinates (latitude = 0,
      * longitude = 0 and height = 0) and with zero Euler angles representing rotation
      * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
      * device is flat on a horizontal surface with the screen facing down.
      *
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(final Time timeInterval) {
         this(convertTime(timeInterval));
     }
 
     /**
      * Constructor.
      * It is assumed that body is located at zero NED coordinates (latitude = 0,
      * longitude = 0, and height = 0) with provided orientation.
      *
      * @param nedC         coordinate transformation from body to local navigation
      *                     (NED) coordinates. This contains orientation respect the
      *                     horizon at current body location.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      * @throws IllegalArgumentException                      if provided time interval
      *                                                       is negative.
      */
     public BodyKinematicsBiasEstimator(final CoordinateTransformation nedC, final Time timeInterval)
             throws InvalidSourceAndDestinationFrameTypeException {
         this(nedC, convertTime(timeInterval));
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude     latitude expressed in radians (rad).
      * @param longitude    longitude expressed in radians (rad).
      * @param height       height expressed in meters (m).
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(
             final double latitude, final double longitude, final double height, final Time timeInterval) {
         this(latitude, longitude, height, convertTime(timeInterval));
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude     latitude.
      * @param longitude    longitude.
      * @param height       height expressed in meters (m).
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(
             final Angle latitude, final Angle longitude, final double height, final Time timeInterval) {
         this(latitude, longitude, height, convertTime(timeInterval));
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude     latitude.
      * @param longitude    longitude.
      * @param height       height.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(
             final Angle latitude, final Angle longitude, final Distance height, final Time timeInterval) {
         this(latitude, longitude, height, convertTime(timeInterval));
     }
 
     /**
      * Constructor.
      *
      * @param position     body position expressed in NED coordinates.
      * @param nedC         coordinate transformation from body to local navigation
      *                     (NED) coordinates. This contains orientation respect the
      *                     horizon at current body location.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      * @throws IllegalArgumentException                      if provided time interval
      *                                                       is negative.
      */
     public BodyKinematicsBiasEstimator(
             final NEDPosition position, final CoordinateTransformation nedC, final Time timeInterval)
             throws InvalidSourceAndDestinationFrameTypeException {
         this(position, nedC, convertTime(timeInterval));
     }
 
     /**
      * Constructor.
      *
      * @param position     body position expressed in ECEF coordinates.
      * @param nedC         coordinate transformation from body to local navigation
      *                     (NED) coordinates. This contains orientation respect the
      *                     horizon at current body location.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      * @throws IllegalArgumentException                      if provided time interval
      *                                                       is negative.
      */
     public BodyKinematicsBiasEstimator(
             final ECEFPosition position, final CoordinateTransformation nedC, final Time timeInterval)
             throws InvalidSourceAndDestinationFrameTypeException {
         this(position, nedC, convertTime(timeInterval));
     }
 
     /**
      * Constructor.
      * It is assumed that body is located at zero NED coordinates (latitude = 0,
      * longitude = 0 and height = 0) and with zero Euler angles representing rotation
      * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
      * device is flat on a horizontal surface with the screen facing down.
      *
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @param listener     listener to handle events raised by this estimator.
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(final Time timeInterval, final BodyKinematicsBiasEstimatorListener listener) {
         this(convertTime(timeInterval), listener);
     }
 
     /**
      * Constructor.
      * It is assumed that body is located at zero NED coordinates (latitude = 0,
      * longitude = 0, and height = 0) with provided orientation.
      *
      * @param nedC         coordinate transformation from body to local navigation
      *                     (NED) coordinates. This contains orientation respect the
      *                     horizon at current body location.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @param listener     listener to handle events raised by this estimator.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      * @throws IllegalArgumentException                      if provided time interval
      *                                                       is negative.
      */
     public BodyKinematicsBiasEstimator(
             final CoordinateTransformation nedC, final Time timeInterval,
             final BodyKinematicsBiasEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException {
         this(nedC, convertTime(timeInterval), listener);
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude     latitude expressed in radians (rad).
      * @param longitude    longitude expressed in radians (rad).
      * @param height       height expressed in meters (m).
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @param listener     listener to handle events raised by this estimator.
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(
             final double latitude, final double longitude, final double height,
             final Time timeInterval, final BodyKinematicsBiasEstimatorListener listener) {
         this(latitude, longitude, height, convertTime(timeInterval), listener);
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude     latitude.
      * @param longitude    longitude.
      * @param height       height expressed in meters (m).
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @param listener     listener to handle events raised by this estimator.
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(
             final Angle latitude, final Angle longitude, final double height, final Time timeInterval,
             final BodyKinematicsBiasEstimatorListener listener) {
         this(latitude, longitude, height, convertTime(timeInterval), listener);
     }
 
     /**
      * Constructor.
      * It is assumed that body has zero Euler angles representing rotation (roll = 0,
      * pitch = 0, yaw = 0) respect the horizon at provided body location.
      * For Android devices this means that the device is flat on a horizontal surface
      * with the screen facing down.
      *
      * @param latitude     latitude.
      * @param longitude    longitude.
      * @param height       height.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @param listener     listener to handle events raised by this estimator.
      * @throws IllegalArgumentException if provided time interval is negative.
      */
     public BodyKinematicsBiasEstimator(
             final Angle latitude, final Angle longitude, final Distance height,
             final Time timeInterval, final BodyKinematicsBiasEstimatorListener listener) {
         this(latitude, longitude, height, convertTime(timeInterval), listener);
     }
 
     /**
      * Constructor.
      *
      * @param position     body position expressed in NED coordinates.
      * @param nedC         coordinate transformation from body to local navigation
      *                     (NED) coordinates. This contains orientation respect the
      *                     horizon at current body location.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @param listener     listener to handle events raised by this estimator.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      * @throws IllegalArgumentException                      if provided time interval
      *                                                       is negative.
      */
     public BodyKinematicsBiasEstimator(
             final NEDPosition position, final CoordinateTransformation nedC,
             final Time timeInterval, final BodyKinematicsBiasEstimatorListener listener)
             throws InvalidSourceAndDestinationFrameTypeException {
         this(position, nedC, convertTime(timeInterval), listener);
     }
 
     /**
      * Constructor.
      *
      * @param position     body position expressed in ECEF coordinates.
      * @param nedC         coordinate transformation from body to local navigation
      *                     (NED) coordinates. This contains orientation respect the
      *                     horizon at current body location.
      * @param timeInterval time interval between body kinematics
      *                     (IMU acceleration + gyroscope) samples.
      * @param listener     listener to handle events raised by this estimator.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not
      *                                                       from body to local
      *                                                       navigation coordinates.
      * @throws IllegalArgumentException                      if provided time interval
      *                                                       is negative.
      */
     public BodyKinematicsBiasEstimator(
             final ECEFPosition position, final CoordinateTransformation nedC,
             final Time timeInterval, final BodyKinematicsBiasEstimatorListener listener)
             throws InvalidSourceAndDestinationFrameTypeException {
         this(position, nedC, convertTime(timeInterval), listener);
     }
 
     /**
      * Gets time interval between body kinematics (IMU acceleration + gyroscope)
      * samples expressed in seconds (s).
      *
      * @return time interval between body kinematics samples.
      */
     public double getTimeInterval() {
         return timeInterval;
     }
 
     /**
      * Sets time interval between body kinematics (IMU acceleration + gyroscope)
      * samples expressed in seconds (s).
      *
      * @param timeInterval time interval between body kinematics samples.
      * @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;
 
         rebuildExpectedKinematics();
     }
 
     /**
      * Gets time interval between body kinematics (IMU acceleration + gyroscope)
      * samples.
      *
      * @return time interval between body kinematics samples.
      */
     public Time getTimeIntervalAsTime() {
         return new Time(timeInterval, TimeUnit.SECOND);
     }
 
     /**
      * Gets time interval between body kinematics (IMU acceleration + gyroscope)
      * 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 (IMU acceleration + gyroscope)
      * samples.
      *
      * @param timeInterval time interval between body kinematics samples.
      * @throws LockedException if estimator is currently running.
      */
     public void setTimeInterval(final Time timeInterval) throws LockedException {
         setTimeInterval(convertTime(timeInterval));
     }
 
     /**
      * Gets current body position expressed in ECEF coordinates.
      *
      * @return current body position expressed in ECEF coordinates.
      */
     public ECEFPosition getEcefPosition() {
         return frame.getECEFPosition();
     }
 
     /**
      * Gets current body position expressed in ECEF coordinates.
      *
      * @param result instance where current body position will be stored.
      */
     public void getEcefPosition(final ECEFPosition result) {
         frame.getECEFPosition(result);
     }
 
     /**
      * Sets current body position expressed in ECEF coordinates.
      *
      * @param position current body position to be set.
      * @throws LockedException if estimator is currently running.
      */
     public void setEcefPosition(final ECEFPosition position) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         frame.setPosition(position);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets current body position expressed in ECEF coordinates.
      *
      * @param x x position resolved around ECEF axes and expressed in meters (m).
      * @param y y position resolved around ECEF axes and expressed in meters (m).
      * @param z z position resolved around ECEF axes and expressed in meters (m).
      * @throws LockedException if estimator is currently running.
      */
     public void setEcefPosition(final double x, final double y, final double z) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         frame.setCoordinates(x, y, z);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets current body position expressed in ECEF coordinates.
      *
      * @param x x position resolved around ECEF axes.
      * @param y y position resolved around ECEF axes.
      * @param z z position resolved around ECEF axes.
      * @throws LockedException if estimator is currently running.
      */
     public void setEcefPosition(final Distance x, final Distance y, final Distance z) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         frame.setPositionCoordinates(x, y, z);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets current body position expressed in ECEF coordinates.
      *
      * @param position position resolved around ECEF axes and expressed in meters (m).
      * @throws LockedException if estimator is currently running.
      */
     public void setEcefPosition(final Point3D position) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         frame.setPosition(position);
         rebuildExpectedKinematics();
     }
 
     /**
      * Gets ECEF frame containing current body position and orientation expressed
      * in ECEF coordinates. Frame also contains body velocity, but it is always
      * assumed to be zero during calibration.
      *
      * @return ECEF frame containing current body position and orientation resolved
      * around ECEF axes.
      */
     public ECEFFrame getEcefFrame() {
         return new ECEFFrame(frame);
     }
 
     /**
      * Gets ECEF frame containing current body position and orientation expressed
      * in ECEF coordinates. Frame also contains body velocity, but it is always
      * assumed to be zero during calibration.
      *
      * @param result instance where ECEF frame containing current body position and
      *               orientation resolved around ECEF axes will be stored.
      */
     public void getEcefFrame(final ECEFFrame result) {
         frame.copyTo(result);
     }
 
     /**
      * Gets NED frame containing current body position and orientation expressed
      * in NED coordinates. Frame also contains body velocity, but it is always
      * assumed to be zero during calibration.
      *
      * @return NED frame containing current body position and orientation resolved
      * around NED axes.
      */
     public NEDFrame getNedFrame() {
         return ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(frame);
     }
 
     /**
      * Gets NED frame containing current body position and orientation expressed
      * in NED coordinates. Frame also contains body velocity, but it is always
      * assumed to be zero during calibration.
      *
      * @param result instance where NED frame containing current body position and
      *               orientation resolved around NED axes will be stored.
      */
     public void getNedFrame(final NEDFrame result) {
         ECEFtoNEDFrameConverter.convertECEFtoNED(frame, result);
     }
 
     /**
      * Gets current body position expressed in NED coordinates.
      *
      * @return current body position expressed in NED coordinates.
      */
     public NEDPosition getNedPosition() {
         return getNedFrame().getPosition();
     }
 
     /**
      * Gets current body position expressed in NED coordinates.
      *
      * @param result instance where current body position will be stored.
      */
     public void getNedPosition(final NEDPosition result) {
         getNedFrame().getPosition(result);
     }
 
     /**
      * Sets current body position expressed in NED coordinates.
      *
      * @param position current body position to be set.
      * @throws LockedException if estimator is currently running.
      */
     public void setNedPosition(final NEDPosition position) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         final var nedFrame = getNedFrame();
         nedFrame.setPosition(position);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets current body position expressed in NED coordinates.
      *
      * @param latitude  latitude NED coordinate expressed in radians (rad).
      * @param longitude longitude NED coordinate expressed in radians (rad).
      * @param height    height NED coordinate expressed in meters (m).
      * @throws LockedException if estimator is currently running.
      */
     public void setNedPosition(
             final double latitude, final double longitude, final double height) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         final var nedFrame = getNedFrame();
         nedFrame.setPosition(latitude, longitude, height);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets current body position expressed in NED coordinates.
      *
      * @param latitude  latitude NED coordinate.
      * @param longitude longitude NED coordinate.
      * @param height    height NED coordinate expressed in meters (m).
      * @throws LockedException if estimator is currently running.
      */
     public void setNedPosition(
             final Angle latitude, final Angle longitude, final double height) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         final var nedFrame = getNedFrame();
         nedFrame.setPosition(latitude, longitude, height);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets current body position expressed in NED coordinates.
      *
      * @param latitude  latitude NED coordinate.
      * @param longitude longitude NED coordinate.
      * @param height    height NED coordinate.
      * @throws LockedException if estimator is currently running.
      */
     public void setNedPosition(
             final Angle latitude, final Angle longitude, final Distance height) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         final var nedFrame = getNedFrame();
         nedFrame.setPosition(latitude, longitude, height);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Gets current body orientation as a transformation from body to ECEF coordinates.
      * Notice that returned orientation refers to ECEF Earth axes, which means that
      * orientation is not relative to the ground or horizon at current body position.
      * Typically it is more convenient to use {@link #getNedC()} to obtain orientation
      * relative to the ground or horizon at current body position. For instance, on
      * Android devices a NED orientation with Euler angles (roll = 0, pitch = 0,
      * yaw = 0) means that the device is laying flat on a horizontal surface with the
      * screen facing down towards the ground.
      *
      * @return current body orientation resolved on ECEF axes.
      */
     public CoordinateTransformation getEcefC() {
         return frame.getCoordinateTransformation();
     }
 
     /**
      * Gets current body orientation as a transformation from body to ECEF coordinates.
      * Notice that returned orientation refers to ECEF Earth axes, which means that
      * orientation is not relative to the ground or horizon at current body position.
      * Typically it is more convenient to use {@link #getNedC()} to obtain orientation
      * relative to the ground or horizon at current body position. For instance, on
      * Android devices a NED orientation with Euler angles (roll = 0, pitch = 0,
      * yaw = 0) means that the device is laying flat on a horizontal surface with the
      * screen facing down towards the ground.
      *
      * @param result instance where current body orientation resolved on ECEF axes
      *               will be stored.
      */
     public void getEcefC(final CoordinateTransformation result) {
         frame.getCoordinateTransformation(result);
     }
 
     /**
      * Sets current body orientation as a transformation from body to ECEF coordinates.
      * Notice that ECEF orientation refers to ECEF Earth axes, which means that
      * orientation is not relative to the ground or horizon at current body position.
      * Typically it is more convenient to use
      * {@link #setNedC(CoordinateTransformation)} to specify orientation relative to
      * the ground or horizon at current body position.
      * For instance, on Android devices a NED orientation with Euler angles (roll = 0,
      * pitch = 0, yaw = 0) means that the device is laying flat on a horizontal surface
      * with the screen facing down towards the ground.
      *
      * @param ecefC body orientation resolved on ECEF axes to be set.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to ECEF coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      */
     public void setEcefC(final CoordinateTransformation ecefC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         frame.setCoordinateTransformation(ecefC);
         rebuildExpectedKinematics();
     }
 
     /**
      * Gets current body orientation as a transformation from body to NED coordinates.
      * Notice that returned orientation refers to current local position. This means
      * that two equal NED orientations will transform into different ECEF orientations
      * if the body is located at different positions.
      * As a reference, on Android devices a NED orientation with Euler angles
      * (roll = 0, pitch = 0, yaw = 0) means that the device is laying flat on a
      * horizontal surface with the screen facing down towards the ground.
      *
      * @return current body orientation resolved on NED axes.
      */
     public CoordinateTransformation getNedC() {
         return getNedFrame().getCoordinateTransformation();
     }
 
     /**
      * Gets current body orientation as a transformation from body to NED coordinates.
      * Notice that returned orientation refers to current local position. This means
      * that two equal NED orientations will transform into different ECEF orientations
      * if the body is located at different positions.
      * As a reference, on Android devices a NED orientation with Euler angles
      * (roll = 0, pitch = 0, yaw = 0) means that the device is laying flat on a
      * horizontal surface with the screen facing down towards the ground.
      *
      * @param result instance where current body orientation resolved on NED axes
      *               will be stored.
      */
     public void getNedC(final CoordinateTransformation result) {
         getNedFrame().getCoordinateTransformation(result);
     }
 
     /**
      * Sets current body orientation as a transformation from body to NED coordinates.
      * Notice that provided orientation refers to current local position. This means
      * that two equal NED orientations will transform into different ECEF orientations
      * if the body is located at different positions.
      * As a reference, on Android devices a NED orientation with Euler angles
      * (roll = 0, pitch = 0, yaw = 0) means that the device is laying flat on a
      * horizontal surface with the screen facing down towards the ground.
      *
      * @param nedC orientation resolved on NED axes to be set.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to NED coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      */
     public void setNedC(final CoordinateTransformation nedC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         final var nedFrame = getNedFrame();
         nedFrame.setCoordinateTransformation(nedC);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position and orientation both expressed on NED coordinates.
      *
      * @param nedPosition position expressed on NED coordinates.
      * @param nedC        body to NED coordinate transformation indicating
      *                    body orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to NED coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setNedPosition(NEDPosition)
      * @see #setNedC(CoordinateTransformation)
      */
     public void setNedPositionAndNedOrientation(
             final NEDPosition nedPosition, final CoordinateTransformation nedC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         final var nedFrame = getNedFrame();
         nedFrame.setPosition(nedPosition);
         nedFrame.setCoordinateTransformation(nedC);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position and orientation both expressed on NED coordinates.
      *
      * @param latitude  latitude expressed in radians (rad).
      * @param longitude longitude expressed in radians (rad).
      * @param height    height expressed in meters (m).
      * @param nedC      body to NED coordinate transformation indicating
      *                  body orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to NED coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setNedPosition(double, double, double)
      * @see #setNedC(CoordinateTransformation)
      */
     public void setNedPositionAndNedOrientation(
             final double latitude, final double longitude, final double height,
             final CoordinateTransformation nedC) throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         final var nedFrame = getNedFrame();
         nedFrame.setPosition(latitude, longitude, height);
         nedFrame.setCoordinateTransformation(nedC);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position and orientation both expressed on NED coordinates.
      *
      * @param latitude  latitude.
      * @param longitude longitude.
      * @param height    height expressed in meters (m).
      * @param nedC      body to NED coordinate transformation indicating
      *                  body orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to NED coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setNedPosition(Angle, Angle, double)
      * @see #setNedC(CoordinateTransformation)
      */
     public void setNedPositionAndNedOrientation(
             final Angle latitude, final Angle longitude, final double height,
             final CoordinateTransformation nedC) throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         final var nedFrame = getNedFrame();
         nedFrame.setPosition(latitude, longitude, height);
         nedFrame.setCoordinateTransformation(nedC);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position and orientation both expressed on NED coordinates.
      *
      * @param latitude  latitude.
      * @param longitude longitude.
      * @param height    height.
      * @param nedC      body to NED coordinate transformation indicating
      *                  body orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to NED coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setNedPosition(Angle, Angle, Distance)
      * @see #setNedC(CoordinateTransformation)
      */
     public void setNedPositionAndNedOrientation(
             final Angle latitude, final Angle longitude, final Distance height,
             final CoordinateTransformation nedC) throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         final var nedFrame = getNedFrame();
         nedFrame.setPosition(latitude, longitude, height);
         nedFrame.setCoordinateTransformation(nedC);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position and orientation both expressed on ECEF coordinates.
      *
      * @param ecefPosition position expressed on ECEF coordinates.
      * @param ecefC        body to ECEF coordinate transformation indicating body
      *                     orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to ECEF coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setEcefPosition(ECEFPosition)
      * @see #setEcefC(CoordinateTransformation)
      */
     public void setEcefPositionAndEcefOrientation(
             final ECEFPosition ecefPosition, final CoordinateTransformation ecefC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         frame.setPosition(ecefPosition);
         frame.setCoordinateTransformation(ecefC);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position and orientation both expressed on ECEF coordinates.
      *
      * @param x     x coordinate of ECEF position expressed in meters (m).
      * @param y     y coordinate of ECEF position expressed in meters (m).
      * @param z     z coordinate of ECEF position expressed in meters (m).
      * @param ecefC body to ECEF coordinate transformation indicating body
      *              orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to ECEF coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setEcefPosition(double, double, double)
      * @see #setEcefC(CoordinateTransformation)
      */
     public void setEcefPositionAndEcefOrientation(
             final double x, final double y, final double z,
             final CoordinateTransformation ecefC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         frame.setCoordinates(x, y, z);
         frame.setCoordinateTransformation(ecefC);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position and orientation both expressed on ECEF coordinates.
      *
      * @param x     x coordinate of ECEF position.
      * @param y     y coordinate of ECEF position.
      * @param z     z coordinate of ECEF position.
      * @param ecefC body to ECEF coordinate transformation indicating body
      *              orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to ECEF coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setEcefPosition(Distance, Distance, Distance)
      * @see #setEcefC(CoordinateTransformation)
      */
     public void setEcefPositionAndEcefOrientation(
             final Distance x, final Distance y, final Distance z,
             final CoordinateTransformation ecefC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         frame.setPositionCoordinates(x, y, z);
         frame.setCoordinateTransformation(ecefC);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position and orientation both expressed on ECEF coordinates.
      *
      * @param position position resolved around ECEF axes and expressed in meters (m).
      * @param ecefC    body to ECEF coordinate transformation indicating body
      *                 orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to ECEF coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setEcefPosition(Point3D)
      * @see #setEcefC(CoordinateTransformation)
      */
     public void setEcefPositionAndEcefOrientation(
             final Point3D position, final CoordinateTransformation ecefC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         frame.setPosition(position);
         frame.setCoordinateTransformation(ecefC);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position expressed on NED coordinates and orientation respect to ECEF
      * axes.
      *
      * @param position position expressed on NED coordinates.
      * @param ecefC    body to ECEF coordinate transformation indicating body
      *                 orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to ECEF coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setNedPosition(NEDPosition)
      * @see #setEcefC(CoordinateTransformation)
      */
     public void setNedPositionAndEcefOrientation(
             final NEDPosition position,
             final CoordinateTransformation ecefC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         final var nedFrame = getNedFrame();
         nedFrame.setPosition(position);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         frame.setCoordinateTransformation(ecefC);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position expressed on NED coordinates and orientation respect to ECEF
      * axes.
      *
      * @param latitude  latitude expressed in radians (rad).
      * @param longitude longitude expressed in radians (rad).
      * @param height    height expressed in meters (m).
      * @param ecefC     body to ECEF coordinate transformation indicating body
      *                  orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to ECEF coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setNedPosition(double, double, double)
      * @see #setEcefC(CoordinateTransformation)
      */
     public void setNedPositionAndEcefOrientation(
             final double latitude, final double longitude, final double height,
             final CoordinateTransformation ecefC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         final var nedFrame = getNedFrame();
         nedFrame.setPosition(latitude, longitude, height);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         frame.setCoordinateTransformation(ecefC);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position expressed on NED coordinates and orientation respect to ECEF
      * axes.
      *
      * @param latitude  latitude.
      * @param longitude longitude.
      * @param height    height expressed in meters (m).
      * @param ecefC     body to ECEF coordinate transformation indicating body
      *                  orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to ECEF coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setNedPosition(Angle, Angle, double)
      * @see #setEcefC(CoordinateTransformation)
      */
     public void setNedPositionAndEcefOrientation(
             final Angle latitude, final Angle longitude, final double height,
             final CoordinateTransformation ecefC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         final var nedFrame = getNedFrame();
         nedFrame.setPosition(latitude, longitude, height);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         frame.setCoordinateTransformation(ecefC);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position expressed on NED coordinates and orientation respect to ECEF
      * axes.
      *
      * @param latitude  latitude.
      * @param longitude longitude.
      * @param height    height.
      * @param ecefC     body to ECEF coordinate transformation indicating body
      *                  orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to ECEF coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setNedPosition(Angle, Angle, Distance)
      * @see #setEcefC(CoordinateTransformation)
      */
     public void setNedPositionAndEcefOrientation(
             final Angle latitude, final Angle longitude, final Distance height,
             final CoordinateTransformation ecefC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         final var nedFrame = getNedFrame();
         nedFrame.setPosition(latitude, longitude, height);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         frame.setCoordinateTransformation(ecefC);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position expressed on ECEF coordinates and orientation respect to
      * NED axes.
      * In order to preserve provided orientation, first position is set and
      * then orientation is applied.
      *
      * @param ecefPosition position expressed on ECEF coordinates.
      * @param nedC         body to NED coordinate transformation indicating body
      *                     orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to NED coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setEcefPosition(ECEFPosition)
      * @see #setNedC(CoordinateTransformation)
      */
     public void setEcefPositionAndNedOrientation(
             final ECEFPosition ecefPosition, final CoordinateTransformation nedC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         frame.setPosition(ecefPosition);
 
         final var nedFrame = getNedFrame();
         nedFrame.setCoordinateTransformation(nedC);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position expressed on ECEF coordinates and orientation respect to
      * NED axes.
      * In order to preserve provided orientation, first position is set and
      * then orientation is applied.
      *
      * @param x    x coordinate of ECEF position expressed in meters (m).
      * @param y    y coordinate of ECEF position expressed in meters (m).
      * @param z    z coordinate of ECEF position expressed in meters (m).
      * @param nedC body to NED coordinate transformation indicating body
      *             orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to NED coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setEcefPosition(double, double, double)
      * @see #setNedC(CoordinateTransformation)
      */
     public void setEcefPositionAndNedOrientation(
             final double x, final double y, final double z, final CoordinateTransformation nedC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         frame.setCoordinates(x, y, z);
 
         final var nedFrame = getNedFrame();
         nedFrame.setCoordinateTransformation(nedC);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position expressed on ECEF coordinates and orientation respect to
      * NED axes.
      * In order to preserve provided orientation, first position is set and
      * then orientation is applied.
      *
      * @param x    x coordinate of ECEF position.
      * @param y    y coordinate of ECEF position.
      * @param z    z coordinate of ECEF position.
      * @param nedC body to NED coordinate transformation indicating body
      *             orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to NED coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setEcefPosition(Distance, Distance, Distance)
      * @see #setNedC(CoordinateTransformation)
      */
     public void setEcefPositionAndNedOrientation(
             final Distance x, final Distance y, final Distance z, final CoordinateTransformation nedC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         frame.setPositionCoordinates(x, y, z);
 
         final var nedFrame = getNedFrame();
         nedFrame.setCoordinateTransformation(nedC);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Sets position expressed on ECEF coordinates and orientation respect to
      * NED axes.
      * In order to preserve provided orientation, first position is set and
      * then orientation is applied.
      *
      * @param position position resolved around ECEF axes and expressed in meters (m).
      * @param nedC     body to NED coordinate transformation indicating body
      *                 orientation.
      * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
      *                                                       transformation is not from
      *                                                       body to NED coordinates.
      * @throws LockedException                               if estimator is currently
      *                                                       running.
      * @see #setEcefPosition(Point3D)
      * @see #setNedC(CoordinateTransformation)
      */
     public void setEcefPositionAndNedOrientation(
             final Point3D position, final CoordinateTransformation nedC)
             throws InvalidSourceAndDestinationFrameTypeException, LockedException {
         if (running) {
             throw new LockedException();
         }
 
         frame.setPosition(position);
 
         final var nedFrame = getNedFrame();
         nedFrame.setCoordinateTransformation(nedC);
         NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
         rebuildExpectedKinematics();
     }
 
     /**
      * Gets listener to handle events raised by this estimator.
      *
      * @return listener to handle events raised by this estimator.
      */
     public BodyKinematicsBiasEstimatorListener 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 BodyKinematicsBiasEstimatorListener listener) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         this.listener = listener;
     }
 
     /**
      * Gets last provided body kinematics values or null if not available.
      *
      * @return last provided body kinematics values or null.
      */
     public BodyKinematics getLastBodyKinematics() {
         return lastBodyKinematics != null ? new BodyKinematics(lastBodyKinematics) : null;
     }
 
     /**
      * Gets last provided body kinematics values.
      *
      * @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 bias of x coordinate of accelerometer sensed specific force
      * expressed in meters per squared second (m/s^2).
      *
      * @return bias of x coordinate of sensed specific force.
      */
     public double getBiasFx() {
         return biasFx;
     }
 
     /**
      * Gets estimated bias of x coordinate of accelerometer sensed specific force.
      *
      * @return bias of x coordinate of sensed specific force.
      */
     public Acceleration getBiasFxAsAcceleration() {
         return new Acceleration(biasFx, AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated bias of x coordinate of accelerometer sensed specific force.
      *
      * @param result instance where bias of x coordinate of sensed specific force
      *               will be stored.
      */
     public void getBiasFxAsAcceleration(final Acceleration result) {
         result.setValue(biasFx);
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated bias of x coordinate of accelerometer sensed specific force
      * expressed in meters per squared second (m/s^2).
      *
      * @return bias of y coordinate of sensed specific force.
      */
     public double getBiasFy() {
         return biasFy;
     }
 
     /**
      * Gets estimated bias of y coordinate of accelerometer sensed specific force.
      *
      * @return bias of y coordinate of sensed specific force.
      */
     public Acceleration getBiasFyAsAcceleration() {
         return new Acceleration(biasFy, AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated bias of y coordinate of accelerometer sensed specific force.
      *
      * @param result instance where bias of y coordinate of sensed specific force
      *               will be stored.
      */
     public void getBiasFyAsAcceleration(final Acceleration result) {
         result.setValue(biasFy);
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated bias of z coordinate of accelerometer sensed specific force
      * expressed in meters per squared second (m/s^2).
      *
      * @return bias of z coordinate of sensed specific force.
      */
     public double getBiasFz() {
         return biasFz;
     }
 
     /**
      * Gets estimated bias of z coordinate of accelerometer sensed specific force.
      *
      * @return bias of z coordinate of sensed specific force.
      */
     public Acceleration getBiasFzAsAcceleration() {
         return new Acceleration(biasFz, AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated bias of z coordinate of accelerometer sensed specific force.
      *
      * @param result instance where bias of z coordinate of sensed specific force
      *               will be stored.
      */
     public void getBiasFzAsAcceleration(final Acceleration result) {
         result.setValue(biasFz);
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated bias of x coordinate of gyroscope sensed angular rate
      * expressed in radians per second (rad/s).
      *
      * @return bias of x coordinate of sensed angular rate.
      */
     public double getBiasAngularRateX() {
         return biasAngularRateX;
     }
 
     /**
      * Gets estimated bias of x coordinate of gyroscope sensed angular rate.
      *
      * @return bias of x coordinate of sensed angular rate.
      */
     public AngularSpeed getBiasAngularRateXAsAngularSpeed() {
         return new AngularSpeed(biasAngularRateX, AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated bias of x coordinate of gyroscope sensed angular rate.
      *
      * @param result instance where bias of x coordinate of sensed angular rate
      *               will be stored.
      */
     public void getBiasAngularRateXAsAngularSpeed(final AngularSpeed result) {
         result.setValue(biasAngularRateX);
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated bias of y coordinate of gyroscope sensed angular rate
      * expressed in radians per second (rad/s).
      *
      * @return bias of y coordinate of sensed angular rate.
      */
     public double getBiasAngularRateY() {
         return biasAngularRateY;
     }
 
     /**
      * Gets estimated bias of y coordinate of gyroscope sensed angular rate.
      *
      * @return bias of y coordinate of sensed angular rate.
      */
     public AngularSpeed getBiasAngularRateYAsAngularSpeed() {
         return new AngularSpeed(biasAngularRateY, AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated bias of y coordinate of gyroscope sensed angular rate.
      *
      * @param result instance where bias of y coordinate of sensed angular rate
      *               will be stored.
      */
     public void getBiasAngularRateYAsAngularSpeed(final AngularSpeed result) {
         result.setValue(biasAngularRateY);
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated bias of z coordinate of gyroscope sensed angular rate
      * expressed in radians per second (rad/s).
      *
      * @return bias of z coordinate of sensed angular rate.
      */
     public double getBiasAngularRateZ() {
         return biasAngularRateZ;
     }
 
     /**
      * Gets estimated bias of z coordinate of gyroscope sensed angular rate.
      *
      * @return bias of z coordinate of sensed angular rate.
      */
     public AngularSpeed getBiasAngularRateZAsAngularSpeed() {
         return new AngularSpeed(biasAngularRateZ, AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated bias of z coordinate of gyroscope sensed angular rate.
      *
      * @param result instance where bias of z coordinate of sensed angular rate
      *               will be stored.
      */
     public void getBiasAngularRateZAsAngularSpeed(final AngularSpeed result) {
         result.setValue(biasAngularRateZ);
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated bias of accelerometer sensed specific force.
      *
      * @return estimated bias of accelerometer sensed specific force.
      */
     public AccelerationTriad getBiasF() {
         return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasFx, biasFy, biasFz);
     }
 
     /**
      * Gets estimated bias of accelerometer sensed specific force.
      *
      * @param result instance where bias of sensed specific force will
      *               be stored.
      */
     public void getBiasF(final AccelerationTriad result) {
         result.setValueCoordinatesAndUnit(biasFx, biasFy, biasFz, AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated bias of gyroscope sensed angular rate.
      *
      * @return estimated bias of gyroscope sensed angular rate.
      */
     public AngularSpeedTriad getBiasAngularRate() {
         return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                 biasAngularRateX, biasAngularRateY, biasAngularRateZ);
     }
 
     /**
      * Gets estimated bias of gyroscope sensed angular rate.
      *
      * @param result instance where bias of gyroscope sensed angular
      *               rate will be stored.
      */
     public void getBiasAngularRate(final AngularSpeedTriad result) {
         result.setValueCoordinatesAndUnit(biasAngularRateX, biasAngularRateY, biasAngularRateZ,
                 AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets body kinematics containing estimated bias values for accelerometer
      * and gyroscope.
      *
      * @return body kinematics containing estimated bias values.
      */
     public BodyKinematics getBiasesAsBodyKinematics() {
         final var result = new BodyKinematics();
         getBiasesAsBodyKinematics(result);
         return result;
     }
 
     /**
      * Gets body kinematics containing estimated bias values for accelerometer
      * and gyroscope.
      *
      * @param result instance where body kinematics containing estimated bias
      *               values will be stored.
      */
     public void getBiasesAsBodyKinematics(final BodyKinematics result) {
         result.setSpecificForceCoordinates(biasFx, biasFy, biasFz);
         result.setAngularRateCoordinates(biasAngularRateX, biasAngularRateY, biasAngularRateZ);
     }
 
     /**
      * 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 getVarianceFx() {
         return varianceFx;
     }
 
     /**
      * 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 getVarianceFy() {
         return varianceFy;
     }
 
     /**
      * 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 getVarianceFz() {
         return varianceFz;
     }
 
     /**
      * 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 (m/s^2).
      *
      * @return estimated standard deviation of x coordinate of sensed specific force.
      */
     public double getStandardDeviationFx() {
         return Math.sqrt(varianceFx);
     }
 
     /**
      * 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 getStandardDeviationFxAsAcceleration() {
         return new Acceleration(getStandardDeviationFx(), 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 getStandardDeviationFxAsAcceleration(final Acceleration result) {
         result.setValue(getStandardDeviationFx());
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of y coordinate of accelerometer
      * sensed specific force expressed in (m/s^2).
      *
      * @return estimated standard deviation of y coordinate of sensed specific
      * force.
      */
     public double getStandardDeviationFy() {
         return Math.sqrt(varianceFy);
     }
 
     /**
      * 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 getStandardDeviationFyAsAcceleration() {
         return new Acceleration(getStandardDeviationFy(),
                 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 getStandardDeviationFyAsAcceleration(final Acceleration result) {
         result.setValue(getStandardDeviationFy());
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of z coordinate of accelerometer
      * sensed specific force expressed in (m/s^2).
      *
      * @return estimated standard deviation of z coordinate of sensed specific
      * force.
      */
     public double getStandardDeviationFz() {
         return Math.sqrt(varianceFz);
     }
 
     /**
      * 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 getStandardDeviationFzAsAcceleration() {
         return new Acceleration(getStandardDeviationFz(), 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 getStandardDeviationFzAsAcceleration(final Acceleration result) {
         result.setValue(getStandardDeviationFz());
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of accelerometer sensed
      * specific force.
      *
      * @return estimated standard deviation of accelerometer
      */
     public AccelerationTriad getStandardDeviationF() {
         return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                 getStandardDeviationFx(),
                 getStandardDeviationFy(),
                 getStandardDeviationFz());
     }
 
     /**
      * Gets estimated standard deviation of accelerometer sensed
      * specific force.
      *
      * @param result instance where estimated standard deviation of
      *               accelerometer will be stored.
      */
     public void getStandardDeviationF(final AccelerationTriad result) {
         result.setValueCoordinatesAndUnit(getStandardDeviationFx(), getStandardDeviationFy(), getStandardDeviationFz(),
                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets average of estimated standard deviation of accelerometer sensed specific
      * force for all coordinates expressed in meters per squared second (m/s^2).
      *
      * @return average of estimated standard deviation of accelerometer.
      */
     public double getAverageAccelerometerStandardDeviation() {
         return (getStandardDeviationFx() + getStandardDeviationFy() + getStandardDeviationFz()) / 3.0;
     }
 
     /**
      * Gets average of estimated standard deviation of accelerometer sensed specific
      * force for all coordinates.
      *
      * @return average of estimated standard deviation of accelerometer.
      */
     public Acceleration getAverageAccelerometerStandardDeviationAsAcceleration() {
         return new Acceleration(getAverageAccelerometerStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets average of estimated standard deviation of accelerometer sensed specific
      * force for all coordinates.
      *
      * @param result instance where result data will be copied to.
      */
     public void getAverageAccelerometerStandardDeviationAsAcceleration(final Acceleration result) {
         result.setValue(getAverageAccelerometerStandardDeviation());
         result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of x coordinate of gyroscope sensed angular
      * rate expressed in (rad/s).
      *
      * @return estimated standard deviation of x coordinate of sensed angular rate.
      */
     public double getStandardDeviationAngularRateX() {
         return Math.sqrt(varianceAngularRateX);
     }
 
     /**
      * Gets estimated standard deviation of x coordinate of gyroscope sensed angular
      * rate.
      *
      * @return estimated standard deviation of x coordinate of sensed angular rate.
      */
     public AngularSpeed getStandardDeviationAngularRateXAsAngularSpeed() {
         return new AngularSpeed(getStandardDeviationAngularRateX(), AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of x coordinate of gyroscope sensed angular
      * rate.
      *
      * @param result instance where estimated standard deviation of x coordinate of
      *               sensed angular rate will be stored.
      */
     public void getStandardDeviationAngularRateXAsAngularSpeed(final AngularSpeed result) {
         result.setValue(getStandardDeviationAngularRateX());
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of y coordinate of gyroscope sensed angular
      * rate expressed in (rad/s).
      *
      * @return estimated standard deviation of y coordinate of sensed angular rate.
      */
     public double getStandardDeviationAngularRateY() {
         return Math.sqrt(varianceAngularRateY);
     }
 
     /**
      * Gets estimated standard deviation of y coordinate of gyroscope sensed angular
      * rate.
      *
      * @return estimated standard deviation of y coordinate of sensed angular rate.
      */
     public AngularSpeed getStandardDeviationAngularRateYAsAngularSpeed() {
         return new AngularSpeed(getStandardDeviationAngularRateY(), AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of y coordinate of gyroscope sensed angular
      * rate.
      *
      * @param result instance where estimated standard deviation of y coordinate of
      *               sensed angular rate will be stored.
      */
     public void getStandardDeviationAngularRateYAsAngularSpeed(final AngularSpeed result) {
         result.setValue(getStandardDeviationAngularRateY());
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of z coordinate of gyroscope sensed angular
      * rate expressed in (rad/s).
      *
      * @return estimated standard deviation of z coordinate of sensed angular rate.
      */
     public double getStandardDeviationAngularRateZ() {
         return Math.sqrt(varianceAngularRateZ);
     }
 
     /**
      * Gets estimated standard deviation of z coordinate of gyroscope sensed angular
      * rate.
      *
      * @return estimated standard deviation of z coordinate of sensed angular rate.
      */
     public AngularSpeed getStandardDeviationAngularRateZAsAngularSpeed() {
         return new AngularSpeed(getStandardDeviationAngularRateZ(), AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of z coordinate of gyroscope sensed angular
      * rate.
      *
      * @param result instance where estimated standard deviation of z coordinate of
      *               sensed angular rate will be stored.
      */
     public void getStandardDeviationAngularRateZAsAngularSpeed(final AngularSpeed result) {
         result.setValue(getStandardDeviationAngularRateZ());
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets estimated standard deviation of sensed angular rate.
      *
      * @return estimated standard deviation of sensed angular rate.
      */
     public AngularSpeedTriad getStandardDeviationAngularRate() {
         return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                 getStandardDeviationAngularRateX(),
                 getStandardDeviationAngularRateY(),
                 getStandardDeviationAngularRateZ());
     }
 
     /**
      * Gets estimated standard deviation of sensed angular rate.
      *
      * @param result instance where estimated standard deviation of
      *               sensed angular rate.
      */
     public void getStandardDeviationAngularRate(final AngularSpeedTriad result) {
         result.setValueCoordinatesAndUnit(getStandardDeviationAngularRateX(),
                 getStandardDeviationAngularRateY(),
                 getStandardDeviationAngularRateZ(),
                 AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets average of estimated standard deviation of gyroscope sensed angular rate
      * for all coordinates expressed in radians per second (rad/s).
      *
      * @return average of estimated standard deviation of gyroscope.
      */
     public double getAverageGyroscopeStandardDeviation() {
         return (getStandardDeviationAngularRateX() + getStandardDeviationAngularRateY()
                 + getStandardDeviationAngularRateZ()) / 3.0;
     }
 
     /**
      * Gets average of estimated standard deviation of gyroscope sensed angular rate
      * for all coordinates.
      *
      * @return average of estimated standard deviation of gyroscope.
      */
     public AngularSpeed getAverageGyroscopeStandardDeviationAsAngularSpeed() {
         return new AngularSpeed(getAverageGyroscopeStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets average of estimated standard deviation of gyroscope sensed angular rate
      * for all coordinates.
      *
      * @param result instance where result data will be copied to.
      */
     public void getAverageGyroscopeStandardDeviationAsAngularSpeed(final AngularSpeed result) {
         result.setValue(getAverageGyroscopeStandardDeviation());
         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 getStandardDeviationsAsBodyKinematics() {
         return new BodyKinematics(getStandardDeviationFx(),
                 getStandardDeviationFy(),
                 getStandardDeviationFz(),
                 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 getStandardDeviationsAsBodyKinematics(final BodyKinematics result) {
         result.setSpecificForceCoordinates(getStandardDeviationFx(),
                 getStandardDeviationFy(), getStandardDeviationFz());
         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 getPSDFx() {
         return varianceFx * 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 getPSDFy() {
         return varianceFy * 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 getPSDFz() {
         return varianceFz * 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 getPSDAngularRateX() {
         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 getPSDAngularRateY() {
         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 getPSDAngularRateZ() {
         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 getRootPSDFx() {
         return Math.sqrt(getPSDFx());
     }
 
     /**
      * 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 getRootPSDFy() {
         return Math.sqrt(getPSDFy());
     }
 
     /**
      * 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 getRootPSDFz() {
         return Math.sqrt(getPSDFz());
     }
 
     /**
      * 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 getRootPSDAngularRateX() {
         return Math.sqrt(getPSDAngularRateX());
     }
 
     /**
      * 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 getRootPSDAngularRateY() {
         return Math.sqrt(getPSDAngularRateY());
     }
 
     /**
      * 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 getRootPSDAngularRateZ() {
         return Math.sqrt(getPSDAngularRateZ());
     }
 
     /**
      * 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 getAccelerometerNoisePSD() {
         return (getPSDFx() + getPSDFy() + getPSDFz()) / 3.0;
     }
 
     /**
      * Gets accelerometer noise root PSD (Power Spectral Density) which is the
      * norm of root PSD components expressed as (m * s^-1.5).
      *
      * @return average accelerometer noise root PSD.
      */
     public double getAccelerometerNoiseRootPSD() {
         return Math.sqrt(getPSDFx() + getPSDFy() + getPSDFz());
     }
 
     /**
      * 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 getGyroNoisePSD() {
         return (getPSDAngularRateX() + getPSDAngularRateY() + getPSDAngularRateZ()) / 3.0;
     }
 
     /**
      * Gets gyroscope noise root PSD (Power Spectral Density) which is the
      * norm of root PSD components expressed in (rad * s^-0.5).
      *
      * @return average gyroscope noise root PSD.
      */
     public double getGyroNoiseRootPSD() {
         return Math.sqrt(getPSDAngularRateX() + getPSDAngularRateY() + getPSDAngularRateZ());
     }
 
     /**
      * Gets estimated bias of accelerometer sensed specific force
      * expressed in meters per squared second (m/s^2) as a 3x1 matrix column vector.
      *
      * @return estimated bias of accelerometer sensed specific force.
      */
     public Matrix getAccelerometerBias() {
         Matrix result;
         try {
             result = new Matrix(BodyKinematics.COMPONENTS, 1);
             getAccelerometerBias(result);
         } catch (final WrongSizeException ignore) {
             // never happens
             result = null;
         }
 
         return result;
     }
 
     /**
      * Gets estimated bias of accelerometer sensed specific force
      * expressed in meters per squared second (m/s^2) as a 3x1 matrix column vector.
      *
      * @param result instance where data will be copied to. Must be 3x1.
      * @throws IllegalArgumentException if provided result matrix is not 3x1.
      */
     public void getAccelerometerBias(final Matrix result) {
         if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
             throw new IllegalArgumentException();
         }
 
         result.setElementAtIndex(0, biasFx);
         result.setElementAtIndex(1, biasFy);
         result.setElementAtIndex(2, biasFz);
     }
 
     /**
      * Gets estimated bias of gyroscope sensed angular rates
      * expressed in radians per second (rad/s) as a 3x1 matrix column vector.
      *
      * @return estimated bias of gyroscope sensed angular rates.
      */
     public Matrix getGyroBias() {
         Matrix result;
         try {
             result = new Matrix(BodyKinematics.COMPONENTS, 1);
             getGyroBias(result);
         } catch (final WrongSizeException ignore) {
             // never happens
             result = null;
         }
 
         return result;
     }
 
     /**
      * Gets estimated bias of gyroscope sensed angular rates
      * expressed in radians per second (rad/s) as a 3x1 matrix column vector.
      *
      * @param result instance where data will be copied to. Must be 3x1.
      * @throws IllegalArgumentException if provided result matrix is not 3x1.
      */
     public void getGyroBias(final Matrix result) {
         if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
             throw new IllegalArgumentException();
         }
 
         result.setElementAtIndex(0, biasAngularRateX);
         result.setElementAtIndex(1, biasAngularRateY);
         result.setElementAtIndex(2, biasAngularRateZ);
     }
 
     /**
      * 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 amount of total elapsed time since first processed measurement expressed
      * in seconds (s).
      *
      * @return amount of total elapsed time.
      */
     public double getElapsedTimeSeconds() {
         return numberOfProcessedSamples * timeInterval;
     }
 
     /**
      * Gets amount of total elapsed time since first processed measurement.
      *
      * @return amount of total elapsed time.
      */
     public Time getElapsedTime() {
         return new Time(getElapsedTimeSeconds(), TimeUnit.SECOND);
     }
 
     /**
      * Gets amount of total elapsed time since first processed measurement.
      *
      * @param result instance where result will be stored.
      */
     public void getElapsedTime(final Time result) {
         result.setValue(getElapsedTimeSeconds());
         result.setUnit(TimeUnit.SECOND);
     }
 
     /**
      * Indicates whether estimator is currently running or not.
      *
      * @return true if estimator is running, false otherwise.
      */
     public boolean isRunning() {
         return running;
     }
 
     /**
      * Gets theoretically expected body kinematics for provided body position and
      * orientation, and provided time interval, assuming that body remains at the
      * same position (zero velocity).
      * When body remains static, sensed specific force and angular rates will remain
      * constant due to gravity and Earth rotation.
      *
      * @return expected body kinematics.
      */
     public BodyKinematics getExpectedKinematics() {
         return new BodyKinematics(expectedKinematics);
     }
 
     /**
      * Gets theoretically expected body kinematics for provided body position and
      * orientation, and provided time interval, assuming that body remains at the
      * same position (zero velocity).
      * When body remains static, sensed specific force and angular rates will remain
      * constant due to gravity and Earth rotation.
      *
      * @param result instance where expected body kinematics will be stored.
      */
     public void getExpectedKinematics(final BodyKinematics result) {
         expectedKinematics.copyTo(result);
     }
 
     /**
      * Adds a sample of body kinematics (accelerometer + gyroscope readings) obtained
      * from an IMU.
      *
      * @param kinematics kinematics instance to be added and processed.
      * @throws LockedException if estimator is currently running.
      */
     public void addBodyKinematics(final BodyKinematics kinematics) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         running = true;
 
         if (lastBodyKinematics == null && listener != null) {
             listener.onStart(this);
         }
 
         final var fx = kinematics.getFx();
         final var fy = kinematics.getFy();
         final var fz = kinematics.getFz();
         final var angularRateX = kinematics.getAngularRateX();
         final var angularRateY = kinematics.getAngularRateY();
         final var angularRateZ = kinematics.getAngularRateZ();
 
         final var expectedFx = expectedKinematics.getFx();
         final var expectedFy = expectedKinematics.getFy();
         final var expectedFz = expectedKinematics.getFz();
         final var expectedAngularRateX = expectedKinematics.getAngularRateX();
         final var expectedAngularRateY = expectedKinematics.getAngularRateY();
         final var expectedAngularRateZ = expectedKinematics.getAngularRateZ();
 
         final var diffFx = fx - expectedFx;
         final var diffFy = fy - expectedFy;
         final var diffFz = fz - expectedFz;
         final var diffAngularRateX = angularRateX - expectedAngularRateX;
         final var diffAngularRateY = angularRateY - expectedAngularRateY;
         final var diffAngularRateZ = angularRateZ - expectedAngularRateZ;
 
         // compute biases
         final var tmp = (double) numberOfProcessedSamples / (double) numberOfProcessedSamplesPlusOne;
         biasFx = biasFx * tmp + diffFx / numberOfProcessedSamplesPlusOne;
         biasFy = biasFy * tmp + diffFy / numberOfProcessedSamplesPlusOne;
         biasFz = biasFz * tmp + diffFz / numberOfProcessedSamplesPlusOne;
 
         biasAngularRateX = biasAngularRateX * tmp + diffAngularRateX / numberOfProcessedSamplesPlusOne;
         biasAngularRateY = biasAngularRateY * tmp + diffAngularRateY / numberOfProcessedSamplesPlusOne;
         biasAngularRateZ = biasAngularRateZ * tmp + diffAngularRateZ / numberOfProcessedSamplesPlusOne;
 
         // compute variances
         final var diffBiasFx = diffFx - biasFx;
         final var diffBiasFy = diffFy - biasFy;
         final var diffBiasFz = diffFz - biasFz;
         final var diffBiasAngularRateX = diffAngularRateX - biasAngularRateX;
         final var diffBiasAngularRateY = diffAngularRateY - biasAngularRateY;
         final var diffBiasAngularRateZ = diffAngularRateZ - biasAngularRateZ;
 
         final var diffBiasFx2 = diffBiasFx * diffBiasFx;
         final var diffBiasFy2 = diffBiasFy * diffBiasFy;
         final var diffBiasFz2 = diffBiasFz * diffBiasFz;
         final var diffBiasAngularRateX2 = diffBiasAngularRateX * diffBiasAngularRateX;
         final var diffBiasAngularRateY2 = diffBiasAngularRateY * diffBiasAngularRateY;
         final var diffBiasAngularRateZ2 = diffBiasAngularRateZ * diffBiasAngularRateZ;
 
         varianceFx = varianceFx * tmp + diffBiasFx2 / numberOfProcessedSamplesPlusOne;
         varianceFy = varianceFy * tmp + diffBiasFy2 / numberOfProcessedSamplesPlusOne;
         varianceFz = varianceFz * tmp + diffBiasFz2 / numberOfProcessedSamplesPlusOne;
 
         varianceAngularRateX = varianceAngularRateX * tmp + diffBiasAngularRateX2 / numberOfProcessedSamplesPlusOne;
         varianceAngularRateY = varianceAngularRateY * tmp + diffBiasAngularRateY2 / numberOfProcessedSamplesPlusOne;
         varianceAngularRateZ = varianceAngularRateZ * tmp + diffBiasAngularRateZ2 / numberOfProcessedSamplesPlusOne;
 
         lastBodyKinematics = kinematics;
 
         numberOfProcessedSamples++;
         numberOfProcessedSamplesPlusOne++;
 
         if (listener != null) {
             listener.onBodyKinematicsAdded(this);
         }
 
         running = 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;
         }
 
         running = true;
         lastBodyKinematics = null;
         biasFx = 0.0;
         biasFy = 0.0;
         biasFz = 0.0;
         biasAngularRateX = 0.0;
         biasAngularRateY = 0.0;
         biasAngularRateZ = 0.0;
         varianceFx = 0.0;
         varianceFy = 0.0;
         varianceFz = 0.0;
         varianceAngularRateX = 0.0;
         varianceAngularRateY = 0.0;
         varianceAngularRateZ = 0.0;
         numberOfProcessedSamples = 0;
         numberOfProcessedSamplesPlusOne = 1;
 
         if (listener != null) {
             listener.onReset(this);
         }
 
         running = false;
 
         return true;
     }
 
     /**
      * Converts provided time instance to seconds.
      *
      * @param time instance to be converted.
      * @return obtained conversion in seconds.
      */
     private static double convertTime(final Time time) {
         return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
     }
 
     /**
      * Rebuilds expected theoretical kinematics for provided body position
      * and orientation and provided time interval, assuming that body
      * remains at the same position (zero velocity).
      * When body remains static, sensed specific force and angular rates will remain
      * constant due to gravity and Earth rotation.
      */
     private void rebuildExpectedKinematics() {
         if (frame == null) {
             return;
         }
         if (expectedKinematics == null) {
             expectedKinematics = new BodyKinematics();
         }
 
         final var ecefC = getEcefC();
         final var x = frame.getX();
         final var y = frame.getY();
         final var z = frame.getZ();
         ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefC, ecefC,
                 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                 x, y, z, expectedKinematics);
     }
 }