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

Class MidPointQuaternionStepIntegrator

java.lang.Object

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

com.irurueta.navigation.inertial.calibration.gyroscope.MidPointQuaternionStepIntegrator

public class MidPointQuaternionStepIntegrator
extends QuaternionStepIntegrator

Computes an integration step of a quaternion using mid-point algorithm. More information available here: https://en.wikipedia.org/wiki/Midpoint_method

Field Summary

Fields

Modifier and Type	Field	Description
private com.irurueta.algebra.Matrix	k1	Quaternion derivative at initial timestamp t0 to be reused.
private com.irurueta.algebra.Matrix	k2	Quaternion derivative at mid-point timestamp between t0 and t1 to be reused.
private com.irurueta.algebra.Matrix	omega0	Angular speed at initial timestamp t0 to be reused.
private com.irurueta.algebra.Matrix	omega01	Angular speed at mid-point timestamp between t0 and t1 to be reused.
private com.irurueta.algebra.Matrix	omega1	Angular speed at end timestamp t1 to be reused.
private com.irurueta.algebra.Matrix	omegaSkew	Skew antisymmetric matrix used for quaternion time derivative computation 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	tmpQ	Temporal quaternion used to compute additional slopes that is being reused.

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

DEFAULT_TYPE

Constructor Summary

Constructors

Constructor	Description
MidPointQuaternionStepIntegrator()	Constructor.

Method Summary

Modifier and Type	Method	Description
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 a mid-point 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 a mid-point 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.algebra.Matrix omega0, com.irurueta.algebra.Matrix omega1, com.irurueta.algebra.Matrix omega01, com.irurueta.algebra.Matrix quat, com.irurueta.algebra.Matrix quatResult, com.irurueta.algebra.Matrix tmpQ, com.irurueta.algebra.Matrix k1, com.irurueta.algebra.Matrix k2, com.irurueta.algebra.Matrix omegaSkew)	Internal method computing an integration step using mid-point 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

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.

omega01

private com.irurueta.algebra.Matrix omega01

Angular speed at mid-point timestamp between t0 and t1 to be reused.

quat

private com.irurueta.algebra.Matrix quat

Initial attitude to be reused.

quatResult

private com.irurueta.algebra.Matrix quatResult

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

tmpQ

private com.irurueta.algebra.Matrix tmpQ

Temporal quaternion used to compute additional slopes that is being reused.

k1

private com.irurueta.algebra.Matrix k1

Quaternion derivative at initial timestamp t0 to be reused.

k2

private com.irurueta.algebra.Matrix k2

Quaternion derivative at mid-point timestamp between t0 and t1 to be reused.

omegaSkew

private com.irurueta.algebra.Matrix omegaSkew

Skew antisymmetric matrix used for quaternion time derivative computation to be reused.

Constructor Details

MidPointQuaternionStepIntegrator

public MidPointQuaternionStepIntegrator()

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 a mid-point integration step. More information available here: https://en.wikipedia.org/wiki/Midpoint_method

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 a mid-point integration step. More information available here: https://en.wikipedia.org/wiki/Midpoint_method

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.algebra.Matrix omega0,
 com.irurueta.algebra.Matrix omega1,
 com.irurueta.algebra.Matrix omega01,
 com.irurueta.algebra.Matrix quat,
 com.irurueta.algebra.Matrix quatResult,
 com.irurueta.algebra.Matrix tmpQ,
 com.irurueta.algebra.Matrix k1,
 com.irurueta.algebra.Matrix k2,
 com.irurueta.algebra.Matrix omegaSkew)
                             throws com.irurueta.geometry.RotationException

Internal method computing an integration step using mid-point 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.

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.

omega01 - angular speed at mid-point timestamp between t0 and t1 to be reused. Must be 3x1.

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

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

tmpQ - temporal quaternion used to compute additional slopes that is being reused. Must be 4x1.

k1 - slope of quaternion derivative at initial timestamp t0 to be reused. Must be 4x1.

k2 - slope of quaternion derivative at mid-point timestamp between t0 and t1 to be reused. Must be 4x1.

omegaSkew - skew antisymmetric matrix used for quaternion time derivative computation to be reused. Must be 4x4.

Throws:

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