Package com.irurueta.ar.slam

Class BaseSlamEstimator<D extends BaseCalibrationData>

java.lang.Object
com.irurueta.ar.slam.BaseSlamEstimator<D>
Type Parameters:
D - calibrator type associated to implementations of SLAM calibration data.
All Implemented Interfaces:
Serializable
Direct Known Subclasses:
AbsoluteOrientationBaseSlamEstimator, ConstantVelocityModelSlamEstimator, SlamEstimator
public abstract class BaseSlamEstimator<D extends BaseCalibrationData> extends Object implements Serializable
Base class to estimate position, velocity, acceleration and orientation of a device using sensor data such as accelerometers and gyroscopes. Implementations of this class are designed taking into account sensors available on Android devices.
See Also:

  • Nested Class Summary

    Nested Classes
    Modifier and Type
    Class
    Description
    static interface 
    BaseSlamEstimator.BaseSlamEstimatorListener<D extends BaseCalibrationData>
    Listener for implementations of this class.

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    protected long
    accelerometerTimestampNanos
    Timestamp expressed in nanoseconds since the epoch time of the last accelerometer sample.
    protected double
    accumulatedAccelerationSampleX
    Average of acceleration along x-axis accumulated since last full sample.
    protected double
    accumulatedAccelerationSampleY
    Average of acceleration along y-axis accumulated since last full sample.
    protected double
    accumulatedAccelerationSampleZ
    Average of acceleration along z-axis accumulated since last full sample.
    protected int
    accumulatedAccelerometerSamples
    Number of accelerometer samples accumulated since last full sample.
    protected double
    accumulatedAngularSpeedSampleX
    Average of angular speed along x-axis accumulated since last full sample.
    protected double
    accumulatedAngularSpeedSampleY
    Average of angular speed along y-axis accumulated since last full sample.
    protected double
    accumulatedAngularSpeedSampleZ
    Average of angular speed along z-axis accumulated since last full sample.
    protected int
    accumulatedGyroscopeSamples
    Number of gyroscope samples accumulated since last full sample.
    protected boolean
    accumulationEnabled
    Indicates whether accumulation of samples is enabled or not.
    protected D
    calibrationData
    Calibration data.
    protected static final boolean
    DEFAULT_ENABLE_SAMPLE_ACCUMULATION
    Indicates whether sample accumulation must be enabled or not.
    protected boolean
    error
    Indicates whether an error occurred during the estimation.
    protected long
    gyroscopeTimestampNanos
    Timestamp expressed in nanoseconds since the epoch time of the last gyroscope sample.
    protected BaseSlamEstimator.BaseSlamEstimatorListener<D>
    listener
    Listener in charge of handling events raised by instances of this class.
    protected static final int
    N_COMPONENTS_3D
    Number of components in 3D.
    protected static final double
    NANOS_TO_SECONDS
    Conversion of nanoseconds to milliseconds.
    protected com.irurueta.statistics.MultivariateNormalDist
    normalDist
    Multivariate distribution to be reused during propagation of calibrated covariance.
    protected double
    stateAccelerationX
    Current linear acceleration of the device along x-axis expressed in meters per squared second (m/s^2).
    protected double
    stateAccelerationY
    Current linear acceleration of the device along y-axis expressed in meters per squared second (m/s^2).
    protected double
    stateAccelerationZ
    Current linear acceleration of the device along z-axis expressed in meters per squared second (m/s^2).
    protected double
    stateAngularSpeedX
    Angular speed of rotation of the device along x-axis expressed in radians per second (rad/s).
    protected double
    stateAngularSpeedY
    Angular speed of rotation of the device along y-axis expressed in radians per second (rad/s).
    protected double
    stateAngularSpeedZ
    Angular speed of rotation of the device along z-axis expressed in radians per second (rad/s).
    protected double
    statePositionX
    Current position of the device along x-axis expressed in meters (m).
    protected double
    statePositionY
    Current position of the device along y-axis expressed in meters (m).
    protected double
    statePositionZ
    Current position of the device along z-axis expressed in meters (m).
    protected double
    stateQuaternionA
    A value of quaternion containing current device orientation.
    protected double
    stateQuaternionB
    B value of quaternion containing current device orientation.
    protected double
    stateQuaternionC
    C value of quaternion containing current device orientation.
    protected double
    stateQuaternionD
    D value of quaternion containing current device orientation.
    protected double
    stateVelocityX
    Current linear velocity of the device along x-axis expressed in meters per second (m/s).
    protected double
    stateVelocityY
    Current linear velocity of the device along y-axis expressed in meters per second (m/s).
    protected double
    stateVelocityZ
    Current linear velocity of the device along z-axis expressed in meters per second (m/s).

  • Constructor Summary

    Constructors
    Modifier
    Constructor
    Description
    protected
    BaseSlamEstimator()
    Constructor.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    correctWithPositionMeasure(double[] position)
    Corrects system state with provided position measure using current position accuracy.
    void
    correctWithPositionMeasure(double[] position, com.irurueta.algebra.Matrix positionCovariance)
    Corrects system state with provided position measure having an accuracy determined by provided covariance matrix.
    abstract void
    correctWithPositionMeasure(double positionX, double positionY, double positionZ)
    Corrects system state with provided position measure using current position accuracy.
    void
    correctWithPositionMeasure(double positionX, double positionY, double positionZ, com.irurueta.algebra.Matrix positionCovariance)
    Corrects system state with provided position measure having an accuracy determined by provided covariance matrix.
    void
    correctWithPositionMeasure(com.irurueta.geometry.Point3D position)
    Corrects system state with provided position measure using current position accuracy.
    void
    correctWithPositionMeasure(com.irurueta.geometry.Point3D position, com.irurueta.algebra.Matrix positionCovariance)
    Corrects system state with provided position measure having an accuracy determined by provided covariance matrix.
    long
    getAccelerometerTimestampNanos()
    Gets timestamp expressed in nanoseconds since the epoch time of the last accelerometer sample, or -1 if no sample has been set yet.
    double[]
    getAccumulatedAccelerationSample()
    Gets average acceleration along x,y,z axes accumulated since last full sample.
    void
    getAccumulatedAccelerationSample(double[] result)
    Gets average acceleration along x,y,z axes accumulated since last full sample.
    double
    getAccumulatedAccelerationSampleX()
    Gets average acceleration along x-axis accumulated since last full sample.
    double
    getAccumulatedAccelerationSampleY()
    Gets average acceleration along y-axis accumulated since last full sample.
    double
    getAccumulatedAccelerationSampleZ()
    Gets average acceleration along z-axis accumulated since last full sample.
    int
    getAccumulatedAccelerometerSamples()
    Gets number of accelerometer samples accumulated since last full sample.
    double[]
    getAccumulatedAngularSpeedSample()
    Gets average angular speed along x,y,z axes accumulated since last full sample.
    void
    getAccumulatedAngularSpeedSample(double[] result)
    Gets average angular speed along x,y,z axes accumulated since last full sample.
    double
    getAccumulatedAngularSpeedSampleX()
    Gets average angular speed along x-axis accumulated since last full sample.
    double
    getAccumulatedAngularSpeedSampleY()
    Gets average angular speed along y-axis accumulated since last full sample.
    double
    getAccumulatedAngularSpeedSampleZ()
    Gets average angular speed along z-axis accumulated since last full sample.
    int
    getAccumulatedGyroscopeSamples()
    Gets number of gyroscope samples accumulated since last full sample.
    D
    getCalibrationData()
    Gets calibration data.
    long
    getGyroscopeTimestampNanos()
    Gets timestamp expressed in nanoseconds since the epoch time of the last gyroscope sample, or -1 if no sample has been set yet.
    BaseSlamEstimator.BaseSlamEstimatorListener<D>
    getListener()
    Gets listener in charge of handling events raised by instances of this class.
    long
    getMostRecentTimestampNanos()
    Gets most recent timestamp of received partial samples (accelerometer or gyroscope).
    abstract com.irurueta.algebra.Matrix
    getPositionCovarianceMatrix()
    Gets current covariance matrix of position measures determining current accuracy of provided position measures.
    double[]
    getStateAcceleration()
    Gets current x,y,z linear acceleration coordinates of the device expressed in meters per squared second (m/s^2).
    void
    getStateAcceleration(double[] result)
    Gets current x,y,z linear acceleration coordinates of the device expressed in meters per squared second (m/s^2).
    double
    getStateAccelerationX()
    Gets current linear acceleration of the device along x-axis expressed in meters per squared second (m/s^2).
    double
    getStateAccelerationY()
    Gets current linear acceleration of the device along y-axis expressed in meters per squared second (m/s^2).
    double
    getStateAccelerationZ()
    Gets current linear acceleration of the device along z-axis expressed in meters per squared second (m/s^2).
    double[]
    getStateAngularSpeed()
    Gets angular speed of the device along x,y,z axes expressed in radians per second (rad/s).
    void
    getStateAngularSpeed(double[] result)
    Gets angular speed of the device along x,y,z axes expressed in radians per second (rad/s) and stores the result into provided array.
    double
    getStateAngularSpeedX()
    Gets angular speed along x-axis expressed in radians per second (rad/s).
    double
    getStateAngularSpeedY()
    Gets angular speed along y-axis expressed in radians per second (rad/s).
    double
    getStateAngularSpeedZ()
    Gets angular speed along z-axis expressed in radians per second (rad/s).
    abstract com.irurueta.algebra.Matrix
    getStateCovariance()
    Gets covariance matrix of state variables (position, velocity, acceleration, orientation and angular speed).
    double[]
    getStatePosition()
    Gets x,y,z coordinates of the device position expressed in meters (m).
    void
    getStatePosition(double[] result)
    Gets x,y,z coordinates of the device position expressed in meters (m) and stores the result into provided array.
    double
    getStatePositionX()
    Obtains current x-position of the device expressed in meters (m).
    double
    getStatePositionY()
    Obtains current y-position of the device expressed in meters (m).
    double
    getStatePositionZ()
    Obtains current z-position of the device expressed in meters (m).
    com.irurueta.geometry.Quaternion
    getStateQuaternion()
    Gets quaternion containing current device orientation.
    void
    getStateQuaternion(com.irurueta.geometry.Quaternion result)
    Gets quaternion containing current device orientation.
    double
    getStateQuaternionA()
    Gets A value of quaternion containing current device orientation.
    double[]
    getStateQuaternionArray()
    Gets A, B, C, D values of quaternion containing current device orientation.
    void
    getStateQuaternionArray(double[] result)
    Gets A, B, C, D values of quaternion containing current device orientation.
    double
    getStateQuaternionB()
    Gets B value of quaternion containing current device orientation.
    double
    getStateQuaternionC()
    Gets C value of quaternion containing current device orientation.
    double
    getStateQuaternionD()
    Gets D value of quaternion containing current device orientation.
    double[]
    getStateVelocity()
    Gets x,y,z coordinates of current linear velocity of the device expressed in meters per second (m/s).
    void
    getStateVelocity(double[] result)
    Gets x,y,z coordinates of current linear velocity of the device expressed in meters per second (m/s).
    double
    getStateVelocityX()
    Gets current linear velocity of the device along x-axis expressed in meters per second (m/s).
    double
    getStateVelocityY()
    Gets current linear velocity of the device along y-axis expressed in meters per second (m/s).
    double
    getStateVelocityZ()
    Gets current linear velocity of the device along z-axis expressed in meters per second (m/s).
    boolean
    hasError()
    Indicates whether an error occurred during the estimation.
    boolean
    isAccelerometerSampleReceived()
    Indicates whether the accelerometer sample has been received since the last full sample (accelerometer + gyroscope).
    boolean
    isAccumulationEnabled()
    Indicates whether accumulation of samples is enabled or not.
    boolean
    isFullSampleAvailable()
    Indicates whether a full sample (accelerometer + gyroscope) has been received or not.
    boolean
    isGyroscopeSampleReceived()
    Indicates whether the gyroscope sample has been received since the last full sample (accelerometer + gyroscope).
    protected void
    notifyFullSampleAndResetSampleReceive()
    Notifies that a full sample has been received and resets flags indicating whether partial samples have been received.
    protected abstract void
    processFullSample()
    Method to be implemented in subclasses to process a full sample.
    final void
    reset()
    Resets position, linear velocity, linear acceleration, orientation and angular speed of the device to zero.
    protected void
    reset(double statePositionX, double statePositionY, double statePositionZ, double stateVelocityX, double stateVelocityY, double stateVelocityZ, double stateAccelerationX, double stateAccelerationY, double stateAccelerationZ, double stateQuaternionA, double stateQuaternionB, double stateQuaternionC, double stateQuaternionD, double stateAngularSpeedX, double stateAngularSpeedY, double stateAngularSpeedZ)
    Resets position, linear velocity, linear acceleration, orientation and angular speed to provided values.
    final void
    resetAcceleration()
    Resets acceleration and timestamp to zero while keeping other state parameters.
    final void
    resetAngularSpeed()
    Resets angular speed and timestamp to zero while keeping other state parameters.
    final void
    resetOrientation()
    Resets orientation and timestamp to zero while keeping other state parameters.
    final void
    resetPosition()
    Resets position and timestamp to zero while keeping other state parameters.
    final void
    resetPositionAndVelocity()
    Resets position, linear velocity and timestamp to zero while keeping other state parameters.
    final void
    resetVelocity()
    Resets linear velocity and timestamp to zero while keeping other state parameters.
    void
    setAccumulationEnabled(boolean accumulationEnabled)
    Specifies whether accumulation of samples is enabled or not.
    void
    setCalibrationData(D calibrationData)
    Sets calibration data.
    void
    setListener(BaseSlamEstimator.BaseSlamEstimatorListener<D> listener)
    Sets listener in charge of handling events raised by instances of this class.
    abstract void
    setPositionCovarianceMatrix(com.irurueta.algebra.Matrix positionCovariance)
    Updates covariance matrix of position measures.
    void
    updateAccelerometerSample(long timestamp, float[] data)
    Provides a new accelerometer sample.
    void
    updateAccelerometerSample(long timestamp, float accelerationX, float accelerationY, float accelerationZ)
    Provides a new accelerometer sample.
    void
    updateGyroscopeSample(long timestamp, float[] data)
    Provides a new gyroscope sample.
    void
    updateGyroscopeSample(long timestamp, float angularSpeedX, float angularSpeedY, float angularSpeedZ)
    Provides a new gyroscope sample.

    Methods inherited from class java.lang.Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Field Details

    • N_COMPONENTS_3D

      protected static final int N_COMPONENTS_3D
      Number of components in 3D.
      See Also:

    • NANOS_TO_SECONDS

      protected static final double NANOS_TO_SECONDS
      Conversion of nanoseconds to milliseconds.
      See Also:

    • DEFAULT_ENABLE_SAMPLE_ACCUMULATION

      protected static final boolean DEFAULT_ENABLE_SAMPLE_ACCUMULATION
      Indicates whether sample accumulation must be enabled or not.
      See Also:

    • statePositionX

      protected double statePositionX
      Current position of the device along x-axis expressed in meters (m).

    • statePositionY

      protected double statePositionY
      Current position of the device along y-axis expressed in meters (m).

    • statePositionZ

      protected double statePositionZ
      Current position of the device along z-axis expressed in meters (m).

    • stateVelocityX

      protected double stateVelocityX
      Current linear velocity of the device along x-axis expressed in meters per second (m/s).

    • stateVelocityY

      protected double stateVelocityY
      Current linear velocity of the device along y-axis expressed in meters per second (m/s).

    • stateVelocityZ

      protected double stateVelocityZ
      Current linear velocity of the device along z-axis expressed in meters per second (m/s).

    • stateAccelerationX

      protected double stateAccelerationX
      Current linear acceleration of the device along x-axis expressed in meters per squared second (m/s^2).

    • stateAccelerationY

      protected double stateAccelerationY
      Current linear acceleration of the device along y-axis expressed in meters per squared second (m/s^2).

    • stateAccelerationZ

      protected double stateAccelerationZ
      Current linear acceleration of the device along z-axis expressed in meters per squared second (m/s^2).

    • stateQuaternionA

      protected double stateQuaternionA
      A value of quaternion containing current device orientation.

    • stateQuaternionB

      protected double stateQuaternionB
      B value of quaternion containing current device orientation.

    • stateQuaternionC

      protected double stateQuaternionC
      C value of quaternion containing current device orientation.

    • stateQuaternionD

      protected double stateQuaternionD
      D value of quaternion containing current device orientation.

    • stateAngularSpeedX

      protected double stateAngularSpeedX
      Angular speed of rotation of the device along x-axis expressed in radians per second (rad/s).

    • stateAngularSpeedY

      protected double stateAngularSpeedY
      Angular speed of rotation of the device along y-axis expressed in radians per second (rad/s).

    • stateAngularSpeedZ

      protected double stateAngularSpeedZ
      Angular speed of rotation of the device along z-axis expressed in radians per second (rad/s).

    • error

      protected boolean error
      Indicates whether an error occurred during the estimation. If an error occurs the estimator should be restarted since state values might become unreliable.

    • accumulationEnabled

      protected boolean accumulationEnabled
      Indicates whether accumulation of samples is enabled or not.

    • accelerometerTimestampNanos

      protected long accelerometerTimestampNanos
      Timestamp expressed in nanoseconds since the epoch time of the last accelerometer sample.

    • gyroscopeTimestampNanos

      protected long gyroscopeTimestampNanos
      Timestamp expressed in nanoseconds since the epoch time of the last gyroscope sample.

    • accumulatedAccelerometerSamples

      protected int accumulatedAccelerometerSamples
      Number of accelerometer samples accumulated since last full sample.

    • accumulatedGyroscopeSamples

      protected int accumulatedGyroscopeSamples
      Number of gyroscope samples accumulated since last full sample.

    • accumulatedAccelerationSampleX

      protected double accumulatedAccelerationSampleX
      Average of acceleration along x-axis accumulated since last full sample. Expressed in meters per squared second (m/s^2).

    • accumulatedAccelerationSampleY

      protected double accumulatedAccelerationSampleY
      Average of acceleration along y-axis accumulated since last full sample. Expressed in meters per squared second (m/s^2).

    • accumulatedAccelerationSampleZ

      protected double accumulatedAccelerationSampleZ
      Average of acceleration along z-axis accumulated since last full sample. Expressed in meters per squared second (m/s^2).

    • accumulatedAngularSpeedSampleX

      protected double accumulatedAngularSpeedSampleX
      Average of angular speed along x-axis accumulated since last full sample. Expressed in radians per second (rad/s).

    • accumulatedAngularSpeedSampleY

      protected double accumulatedAngularSpeedSampleY
      Average of angular speed along y-axis accumulated since last full sample. Expressed in radians per second (rad/s).

    • accumulatedAngularSpeedSampleZ

      protected double accumulatedAngularSpeedSampleZ
      Average of angular speed along z-axis accumulated since last full sample. Expressed in radians per second (red/s).

    • listener

      protected transient BaseSlamEstimator.BaseSlamEstimatorListener<D extends BaseCalibrationData> listener
      Listener in charge of handling events raised by instances of this class.

    • calibrationData

      protected D extends BaseCalibrationData calibrationData
      Calibration data. When provided, its mean and covariance are used to correct control samples and adjust process covariance matrix during Kalman filtering in prediction stage.

    • normalDist

      protected transient com.irurueta.statistics.MultivariateNormalDist normalDist
      Multivariate distribution to be reused during propagation of calibrated covariance.

  • Constructor Details

    • BaseSlamEstimator

      protected BaseSlamEstimator()
      Constructor.

  • Method Details

    • resetPosition

      public final void resetPosition()
      Resets position and timestamp to zero while keeping other state parameters.

    • resetVelocity

      public final void resetVelocity()
      Resets linear velocity and timestamp to zero while keeping other state parameters.

    • resetPositionAndVelocity

      public final void resetPositionAndVelocity()
      Resets position, linear velocity and timestamp to zero while keeping other state parameters.

    • resetAcceleration

      public final void resetAcceleration()
      Resets acceleration and timestamp to zero while keeping other state parameters.

    • resetOrientation

      public final void resetOrientation()
      Resets orientation and timestamp to zero while keeping other state parameters.

    • resetAngularSpeed

      public final void resetAngularSpeed()
      Resets angular speed and timestamp to zero while keeping other state parameters.

    • reset

      public final void reset()
      Resets position, linear velocity, linear acceleration, orientation and angular speed of the device to zero.

    • getStatePositionX

      public double getStatePositionX()
      Obtains current x-position of the device expressed in meters (m).
      Returns:
      x-position of the device expressed in meters (m).

    • getStatePositionY

      public double getStatePositionY()
      Obtains current y-position of the device expressed in meters (m).
      Returns:
      y-position of the device expressed in meters (m).

    • getStatePositionZ

      public double getStatePositionZ()
      Obtains current z-position of the device expressed in meters (m).
      Returns:
      z-position of the device expressed in meters (m).

    • getStatePosition

      public double[] getStatePosition()
      Gets x,y,z coordinates of the device position expressed in meters (m).
      Returns:
      position of the device.

    • getStatePosition

      public void getStatePosition(double[] result)
      Gets x,y,z coordinates of the device position expressed in meters (m) and stores the result into provided array.
      Parameters:
      result - array where position coordinates will be stored.
      Throws:
      IllegalArgumentException - if the array does not have length 3.

    • getStateVelocityX

      public double getStateVelocityX()
      Gets current linear velocity of the device along x-axis expressed in meters per second (m/s).
      Returns:
      current velocity along x-axis expressed in meters per second (m/s).

    • getStateVelocityY

      public double getStateVelocityY()
      Gets current linear velocity of the device along y-axis expressed in meters per second (m/s).
      Returns:
      current velocity along y-axis expressed in meters per second (m/s).

    • getStateVelocityZ

      public double getStateVelocityZ()
      Gets current linear velocity of the device along z-axis expressed in meters per second (m/s).
      Returns:
      current velocity along z-axis expressed in meters per second (m/s).

    • getStateVelocity

      public double[] getStateVelocity()
      Gets x,y,z coordinates of current linear velocity of the device expressed in meters per second (m/s).
      Returns:
      current linear velocity of the device.

    • getStateVelocity

      public void getStateVelocity(double[] result)
      Gets x,y,z coordinates of current linear velocity of the device expressed in meters per second (m/s).
      Parameters:
      result - array where linear velocity of the device will be stored.
      Throws:
      IllegalArgumentException - if result array does not have length 3.

    • getStateAccelerationX

      public double getStateAccelerationX()
      Gets current linear acceleration of the device along x-axis expressed in meters per squared second (m/s^2).
      Returns:
      linear acceleration of the device along x-axis.

    • getStateAccelerationY

      public double getStateAccelerationY()
      Gets current linear acceleration of the device along y-axis expressed in meters per squared second (m/s^2).
      Returns:
      linear acceleration of the device along y-axis.

    • getStateAccelerationZ

      public double getStateAccelerationZ()
      Gets current linear acceleration of the device along z-axis expressed in meters per squared second (m/s^2).
      Returns:
      linear acceleration of the device along z-axis.

    • getStateAcceleration

      public double[] getStateAcceleration()
      Gets current x,y,z linear acceleration coordinates of the device expressed in meters per squared second (m/s^2).
      Returns:
      current linear acceleration of the device.

    • getStateAcceleration

      public void getStateAcceleration(double[] result)
      Gets current x,y,z linear acceleration coordinates of the device expressed in meters per squared second (m/s^2).
      Parameters:
      result - array where resulting linear acceleration coordinates will be stored.
      Throws:
      IllegalArgumentException - if array does not have length 3.

    • getStateQuaternionA

      public double getStateQuaternionA()
      Gets A value of quaternion containing current device orientation.
      Returns:
      A value of quaternion containing current device orientation.

    • getStateQuaternionB

      public double getStateQuaternionB()
      Gets B value of quaternion containing current device orientation.
      Returns:
      B value of quaternion containing current device orientation.

    • getStateQuaternionC

      public double getStateQuaternionC()
      Gets C value of quaternion containing current device orientation.
      Returns:
      C value of quaternion containing current device orientation.

    • getStateQuaternionD

      public double getStateQuaternionD()
      Gets D value of quaternion containing current device orientation.
      Returns:
      D value of quaternion containing current device orientation.

    • getStateQuaternionArray

      public double[] getStateQuaternionArray()
      Gets A, B, C, D values of quaternion containing current device orientation.
      Returns:
      A, B, C, D values of quaternion containing current device orientation.

    • getStateQuaternionArray

      public void getStateQuaternionArray(double[] result)
      Gets A, B, C, D values of quaternion containing current device orientation.
      Parameters:
      result - array where A, B, C, D values of quaternion will be stored. Must have length 4.
      Throws:
      IllegalArgumentException - if provided array does not have length 4.

    • getStateQuaternion

      public com.irurueta.geometry.Quaternion getStateQuaternion()
      Gets quaternion containing current device orientation.
      Returns:
      quaternion containing current device orientation.

    • getStateQuaternion

      public void getStateQuaternion(com.irurueta.geometry.Quaternion result)
      Gets quaternion containing current device orientation.
      Parameters:
      result - instance where quaternion data will be stored.

    • getStateAngularSpeedX

      public double getStateAngularSpeedX()
      Gets angular speed along x-axis expressed in radians per second (rad/s).
      Returns:
      angular speed along x-axis.

    • getStateAngularSpeedY

      public double getStateAngularSpeedY()
      Gets angular speed along y-axis expressed in radians per second (rad/s).
      Returns:
      angular speed along y-axis.

    • getStateAngularSpeedZ

      public double getStateAngularSpeedZ()
      Gets angular speed along z-axis expressed in radians per second (rad/s).
      Returns:
      angular speed along z-axis.

    • getStateAngularSpeed

      public double[] getStateAngularSpeed()
      Gets angular speed of the device along x,y,z axes expressed in radians per second (rad/s).
      Returns:
      device's angular speed.

    • getStateAngularSpeed

      public void getStateAngularSpeed(double[] result)
      Gets angular speed of the device along x,y,z axes expressed in radians per second (rad/s) and stores the result into provided array.
      Parameters:
      result - array where angular speed will be stored. Must have length 3.
      Throws:
      IllegalArgumentException - if provided array does not have length 3.

    • getStateCovariance

      public abstract com.irurueta.algebra.Matrix getStateCovariance()
      Gets covariance matrix of state variables (position, velocity, acceleration, orientation and angular speed). Actual meaning of elements in returned matrix will depend on actual implementation of the estimator.
      Returns:
      covariance matrix of state variables.

    • hasError

      public boolean hasError()
      Indicates whether an error occurred during the estimation. If an error occurs the estimator should be restarted since state values might become unreliable.
      Returns:
      true if an error occurred since last start time, false otherwise.

    • isAccumulationEnabled

      public boolean isAccumulationEnabled()
      Indicates whether accumulation of samples is enabled or not.
      Returns:
      true if accumulation of samples is enabled, false otherwise.

    • setAccumulationEnabled

      public void setAccumulationEnabled(boolean accumulationEnabled)
      Specifies whether accumulation of samples is enabled or not.
      Parameters:
      accumulationEnabled - true if accumulation of samples is enabled, false otherwise.

    • getAccelerometerTimestampNanos

      public long getAccelerometerTimestampNanos()
      Gets timestamp expressed in nanoseconds since the epoch time of the last accelerometer sample, or -1 if no sample has been set yet.
      Returns:
      timestamp expressed in nanoseconds since the epoch time of the last accelerometer sample, or -1.

    • getGyroscopeTimestampNanos

      public long getGyroscopeTimestampNanos()
      Gets timestamp expressed in nanoseconds since the epoch time of the last gyroscope sample, or -1 if no sample has been set yet.
      Returns:
      timestamp expressed in nanoseconds since the epoch time of the last gyroscope sample, or -1.

    • getAccumulatedAccelerometerSamples

      public int getAccumulatedAccelerometerSamples()
      Gets number of accelerometer samples accumulated since last full sample.
      Returns:
      number of accelerometer samples accumulated since last full sample.

    • getAccumulatedGyroscopeSamples

      public int getAccumulatedGyroscopeSamples()
      Gets number of gyroscope samples accumulated since last full sample.
      Returns:
      number of gyroscope samples accumulated since last full sample.

    • isAccelerometerSampleReceived

      public boolean isAccelerometerSampleReceived()
      Indicates whether the accelerometer sample has been received since the last full sample (accelerometer + gyroscope).
      Returns:
      true if accelerometer sample has been received, false otherwise.

    • isGyroscopeSampleReceived

      public boolean isGyroscopeSampleReceived()
      Indicates whether the gyroscope sample has been received since the last full sample (accelerometer + gyroscope).
      Returns:
      true if gyroscope sample has been received, false otherwise.

    • isFullSampleAvailable

      public boolean isFullSampleAvailable()
      Indicates whether a full sample (accelerometer + gyroscope) has been received or not.
      Returns:
      true if full sample has been received, false otherwise.

    • getAccumulatedAccelerationSampleX

      public double getAccumulatedAccelerationSampleX()
      Gets average acceleration along x-axis accumulated since last full sample. Expressed in meters per squared second (m/s^2).
      Returns:
      average acceleration along x-axis accumulated since last full sample.

    • getAccumulatedAccelerationSampleY

      public double getAccumulatedAccelerationSampleY()
      Gets average acceleration along y-axis accumulated since last full sample. Expressed in meters per squared second (m/s^2).
      Returns:
      average acceleration along y-axis accumulated since last full sample.

    • getAccumulatedAccelerationSampleZ

      public double getAccumulatedAccelerationSampleZ()
      Gets average acceleration along z-axis accumulated since last full sample. Expressed in meters per squared second (m/s^2).
      Returns:
      average acceleration along z-axis accumulated since last full sample.

    • getAccumulatedAccelerationSample

      public double[] getAccumulatedAccelerationSample()
      Gets average acceleration along x,y,z axes accumulated since last full sample. Expressed in meters per squared second (m/s^2).
      Returns:
      average acceleration along x,y,z axes expressed in meters per squared second (m/s^2).

    • getAccumulatedAccelerationSample

      public void getAccumulatedAccelerationSample(double[] result)
      Gets average acceleration along x,y,z axes accumulated since last full sample. Expressed in meters per squared second (m/s^2).
      Parameters:
      result - array where average acceleration along x,y,z axes will be stored.
      Throws:
      IllegalArgumentException - if provided array does not have length 3.

    • getAccumulatedAngularSpeedSampleX

      public double getAccumulatedAngularSpeedSampleX()
      Gets average angular speed along x-axis accumulated since last full sample. Expressed in radians per second (rad/s).
      Returns:
      average angular speed along x-axis expressed in radians per second (rad/s).

    • getAccumulatedAngularSpeedSampleY

      public double getAccumulatedAngularSpeedSampleY()
      Gets average angular speed along y-axis accumulated since last full sample. Expressed in radians per second (rad/s).
      Returns:
      average angular speed along y-axis expressed in radians per second (rad/s).

    • getAccumulatedAngularSpeedSampleZ

      public double getAccumulatedAngularSpeedSampleZ()
      Gets average angular speed along z-axis accumulated since last full sample. Expressed in radians per second (rad/s).
      Returns:
      average angular speed along z-axis expressed in radians per second.

    • getAccumulatedAngularSpeedSample

      public double[] getAccumulatedAngularSpeedSample()
      Gets average angular speed along x,y,z axes accumulated since last full sample. Expressed in radians per second (rad/s).
      Returns:
      average angular speed along x,y,z axes expressed in radians per second.

    • getAccumulatedAngularSpeedSample

      public void getAccumulatedAngularSpeedSample(double[] result)
      Gets average angular speed along x,y,z axes accumulated since last full sample. Expressed in radians per second (rad/s).
      Parameters:
      result - array where average angular speed along x,y,z axes will be stored.
      Throws:
      IllegalArgumentException - if provided array does not have length 3.

    • updateAccelerometerSample

      public void updateAccelerometerSample(long timestamp, float accelerationX, float accelerationY, float accelerationZ)
      Provides a new accelerometer sample. If accumulation is enabled, samples are averaged until a full sample is received. When a full sample (accelerometer + gyroscope) is received, internal state gets also updated.
      Parameters:
      timestamp - timestamp of accelerometer sample since epoch time and expressed in nanoseconds.
      accelerationX - linear acceleration along x-axis expressed in meters per squared second (m/s^2).
      accelerationY - linear acceleration along y-axis expressed in meters per squared second (m/s^2).
      accelerationZ - linear acceleration along z-axis expressed in meters per squared second (m/s^2).

    • updateAccelerometerSample

      public void updateAccelerometerSample(long timestamp, float[] data)
      Provides a new accelerometer sample. If accumulation is enabled, samples are averaged until a full sample is received. When a full sample (accelerometer + gyroscope) is received, internal state gets also updated.
      Parameters:
      timestamp - timestamp of accelerometer sample since epoch time and expressed in nanoseconds.
      data - array containing x,y,z components of linear acceleration expressed in meters per squared second (m/s^2).
      Throws:
      IllegalArgumentException - if provided array does not have length 3.

    • updateGyroscopeSample

      public void updateGyroscopeSample(long timestamp, float angularSpeedX, float angularSpeedY, float angularSpeedZ)
      Provides a new gyroscope sample. If accumulation is enabled, samples are averaged until a full sample is received. When a full sample (accelerometer + gyroscope) is received, internal state gets also updated.
      Parameters:
      timestamp - timestamp of gyroscope sample since epoch time and expressed in nanoseconds.
      angularSpeedX - angular speed of rotation along x-axis expressed in radians per second (rad/s).
      angularSpeedY - angular speed of rotation along y-axis expressed in radians per second (rad/s).
      angularSpeedZ - angular speed of rotation along z-axis expressed in radians per second (rad/s).

    • updateGyroscopeSample

      public void updateGyroscopeSample(long timestamp, float[] data)
      Provides a new gyroscope sample. If accumulation is enabled, samples are averaged until a full sample is received. When a full sample (accelerometer + gyroscope) is received, internal state gets also updated.
      Parameters:
      timestamp - timestamp of gyroscope sample since epoch time and expressed in nanoseconds.
      data - angular speed of rotation along x,y,z axes expressed in radians per second (rad/s).
      Throws:
      IllegalArgumentException - if provided array does not have length 3.

    • getMostRecentTimestampNanos

      public long getMostRecentTimestampNanos()
      Gets most recent timestamp of received partial samples (accelerometer or gyroscope).
      Returns:
      most recent timestamp of received partial sample.

    • correctWithPositionMeasure

      public void correctWithPositionMeasure(double positionX, double positionY, double positionZ, com.irurueta.algebra.Matrix positionCovariance)
      Corrects system state with provided position measure having an accuracy determined by provided covariance matrix.
      Parameters:
      positionX - new position along x-axis expressed in meters (m).
      positionY - new position along y-axis expressed in meters (m).
      positionZ - new position along z-axis expressed in meters (m).
      positionCovariance - new position covariance matrix determining new position accuracy or null if last available covariance does not need to be updated.
      Throws:
      IllegalArgumentException - if provided covariance matrix is not 3x3.

    • correctWithPositionMeasure

      public void correctWithPositionMeasure(double[] position, com.irurueta.algebra.Matrix positionCovariance)
      Corrects system state with provided position measure having an accuracy determined by provided covariance matrix.
      Parameters:
      position - x,y,z coordinates of position expressed in meters (m). Must have length 3.
      positionCovariance - new position covariance matrix determining new position accuracy or null if last available covariance does not need to be updated.
      Throws:
      IllegalArgumentException - if provided covariance matrix is not 3x3 or if provided position array does not have length 3.

    • correctWithPositionMeasure

      public void correctWithPositionMeasure(com.irurueta.geometry.Point3D position, com.irurueta.algebra.Matrix positionCovariance)
      Corrects system state with provided position measure having an accuracy determined by provided covariance matrix.
      Parameters:
      position - position expressed in meters (m).
      positionCovariance - new position covariance matrix determining new position accuracy or null if last available covariance does not need to be updated.
      Throws:
      IllegalArgumentException - if provided covariance matrix is not 3x3.

    • setPositionCovarianceMatrix

      public abstract void setPositionCovarianceMatrix(com.irurueta.algebra.Matrix positionCovariance)
      Updates covariance matrix of position measures. If null is provided, covariance matrix is not updated.
      Parameters:
      positionCovariance - new position covariance determining position accuracy or null if last available covariance does not need to be updated.
      Throws:
      IllegalArgumentException - if provided covariance matrix is not 3x3.

    • getPositionCovarianceMatrix

      public abstract com.irurueta.algebra.Matrix getPositionCovarianceMatrix()
      Gets current covariance matrix of position measures determining current accuracy of provided position measures.
      Returns:
      covariance matrix of position measures.

    • correctWithPositionMeasure

      public abstract void correctWithPositionMeasure(double positionX, double positionY, double positionZ)
      Corrects system state with provided position measure using current position accuracy.
      Parameters:
      positionX - new position along x-axis expressed in meters (m).
      positionY - new position along y-axis expressed in meters (m).
      positionZ - new position along z-axis expressed in meters (m).

    • correctWithPositionMeasure

      public void correctWithPositionMeasure(double[] position)
      Corrects system state with provided position measure using current position accuracy.
      Parameters:
      position - x,y,z coordinates of position expressed in meters (m). Must have length 3.
      Throws:
      IllegalArgumentException - if provided array does not have length 3.

    • correctWithPositionMeasure

      public void correctWithPositionMeasure(com.irurueta.geometry.Point3D position)
      Corrects system state with provided position measure using current position accuracy.
      Parameters:
      position - position expressed in meters (m).

    • getListener

      public BaseSlamEstimator.BaseSlamEstimatorListener<D> getListener()
      Gets listener in charge of handling events raised by instances of this class.
      Returns:
      listener in charge of handling events raised by instances of this class.

    • setListener

      public void setListener(BaseSlamEstimator.BaseSlamEstimatorListener<D> listener)
      Sets listener in charge of handling events raised by instances of this class.
      Parameters:
      listener - listener in charge of handling events raised by instances of this class.

    • getCalibrationData

      public D getCalibrationData()
      Gets calibration data. When provided, its mean and covariance are used to correct control samples and adjust process covariance matrix during Kalman filtering in prediction stage.
      Returns:
      calibration data.

    • setCalibrationData

      public void setCalibrationData(D calibrationData)
      Sets calibration data. When provided, its mean and covariance are used to correct control samples and adjust process covariance matrix during Kalman filtering in prediction stage.
      Parameters:
      calibrationData - calibration data.

    • reset

      protected void reset(double statePositionX, double statePositionY, double statePositionZ, double stateVelocityX, double stateVelocityY, double stateVelocityZ, double stateAccelerationX, double stateAccelerationY, double stateAccelerationZ, double stateQuaternionA, double stateQuaternionB, double stateQuaternionC, double stateQuaternionD, double stateAngularSpeedX, double stateAngularSpeedY, double stateAngularSpeedZ)
      Resets position, linear velocity, linear acceleration, orientation and angular speed to provided values.
      Parameters:
      statePositionX - position along x-axis expressed in meters (m).
      statePositionY - position along y-axis expressed in meters (m).
      statePositionZ - position along z-axis expressed in meters (m).
      stateVelocityX - linear velocity along x-axis expressed in meters per second (m/s).
      stateVelocityY - linear velocity along y-axis expressed in meters per second (m/s).
      stateVelocityZ - linear velocity along z-axis expressed in meters per second (m/s).
      stateAccelerationX - linear acceleration along x-axis expressed in meters per squared second (m/s^2).
      stateAccelerationY - linear acceleration along y-axis expressed in meters per squared second (m/s^2).
      stateAccelerationZ - linear acceleration along z-axis expressed in meters per squared second (m/s^2).
      stateQuaternionA - A value of orientation quaternion.
      stateQuaternionB - B value of orientation quaternion.
      stateQuaternionC - C value of orientation quaternion.
      stateQuaternionD - D value of orientation quaternion.
      stateAngularSpeedX - angular speed along x-axis expressed in radians per second (rad/s).
      stateAngularSpeedY - angular speed along y-axis expressed in radians per second (rad/s).
      stateAngularSpeedZ - angular speed along z-axis expressed in radians per second (rad/s).

    • notifyFullSampleAndResetSampleReceive

      protected void notifyFullSampleAndResetSampleReceive()
      Notifies that a full sample has been received and resets flags indicating whether partial samples have been received.

    • processFullSample

      protected abstract void processFullSample()
      Method to be implemented in subclasses to process a full sample.