Package com.irurueta.ar.slam

Class AbsoluteOrientationConstantVelocityModelSlamEstimator

java.lang.Object
com.irurueta.ar.slam.BaseSlamEstimator<AbsoluteOrientationConstantVelocityModelSlamCalibrationData>
com.irurueta.ar.slam.AbsoluteOrientationBaseSlamEstimator<AbsoluteOrientationConstantVelocityModelSlamCalibrationData>
com.irurueta.ar.slam.AbsoluteOrientationConstantVelocityModelSlamEstimator
All Implemented Interfaces:
Serializable
public class AbsoluteOrientationConstantVelocityModelSlamEstimator extends AbsoluteOrientationBaseSlamEstimator<AbsoluteOrientationConstantVelocityModelSlamCalibrationData> implements Serializable
Estimates position, velocity, acceleration, orientation and angular speed using data from accelerometer, gyroscope and magnetic field, assuming a constant velocity model. This implementation of BaseSlamEstimator is an improvement respect SlamEstimator to be able to take into account absolute orientation respect Earth frame instead of just having a relative orientation respect to the start time of the estimator.
See Also:

  • Field Details

    • STATE_LENGTH

      protected static final int STATE_LENGTH
      Internal state array length.
      See Also:

    • CONTROL_LENGTH

      protected static final int CONTROL_LENGTH
      Length of control array (changes in acceleration and angular speed).
      See Also:

    • MEASUREMENT_LENGTH

      private static final int MEASUREMENT_LENGTH
      Length of position measurement, to correct any possible deviations of the system after doing multiple predictions.
      See Also:

    • x

      private final double[] x
      Contains device status containing the following values: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, angular-velocity-x, angular-velocity-y, angular-velocity-z.

    • u

      private final double[] u
      Control signals containing the following values: quaternion-change-a, quaternion-change-b, quaternion-change-c, quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z.

    • jacobianPredictionX

      private com.irurueta.algebra.Matrix jacobianPredictionX
      Jacobian respect x state during prediction (16x16).

    • jacobianPredictionU

      private com.irurueta.algebra.Matrix jacobianPredictionU
      Jacobian respect u control during state prediction (16x13).

    • control

      private com.irurueta.algebra.Matrix control
      Column matrix containing mU values to be passed as control values during Kalman filter prediction.

    • kalmanFilter

      private com.irurueta.numerical.signal.processing.KalmanFilter kalmanFilter
      Kalman's filter to remove effects of noise.

    • measurementMatrix

      private com.irurueta.algebra.Matrix measurementMatrix
      Matrix of size 3x13 relating system status with obtained measures. [1 0 0 0 0 0 0 0 0 0 0 0 0] [0 1 0 0 0 0 0 0 0 0 0 0 0] [0 0 1 0 0 0 0 0 0 0 0 0 0]

    • measurement

      private com.irurueta.algebra.Matrix measurement
      Measurement data for the Kalman filter in a column matrix. Contains data in the following order: [accelerationX] [accelerationY] [accelerationZ] [angularSpeedX] [angularSpeedY] [angularSpeedZ]

    • lastOrientation

      private final com.irurueta.geometry.Quaternion lastOrientation
      Last sample of absolute orientation.

    • deltaOrientation

      private final com.irurueta.geometry.Quaternion deltaOrientation
      Variation of orientation respect to last sample.

    • stateOrientation

      private final com.irurueta.geometry.Quaternion stateOrientation
      Current state orientation.

    • lastAngularSpeedX

      private double lastAngularSpeedX
      Last sample of angular speed along x-axis.

    • lastAngularSpeedY

      private double lastAngularSpeedY
      Last sample of angular speed along y-axis.

    • lastAngularSpeedZ

      private double lastAngularSpeedZ
      Last sample of angular speed along z-axis.

    • lastTimestampNanos

      private long lastTimestampNanos
      Last timestamp of a full sample expressed in nanoseconds since the epoch time.

    • predictionAvailable

      private boolean predictionAvailable
      Indicates whether a prediction has been made to initialize the internal Kalman filter. Corrections can only be requested to Kalman filter once a prediction has been made. Attempts to request corrections before having a prediction will be ignored.

  • Constructor Details

    • AbsoluteOrientationConstantVelocityModelSlamEstimator

      public AbsoluteOrientationConstantVelocityModelSlamEstimator()
      Constructor.

  • Method Details

    • getStateCovariance

      public com.irurueta.algebra.Matrix getStateCovariance()
      Gets covariance matrix of state variables (position, velocity, acceleration, orientation and angular speed). Diagonal elements of matrix returned by this method are in the following order: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Off-diagonal elements correspond to cross-correlation values of diagonal ones.
      Specified by:
      getStateCovariance in class BaseSlamEstimator<AbsoluteOrientationConstantVelocityModelSlamCalibrationData>
      Returns:
      covariance matrix of state variables.

    • setPositionCovarianceMatrix

      public void setPositionCovarianceMatrix(com.irurueta.algebra.Matrix positionCovariance)
      Updates covariance matrix of position measures. If null is provided, covariance matrix is not updated.
      Specified by:
      setPositionCovarianceMatrix in class BaseSlamEstimator<AbsoluteOrientationConstantVelocityModelSlamCalibrationData>
      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 com.irurueta.algebra.Matrix getPositionCovarianceMatrix()
      Gets current covariance matrix of position measures determining current accuracy of provided position measures.
      Specified by:
      getPositionCovarianceMatrix in class BaseSlamEstimator<AbsoluteOrientationConstantVelocityModelSlamCalibrationData>
      Returns:
      covariance matrix of position measures.

    • correctWithPositionMeasure

      public void correctWithPositionMeasure(double positionX, double positionY, double positionZ)
      Corrects system state with provided position measure using current position accuracy.
      Specified by:
      correctWithPositionMeasure in class BaseSlamEstimator<AbsoluteOrientationConstantVelocityModelSlamCalibrationData>
      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).

    • createCalibrator

      public static AbsoluteOrientationConstantVelocityModelSlamCalibrator createCalibrator()
      Creates an instance of a calibrator to be used with this SLAM estimator.
      Returns:
      a calibrator.

    • processFullSample

      protected void processFullSample()
      Processes a full sample (accelerometer + gyroscope) to update system state.
      Specified by:
      processFullSample in class BaseSlamEstimator<AbsoluteOrientationConstantVelocityModelSlamCalibrationData>

    • 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. This method implementation also resets Kalman filter state.
      Overrides:
      reset in class BaseSlamEstimator<AbsoluteOrientationConstantVelocityModelSlamCalibrationData>
      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).

    • updatePredictedState

      private void updatePredictedState(com.irurueta.algebra.Matrix state)
      Updates state data of the device by using state matrix obtained after prediction from Kalman filter. to ensure that state follows proper values (specially on quaternions), we keep x values, which have been predicted using the state predictor, which uses analytical values. We then updated x using latest Kalman filter state for next iteration on state predictor.
      Parameters:
      state - state matrix obtained from Kalman filter.

    • updateCorrectedState

      private void updateCorrectedState(com.irurueta.algebra.Matrix state)
      Updates state data of the device by using state matrix obtained from Kalman filter after correction.
      Parameters:
      state - state matrix obtained from Kalman filter.