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

Class RungeKuttaQuaternionStepIntegrator

java.lang.Object
com.irurueta.navigation.inertial.calibration.gyroscope.QuaternionStepIntegrator
com.irurueta.navigation.inertial.calibration.gyroscope.RungeKuttaQuaternionStepIntegrator
public class RungeKuttaQuaternionStepIntegrator extends QuaternionStepIntegrator
Computes one step of a Runge-Kutta (RK4) integration algorithm. More information available here: https://en.wikipedia.org/wiki/Runge–Kutta_methods

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    private com.irurueta.algebra.Matrix
    k1
    First Runge-Kutta coefficient.
    private com.irurueta.algebra.Matrix
    k2
    Second Runge-Kutta coefficient.
    private com.irurueta.algebra.Matrix
    k3
    Third Runge-Kutta coefficient to be reused.
    private com.irurueta.algebra.Matrix
    k4
    Fourth Runge-Kutta coefficient.
    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 static final double
    ONE_SIXTH
    Factor to multiply slopes k1 and k4.
    private static final double
    ONE_THIRD
    Factor to multiply slopes k2 and k3.
    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
    RungeKuttaQuaternionStepIntegrator()
    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 an RK4 Runge-Kutta 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 an RK4 Runge-Kutta 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 k3, com.irurueta.algebra.Matrix k4, com.irurueta.algebra.Matrix omegaSkew)
    Performs an RK4 Runge-Kutta integration step.

    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

    • ONE_SIXTH

      private static final double ONE_SIXTH
      Factor to multiply slopes k1 and k4.
      See Also:

    • ONE_THIRD

      private static final double ONE_THIRD
      Factor to multiply slopes k2 and k3.
      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.

    • 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
      First Runge-Kutta coefficient. Quaternion derivative at initial timestamp t0 to be reused.

    • k2

      private com.irurueta.algebra.Matrix k2
      Second Runge-Kutta coefficient. Quaternion derivative at mid-point timestamp between t0 and t1 to be reused.

    • k3

      private com.irurueta.algebra.Matrix k3
      Third Runge-Kutta coefficient to be reused.

    • k4

      private com.irurueta.algebra.Matrix k4
      Fourth Runge-Kutta coefficient. Quaternion derivative at end timestamp 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

    • RungeKuttaQuaternionStepIntegrator

      public RungeKuttaQuaternionStepIntegrator()
      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 an RK4 Runge-Kutta integration step. More information available here: https://en.wikipedia.org/wiki/Runge–Kutta_methods –
      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 an RK4 Runge-Kutta integration step. More information available here: https://en.wikipedia.org/wiki/Runge–Kutta_methods
      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 k3, com.irurueta.algebra.Matrix k4, com.irurueta.algebra.Matrix omegaSkew) throws com.irurueta.geometry.RotationException
      Performs an RK4 Runge-Kutta integration step. More information available here: https://en.wikipedia.org/wiki/Runge–Kutta_methods
      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.
      k3 - slope of quaternion derivative at mid-point timestamp between t0 and t1 to be reused. Must be 4x1.
      k4 - slope of quaternion derivative at end timestamp 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.