Package com.irurueta.ar.slam

Class ConstantVelocityModelSlamEstimator

java.lang.Object

com.irurueta.ar.slam.BaseSlamEstimator<ConstantVelocityModelSlamCalibrationData>

com.irurueta.ar.slam.ConstantVelocityModelSlamEstimator

All Implemented Interfaces:

Serializable

public class ConstantVelocityModelSlamEstimator
extends BaseSlamEstimator<ConstantVelocityModelSlamCalibrationData>
implements Serializable

Estimates position, velocity, acceleration and angular speed using data from accelerometer and gyroscope and assuming a constant velocity model.

See Also:

• Serialized Form

Nested Class Summary

Nested classes/interfaces inherited from class com.irurueta.ar.slam.BaseSlamEstimator

BaseSlamEstimator.BaseSlamEstimatorListener<D extends BaseCalibrationData>

Field Summary

Fields

Modifier and Type	Field	Description
private com.irurueta.algebra.Matrix	control	Column matrix containing mU values to be passed as control values during Kalman filter prediction.
protected static final int	CONTROL_LENGTH	Length of control array (changes in acceleration and angular speed).
private com.irurueta.algebra.Matrix	jacobianPredictionU	Jacobian respect u control during state prediction (13x6).
private com.irurueta.algebra.Matrix	jacobianPredictionX	Jacobian respect x state during prediction (13x13).
private com.irurueta.numerical.signal.processing.KalmanFilter	kalmanFilter	Kalman's filter to remove effects of noise.
private double	lastAngularSpeedX	Last sample of angular speed along x-axis.
private double	lastAngularSpeedY	Last sample of angular speed along y-axis.
private double	lastAngularSpeedZ	Last sample of angular speed along z-axis.
private long	lastTimestampNanos	Last timestamp of a full sample expressed in nanoseconds since the epoch time.
private com.irurueta.algebra.Matrix	measurement	Measurement data for the Kalman filter in a column matrix.
private static final int	MEASUREMENT_LENGTH	Length of position measurement, to correct any possible deviations of the system after doing multiple predictions.
private com.irurueta.algebra.Matrix	measurementMatrix	Matrix of size 3x13 relating system status with obtained measures
private boolean	predictionAvailable	Indicates whether a prediction has been made to initialize the internal Kalman filter.
protected static final int	STATE_LENGTH	Internal state array length.
private final double[]	u	Control signals containing the following values: linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z.
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.

Fields inherited from class com.irurueta.ar.slam.BaseSlamEstimator

accelerometerTimestampNanos, accumulatedAccelerationSampleX, accumulatedAccelerationSampleY, accumulatedAccelerationSampleZ, accumulatedAccelerometerSamples, accumulatedAngularSpeedSampleX, accumulatedAngularSpeedSampleY, accumulatedAngularSpeedSampleZ, accumulatedGyroscopeSamples, accumulationEnabled, calibrationData, DEFAULT_ENABLE_SAMPLE_ACCUMULATION, error, gyroscopeTimestampNanos, listener, N_COMPONENTS_3D, NANOS_TO_SECONDS, normalDist, stateAccelerationX, stateAccelerationY, stateAccelerationZ, stateAngularSpeedX, stateAngularSpeedY, stateAngularSpeedZ, statePositionX, statePositionY, statePositionZ, stateQuaternionA, stateQuaternionB, stateQuaternionC, stateQuaternionD, stateVelocityX, stateVelocityY, stateVelocityZ

Constructor Summary

Constructors

Constructor	Description
ConstantVelocityModelSlamEstimator()	Constructor.

Method Summary

Modifier and Type	Method	Description
void	correctWithPositionMeasure(double positionX, double positionY, double positionZ)	Corrects system state with provided position measure using current position accuracy.
static ConstantVelocityModelSlamCalibrator	createCalibrator()	Creates an instance of a calibrator to be used with this SLAM estimator.
com.irurueta.algebra.Matrix	getPositionCovarianceMatrix()	Gets current covariance matrix of position measures determining current accuracy of provided position measures.
com.irurueta.algebra.Matrix	getStateCovariance()	Gets covariance matrix of state variables (position, velocity, acceleration, orientation and angular speed).
protected void	processFullSample()	Processes a full sample (accelerometer + gyroscope) to update system state.
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.
void	setPositionCovarianceMatrix(com.irurueta.algebra.Matrix positionCovariance)	Updates covariance matrix of position measures.
private void	updateCorrectedState(com.irurueta.algebra.Matrix state)	Updates state data of the device by using state matrix obtained from Kalman filter after correction.
private void	updatePredictedState(com.irurueta.algebra.Matrix state)	Updates state data of the device by using state matrix obtained after prediction from Kalman filter.

Methods inherited from class com.irurueta.ar.slam.BaseSlamEstimator

correctWithPositionMeasure, correctWithPositionMeasure, correctWithPositionMeasure, correctWithPositionMeasure, correctWithPositionMeasure, getAccelerometerTimestampNanos, getAccumulatedAccelerationSample, getAccumulatedAccelerationSample, getAccumulatedAccelerationSampleX, getAccumulatedAccelerationSampleY, getAccumulatedAccelerationSampleZ, getAccumulatedAccelerometerSamples, getAccumulatedAngularSpeedSample, getAccumulatedAngularSpeedSample, getAccumulatedAngularSpeedSampleX, getAccumulatedAngularSpeedSampleY, getAccumulatedAngularSpeedSampleZ, getAccumulatedGyroscopeSamples, getCalibrationData, getGyroscopeTimestampNanos, getListener, getMostRecentTimestampNanos, getStateAcceleration, getStateAcceleration, getStateAccelerationX, getStateAccelerationY, getStateAccelerationZ, getStateAngularSpeed, getStateAngularSpeed, getStateAngularSpeedX, getStateAngularSpeedY, getStateAngularSpeedZ, getStatePosition, getStatePosition, getStatePositionX, getStatePositionY, getStatePositionZ, getStateQuaternion, getStateQuaternion, getStateQuaternionA, getStateQuaternionArray, getStateQuaternionArray, getStateQuaternionB, getStateQuaternionC, getStateQuaternionD, getStateVelocity, getStateVelocity, getStateVelocityX, getStateVelocityY, getStateVelocityZ, hasError, isAccelerometerSampleReceived, isAccumulationEnabled, isFullSampleAvailable, isGyroscopeSampleReceived, notifyFullSampleAndResetSampleReceive, reset, resetAcceleration, resetAngularSpeed, resetOrientation, resetPosition, resetPositionAndVelocity, resetVelocity, setAccumulationEnabled, setCalibrationData, setListener, updateAccelerometerSample, updateAccelerometerSample, updateGyroscopeSample, updateGyroscopeSample

Methods inherited from class java.lang.Object

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

Field Details

STATE_LENGTH

protected static final int STATE_LENGTH

Internal state array length.

See Also:

• Constant Field Values

CONTROL_LENGTH

protected static final int CONTROL_LENGTH

Length of control array (changes in acceleration and angular speed).

See Also:

• Constant Field Values

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:

• Constant Field Values

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: 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 (13x13).

jacobianPredictionU

private com.irurueta.algebra.Matrix jacobianPredictionU

Jacobian respect u control during state prediction (13x6).

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]

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

ConstantVelocityModelSlamEstimator

public ConstantVelocityModelSlamEstimator()

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<ConstantVelocityModelSlamCalibrationData>

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<ConstantVelocityModelSlamCalibrationData>

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<ConstantVelocityModelSlamCalibrationData>

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<ConstantVelocityModelSlamCalibrationData>

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 ConstantVelocityModelSlamCalibrator 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<ConstantVelocityModelSlamCalibrationData>

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<ConstantVelocityModelSlamCalibrationData>

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.