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

Class SuhQuaternionStepIntegrator

java.lang.Object
com.irurueta.navigation.inertial.calibration.gyroscope.QuaternionStepIntegrator
com.irurueta.navigation.inertial.calibration.gyroscope.SuhQuaternionStepIntegrator
public class SuhQuaternionStepIntegrator extends QuaternionStepIntegrator
Computes an integration step of a quaternion using Suh's method. More information available here: Young Soo Suh. "Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of external acceleration". 2010.

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    private com.irurueta.algebra.Matrix
    constant
    Constant matrix to be reused.
    private com.irurueta.algebra.Matrix
    identity
    Identity matrix to be reused.
    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
    omegaSkew0
    Skew matrix of omega0 to be reused.
    private com.irurueta.algebra.Matrix
    omegaSkew0A
    Temporary matrix to be reused.
    private com.irurueta.algebra.Matrix
    omegaSkew1
    Skew matrix of omega1 to be reused.
    private com.irurueta.algebra.Matrix
    omegaSkew10
    Temporary matrix to be reused.
    private com.irurueta.algebra.Matrix
    omegaSkew1A
    Temporary matrix to be reused.
    private com.irurueta.algebra.Matrix
    omegaSkew1B
    Temporary matrix 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 static final double
    THREE_FOURTHS
    Precomputed 3/4 factor
    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
    SuhQuaternionStepIntegrator()
    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 Suh'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 Suh'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.algebra.Matrix omega0, com.irurueta.algebra.Matrix omega1, com.irurueta.algebra.Matrix quat, com.irurueta.algebra.Matrix omegaSkew0, com.irurueta.algebra.Matrix omegaSkew1, com.irurueta.algebra.Matrix constant, com.irurueta.algebra.Matrix omegaSkew10, com.irurueta.algebra.Matrix identity, com.irurueta.algebra.Matrix omegaSkew1A, com.irurueta.algebra.Matrix omegaSkew0A, com.irurueta.algebra.Matrix omegaSkew1B, com.irurueta.algebra.Matrix tmp, com.irurueta.algebra.Matrix quatResult)
    Internal method computing an integration step using Suh'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

    • THREE_FOURTHS

      private static final double THREE_FOURTHS
      Precomputed 3/4 factor
      See Also:

    • 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.

    • quat

      private com.irurueta.algebra.Matrix quat
      Initial attitude 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.

    • constant

      private com.irurueta.algebra.Matrix constant
      Constant matrix to be reused.

    • omegaSkew10

      private com.irurueta.algebra.Matrix omegaSkew10
      Temporary matrix to be reused.

    • identity

      private com.irurueta.algebra.Matrix identity
      Identity matrix to be reused.

    • omegaSkew1A

      private com.irurueta.algebra.Matrix omegaSkew1A
      Temporary matrix to be reused.

    • omegaSkew0A

      private com.irurueta.algebra.Matrix omegaSkew0A
      Temporary matrix to be reused.

    • omegaSkew1B

      private com.irurueta.algebra.Matrix omegaSkew1B
      Temporary matrix to be reused.

    • tmp

      private com.irurueta.algebra.Matrix tmp
      Temporary 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

    • SuhQuaternionStepIntegrator

      public SuhQuaternionStepIntegrator()
      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 Suh's integration step. More information available here: Young Soo Suh. "Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of external acceleration". 2010.
      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 Suh's integration step. More information available here: Young Soo Suh. "Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of external acceleration". 2010.
      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 quat, com.irurueta.algebra.Matrix omegaSkew0, com.irurueta.algebra.Matrix omegaSkew1, com.irurueta.algebra.Matrix constant, com.irurueta.algebra.Matrix omegaSkew10, com.irurueta.algebra.Matrix identity, com.irurueta.algebra.Matrix omegaSkew1A, com.irurueta.algebra.Matrix omegaSkew0A, com.irurueta.algebra.Matrix omegaSkew1B, com.irurueta.algebra.Matrix tmp, com.irurueta.algebra.Matrix quatResult) throws com.irurueta.geometry.RotationException
      Internal method computing an integration step using Suh'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.
      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.
      quat - initial attitude to be reused. Must be 4x1.
      omegaSkew0 - skew matrix of omega0 to be reused. Must be 4x4.
      omegaSkew1 - skew matrix of omega1 to be reused. Must be 4x4.
      constant - constant matrix to be reused. Must be 4x4.
      omegaSkew10 - temporary matrix to be reused. Must be 4x4.
      identity - identity matrix to be reused. Must be 4x4.
      omegaSkew1A - temporary matrix to be reused. Must be 4x4.
      omegaSkew0A - temporary matrix to be reused. Must be 4x4.
      omegaSkew1B - temporary matrix to be reused. Must be 4x4.
      tmp - temporary 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.