Package com.irurueta.navigation.inertial.calibration.gyroscope

Class TrawnyQuaternionStepIntegrator

java.lang.Object

com.irurueta.navigation.inertial.calibration.gyroscope.QuaternionStepIntegrator

com.irurueta.navigation.inertial.calibration.gyroscope.TrawnyQuaternionStepIntegrator

public class TrawnyQuaternionStepIntegrator
extends QuaternionStepIntegrator

Computes an integration step of a quaternion using Suh's method. More information available here: Trawny, N. "Indirect Kalman Filter for 3D Attitude Estimation". 2005.

Field Summary

Fields

Modifier and Type	Field	Description
private com.irurueta.algebra.Matrix	a	Temporary matrix to be reused.
private com.irurueta.algebra.Matrix	b	Temporary matrix to be reused.
private com.irurueta.algebra.Matrix	dotOmega	Derivative of angular speed to be reused.
private com.irurueta.algebra.Matrix	expA	Exponential matrix to be reused.
private final com.irurueta.numerical.ExponentialMatrixEstimator	exponentialMatrixEstimator	Estimates exponential of a square matrix.
private com.irurueta.algebra.Matrix	omega0	Angular speed at initial timestamp t0 to be reused.
private com.irurueta.algebra.Matrix	omega1	Angular speed at end timestamp t1 to be reused.
private com.irurueta.algebra.Matrix	omegaAvgOmega	Average skew of angular speeds omega0 and omega1 to be reused.
private com.irurueta.algebra.Matrix	omegaSkew0	Skew matrix of omega0 to be reused.
private com.irurueta.algebra.Matrix	omegaSkew1	Skew matrix of omega1 to be reused.
private com.irurueta.algebra.Matrix	omegaSkewDotOmega	Skew matrix of derivative of angular speed to be reused.
private com.irurueta.algebra.Matrix	quat	Initial attitude to be reused.
private com.irurueta.algebra.Matrix	quatResult	Instance where result of integration is stored in matrix form being reused.
private com.irurueta.algebra.Matrix	tmp	Temporary matrix to be reused.

Fields inherited from class com.irurueta.navigation.inertial.calibration.gyroscope.QuaternionStepIntegrator

DEFAULT_TYPE

Constructor Summary

Constructors

Constructor	Description
TrawnyQuaternionStepIntegrator()	Constructor.

Method Summary

Modifier and Type	Method	Description
private static void	computeOmegaAvgOmega(com.irurueta.algebra.Matrix omega0, com.irurueta.algebra.Matrix omega1, double dt, com.irurueta.algebra.Matrix omegaSkew1, com.irurueta.algebra.Matrix result, com.irurueta.algebra.Matrix dotOmega, com.irurueta.algebra.Matrix omegaSkewDotOmega)	Computes the skew antisymmetric matrix of a vector matrix containing angular speed.
QuaternionStepIntegratorType	getType()	Gets type of this integrator.
void	integrate(com.irurueta.geometry.Quaternion initialAttitude, double initialWx, double initialWy, double initialWz, double currentWx, double currentWy, double currentWz, double dt, com.irurueta.geometry.Quaternion result)	Performs Trawny's integration step.
static void	integrationStep(com.irurueta.geometry.Quaternion initialAttitude, double initialWx, double initialWy, double initialWz, double currentWx, double currentWy, double currentWz, double dt, com.irurueta.geometry.Quaternion result)	Performs Trawny's integration step.
private static void	integrationStep(com.irurueta.geometry.Quaternion initialAttitude, double initialWx, double initialWy, double initialWz, double currentWx, double currentWy, double currentWz, double dt, com.irurueta.geometry.Quaternion result, com.irurueta.numerical.ExponentialMatrixEstimator exponentialMatrixEstimator, com.irurueta.algebra.Matrix quat, com.irurueta.algebra.Matrix omega0, com.irurueta.algebra.Matrix omega1, com.irurueta.algebra.Matrix omegaSkew0, com.irurueta.algebra.Matrix omegaSkew1, com.irurueta.algebra.Matrix omegaAvgOmega, com.irurueta.algebra.Matrix dotOmega, com.irurueta.algebra.Matrix omegaSkewDotOmega, com.irurueta.algebra.Matrix a, com.irurueta.algebra.Matrix b, com.irurueta.algebra.Matrix tmp, com.irurueta.algebra.Matrix expA, com.irurueta.algebra.Matrix quatResult)	Internal method computing an integration step using Trawny's algorithm.

Methods inherited from class com.irurueta.navigation.inertial.calibration.gyroscope.QuaternionStepIntegrator

computeAverageAngularSpeed, computeOmegaSkew, computeTimeDerivative, copyAngularSpeedToMatrix, create, create

Methods inherited from class java.lang.Object

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

Field Details

exponentialMatrixEstimator

private final com.irurueta.numerical.ExponentialMatrixEstimator exponentialMatrixEstimator

Estimates exponential of a square matrix.

quat

private com.irurueta.algebra.Matrix quat

Initial attitude to be reused.

omega0

private com.irurueta.algebra.Matrix omega0

Angular speed at initial timestamp t0 to be reused.

omega1

private com.irurueta.algebra.Matrix omega1

Angular speed at end timestamp t1 to be reused.

omegaSkew0

private com.irurueta.algebra.Matrix omegaSkew0

Skew matrix of omega0 to be reused.

omegaSkew1

private com.irurueta.algebra.Matrix omegaSkew1

Skew matrix of omega1 to be reused.

omegaAvgOmega

private com.irurueta.algebra.Matrix omegaAvgOmega

Average skew of angular speeds omega0 and omega1 to be reused.

dotOmega

private com.irurueta.algebra.Matrix dotOmega

Derivative of angular speed to be reused.

omegaSkewDotOmega

private com.irurueta.algebra.Matrix omegaSkewDotOmega

Skew matrix of derivative of angular speed to be reused.

a

private com.irurueta.algebra.Matrix a

Temporary matrix to be reused.

b

private com.irurueta.algebra.Matrix b

Temporary matrix to be reused.

tmp

private com.irurueta.algebra.Matrix tmp

Temporary matrix to be reused.

expA

private com.irurueta.algebra.Matrix expA

Exponential matrix to be reused.

quatResult

private com.irurueta.algebra.Matrix quatResult

Instance where result of integration is stored in matrix form being reused.

Constructor Details

TrawnyQuaternionStepIntegrator

public TrawnyQuaternionStepIntegrator()

Constructor. Initializes matrices being reused.

Method Details

getType

public QuaternionStepIntegratorType getType()

Gets type of this integrator.

Specified by:

getType in class QuaternionStepIntegrator

Returns:

indicates type of this integrator.

integrate

public void integrate(com.irurueta.geometry.Quaternion initialAttitude,
 double initialWx,
 double initialWy,
 double initialWz,
 double currentWx,
 double currentWy,
 double currentWz,
 double dt,
 com.irurueta.geometry.Quaternion result)
               throws com.irurueta.geometry.RotationException

Performs Trawny's integration step. More information available here: Trawny, N. "Indirect Kalman Filter for 3D Attitude Estimation". 2005.

Specified by:

integrate in class QuaternionStepIntegrator

Parameters:

initialAttitude - initial attitude.

initialWx - initial x-coordinate rotation velocity at initial timestamp expressed in radians per second (rad/s).

initialWy - initial y-coordinate rotation velocity at initial timestamp expressed in radians per second (rad/s).

initialWz - initial z-coordinate rotation velocity at initial timestamp expressed in radians per second (rad/s).*

currentWx - end x-coordinate rotation velocity at current timestamp expressed in radians per second (rad/s).

currentWy - end y-coordinate rotation velocity at current timestamp expressed in radians per second (rad/s).

currentWz - end z-coordinate rotation velocity at current timestamp expressed in radians per second (rad/s).

dt - time step expressed in seconds.

result - instance where result of integration will be stored.

Throws:

com.irurueta.geometry.RotationException - if a numerical error occurs.

integrationStep

public static void integrationStep(com.irurueta.geometry.Quaternion initialAttitude,
 double initialWx,
 double initialWy,
 double initialWz,
 double currentWx,
 double currentWy,
 double currentWz,
 double dt,
 com.irurueta.geometry.Quaternion result)
                            throws com.irurueta.geometry.RotationException

Performs Trawny's integration step. More information available here: Trawny, N. "Indirect Kalman Filter for 3D Attitude Estimation". 2005.

Parameters:

initialAttitude - initial attitude.

initialWx - initial x-coordinate rotation velocity at initial timestamp expressed in radians per second (rad/s).

initialWy - initial y-coordinate rotation velocity at initial timestamp expressed in radians per second (rad/s).

initialWz - initial z-coordinate rotation velocity at initial timestamp expressed in radians per second (rad/s).

currentWx - end x-coordinate rotation velocity at end timestamp expressed in radians per second (rad/s).

currentWy - end y-coordinate rotation velocity at end timestamp expressed in radians per second (rad/s).

currentWz - end z-coordinate rotation velocity at end timestamp expressed in radians per second (rad/s).

dt - time step expressed in seconds (t1 - t0).

result - instance where result of integration will be stored.

Throws:

com.irurueta.geometry.RotationException - if a numerical error occurs.

integrationStep

private static void integrationStep(com.irurueta.geometry.Quaternion initialAttitude,
 double initialWx,
 double initialWy,
 double initialWz,
 double currentWx,
 double currentWy,
 double currentWz,
 double dt,
 com.irurueta.geometry.Quaternion result,
 com.irurueta.numerical.ExponentialMatrixEstimator exponentialMatrixEstimator,
 com.irurueta.algebra.Matrix quat,
 com.irurueta.algebra.Matrix omega0,
 com.irurueta.algebra.Matrix omega1,
 com.irurueta.algebra.Matrix omegaSkew0,
 com.irurueta.algebra.Matrix omegaSkew1,
 com.irurueta.algebra.Matrix omegaAvgOmega,
 com.irurueta.algebra.Matrix dotOmega,
 com.irurueta.algebra.Matrix omegaSkewDotOmega,
 com.irurueta.algebra.Matrix a,
 com.irurueta.algebra.Matrix b,
 com.irurueta.algebra.Matrix tmp,
 com.irurueta.algebra.Matrix expA,
 com.irurueta.algebra.Matrix quatResult)
                             throws com.irurueta.geometry.RotationException

Internal method computing an integration step using Trawny's algorithm. This method is used internally so that reusable instances can be provided as parameters.

Parameters:

initialAttitude - initial attitude.

initialWx - initial x-coordinate rotation velocity at initial timestamp expressed in radians per second (rad/s).

initialWy - initial y-coordinate rotation velocity at initial timestamp expressed in radians per second (rad/s).

initialWz - initial z-coordinate rotation velocity at initial timestamp expressed in radians per second (rad/s).

currentWx - end x-coordinate rotation velocity at end timestamp expressed in radians per second (rad/s).

currentWy - end y-coordinate rotation velocity at end timestamp expressed in radians per second (rad/s).

currentWz - end z-coordinate rotation velocity at end timestamp expressed in radians per second (rad/s).

dt - time step expressed in seconds (t1 - t0).

result - instance where result of integration will be stored.

quat - initial attitude to be reused. Must be 4x1.

omega0 - angular speed at initial timestamp t0 to be reused. Must be 3x1.

omega1 - angular speed at end timestamp t1 to be reused. Must be 3x1.

omegaSkew0 - skew matrix of omega0 to be reused. Must be 4x4.

omegaSkew1 - skew matrix of omega1 to be reused. Must be 4x4.

omegaAvgOmega - average skew of angular speeds omega0 and omega1 to be reused. Must be 4x4.

dotOmega - derivative of angular speed to be reused. Must be 3x1.

omegaSkewDotOmega - skew matrix of derivative of angular speed to be reused. Must be 4x4.

a - temporary matrix to be reused. Must be 4x4.

b - temporary matrix to be reused. Must be 4x4.

tmp - temporary matrix to be reused. Must be 4x4.

expA - exponential matrix to be reused. Must be 4x4.

quatResult - instance where result of integration is stored in matrix form being reused. Must be 4x1.

Throws:

com.irurueta.geometry.RotationException - if a numerical error occurs.

computeOmegaAvgOmega

private static void computeOmegaAvgOmega(com.irurueta.algebra.Matrix omega0,
 com.irurueta.algebra.Matrix omega1,
 double dt,
 com.irurueta.algebra.Matrix omegaSkew1,
 com.irurueta.algebra.Matrix result,
 com.irurueta.algebra.Matrix dotOmega,
 com.irurueta.algebra.Matrix omegaSkewDotOmega)
                                  throws com.irurueta.algebra.AlgebraException

Computes the skew antisymmetric matrix of a vector matrix containing angular speed.

Parameters:

omega0 - column vector matrix containing angular speed. Must be 3x1.

omega1 - column vector matrix containing angular speed. Must be 3x1.

dt - time step expressed in seconds.

omegaSkew1 - instance being reused containing the skew antisymmetric matrix. Must be 4x4.

result - instance where result must be stored. Must be 4x4.

dotOmega - instance where computed time derivative will be stored. Must be 3x1.

omegaSkewDotOmega - instance where computed time derivative will be stored. Must be 4x4.

Throws:

com.irurueta.algebra.AlgebraException - if provided matrices do not have proper size.