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

Class YuanQuaternionStepIntegrator

java.lang.Object
com.irurueta.navigation.inertial.calibration.gyroscope.QuaternionStepIntegrator
com.irurueta.navigation.inertial.calibration.gyroscope.YuanQuaternionStepIntegrator
public class YuanQuaternionStepIntegrator extends QuaternionStepIntegrator
Computes an integration step of a quaternion using Suh's method. More information available here: Yuan, S. "Quaternion-based Unscented Kalman Filter for Real-time". 2015,

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    private com.irurueta.algebra.Matrix
    a
    Temporary matrix to be reused.
    private static final double
    EPSILON
     
    private com.irurueta.algebra.Matrix
    identity
    Identity matrix to be reused.
    private com.irurueta.algebra.Matrix
    omega
    Angular speed at initial timestamp t0 to be reused.
    private com.irurueta.algebra.Matrix
    quat
    Angular speed at initial timestamp t0 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
    YuanQuaternionStepIntegrator()
    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 Yuan'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 Yuan'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 quat, com.irurueta.algebra.Matrix omega, com.irurueta.algebra.Matrix a, com.irurueta.algebra.Matrix identity, com.irurueta.algebra.Matrix tmp, 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

    • EPSILON

      private static final double EPSILON
      See Also:

    • quat

      private com.irurueta.algebra.Matrix quat
      Angular speed at initial timestamp t0 to be reused.

    • omega

      private com.irurueta.algebra.Matrix omega
      Angular speed at initial timestamp t0 to be reused.

    • a

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

    • identity

      private com.irurueta.algebra.Matrix identity
      Identity 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

    • YuanQuaternionStepIntegrator

      public YuanQuaternionStepIntegrator()
      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 Yuan's integration step. More information available here: Yuan, S. "Quaternion-based Unscented Kalman Filter for Real-time". 2015,
      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 Yuan's integration step. More information available here: Yuan, S. "Quaternion-based Unscented Kalman Filter for Real-time". 2015,
      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 quat, com.irurueta.algebra.Matrix omega, com.irurueta.algebra.Matrix a, com.irurueta.algebra.Matrix identity, com.irurueta.algebra.Matrix tmp, 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.
      omega - angular speed at initial timestamp t0 to be reused. Must be 3x1.
      a - temporary matrix to be reused. Must be 4x4.
      identity - identity 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.