Package com.irurueta.ar.slam

Class StatePredictor

java.lang.Object
com.irurueta.ar.slam.StatePredictor
public class StatePredictor extends Object
Utility class to predict device state (position, orientation, linear velocity, linear acceleration and angular velocity).

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    static final int
    ACCELERATION_COMPONENTS
    Number of components of acceleration.
    static final int
    ANGULAR_SPEED_COMPONENTS
    Number of components on angular speed.
    static final int
    CONTROL_COMPONENTS
    Number of components of constant acceleration model control signal.
    static final int
    CONTROL_WITH_POSITION_ADJUSTMENT_COMPONENTS
    Number of components of constant acceleration model with position adjustment control signal.
    static final int
    CONTROL_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS
    Number of components of constant acceleration model with position and rotation adjustment control signal.
    static final int
    CONTROL_WITH_ROTATION_ADJUSTMENT_COMPONENTS
    Number of components of constant acceleration model with rotation adjustment control signal.
    static final int
    SPEED_COMPONENTS
    Number of components of speed.
    static final int
    STATE_COMPONENTS
    Number of components of constant acceleration model state.
    static final int
    STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS
    Number of components of constant acceleration model state with position adjustment.
    static final int
    STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS
    Number of components of constant acceleration model with position and rotation adjustment.
    static final int
    STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS
    Number of components of constant acceleration model state with rotation adjustment.

  • Constructor Summary

    Constructors
    Modifier
    Constructor
    Description
    private
    StatePredictor()
    Constructor.

  • Method Summary

    Modifier and Type
    Method
    Description
    static double[]
    predict(double[] x, double[] u, double dt)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static void
    predict(double[] x, double[] u, double dt, double[] result)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static void
    predict(double[] x, double[] u, double dt, double[] result, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static double[]
    predict(double[] x, double[] u, double dt, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static double[]
    predictWithPositionAdjustment(double[] x, double[] u, double dt)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static void
    predictWithPositionAdjustment(double[] x, double[] u, double dt, double[] result)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static void
    predictWithPositionAdjustment(double[] x, double[] u, double dt, double[] result, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static double[]
    predictWithPositionAdjustment(double[] x, double[] u, double dt, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static double[]
    predictWithPositionAndRotationAdjustment(double[] x, double[] u, double dt)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static void
    predictWithPositionAndRotationAdjustment(double[] x, double[] u, double dt, double[] result)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static void
    predictWithPositionAndRotationAdjustment(double[] x, double[] u, double dt, double[] result, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static double[]
    predictWithPositionAndRotationAdjustment(double[] x, double[] u, double dt, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static double[]
    predictWithRotationAdjustment(double[] x, double[] u, double dt)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static void
    predictWithRotationAdjustment(double[] x, double[] u, double dt, double[] result)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static void
    predictWithRotationAdjustment(double[] x, double[] u, double dt, double[] result, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
    static double[]
    predictWithRotationAdjustment(double[] x, double[] u, double dt, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
    Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.

    Methods inherited from class java.lang.Object

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

  • Field Details

    • ANGULAR_SPEED_COMPONENTS

      public static final int ANGULAR_SPEED_COMPONENTS
      Number of components on angular speed.
      See Also:

    • SPEED_COMPONENTS

      public static final int SPEED_COMPONENTS
      Number of components of speed.
      See Also:

    • ACCELERATION_COMPONENTS

      public static final int ACCELERATION_COMPONENTS
      Number of components of acceleration.
      See Also:

    • STATE_COMPONENTS

      public static final int STATE_COMPONENTS
      Number of components of constant acceleration model state.
      See Also:

    • CONTROL_COMPONENTS

      public static final int CONTROL_COMPONENTS
      Number of components of constant acceleration model control signal.
      See Also:

    • STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS

      public static final int STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS
      Number of components of constant acceleration model state with position adjustment.
      See Also:

    • CONTROL_WITH_POSITION_ADJUSTMENT_COMPONENTS

      public static final int CONTROL_WITH_POSITION_ADJUSTMENT_COMPONENTS
      Number of components of constant acceleration model with position adjustment control signal.
      See Also:

    • STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS

      public static final int STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS
      Number of components of constant acceleration model state with rotation adjustment.
      See Also:

    • CONTROL_WITH_ROTATION_ADJUSTMENT_COMPONENTS

      public static final int CONTROL_WITH_ROTATION_ADJUSTMENT_COMPONENTS
      Number of components of constant acceleration model with rotation adjustment control signal.
      See Also:

    • STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS

      public static final int STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS
      Number of components of constant acceleration model with position and rotation adjustment.
      See Also:

    • CONTROL_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS

      public static final int CONTROL_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS
      Number of components of constant acceleration model with position and rotation adjustment control signal.
      See Also:

  • Constructor Details

    • StatePredictor

      private StatePredictor()
      Constructor.

  • Method Details

    • predict

      public static void predict(double[] x, double[] u, double dt, double[] result, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 9.
      dt - time interval to compute prediction expressed in seconds.
      result - instance where updated system model will be stored. Must have length 16.
      jacobianX - jacobian wrt system state. Must be 16x16.
      jacobianU - jacobian wrt control. must be 16x9.
      Throws:
      IllegalArgumentException - if system state array, control array, result or jacobians do not have proper size.

    • predict

      public static void predict(double[] x, double[] u, double dt, double[] result)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 9.
      dt - time interval to compute prediction expressed in seconds.
      result - instance where updated system model will be stored. Must have length 16.
      Throws:
      IllegalArgumentException - if system state array, control array or result do not have proper size.

    • predict

      public static double[] predict(double[] x, double[] u, double dt, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 9.
      dt - time interval to compute prediction expressed in seconds.
      jacobianX - jacobian wrt system state. Must be 16x16.
      jacobianU - jacobian wrt control. must be 16x9.
      Returns:
      a new instance containing the updated system state.
      Throws:
      IllegalArgumentException - if system state array, control array or jacobians do not have proper size.

    • predict

      public static double[] predict(double[] x, double[] u, double dt)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 9.
      dt - time interval to compute prediction expressed in seconds.
      Returns:
      a new instance containing the updated system state.
      Throws:
      IllegalArgumentException - if system state array or control array do not have proper size.

    • predictWithPositionAdjustment

      public static void predictWithPositionAdjustment(double[] x, double[] u, double dt, double[] result, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: position-change-x, position-change-y, position-change-z, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 12.
      dt - time interval to compute prediction expressed in seconds.
      result - instance where updated system model will be stored. Must have length 16.
      jacobianX - jacobian wrt system state. Must be 16x16.
      jacobianU - jacobian wrt control. must be 16x12.
      Throws:
      IllegalArgumentException - if system state array, control array, result array or jacobians do not have proper size.

    • predictWithPositionAdjustment

      public static void predictWithPositionAdjustment(double[] x, double[] u, double dt, double[] result)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: position-change-x, position-change-y, position-change-z, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 12.
      dt - time interval to compute prediction expressed in seconds.
      result - instance where updated system model will be stored. Must have length 16.
      Throws:
      IllegalArgumentException - if system state array, control array or result array do not have proper size.

    • predictWithPositionAdjustment

      public static double[] predictWithPositionAdjustment(double[] x, double[] u, double dt, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: position-change-x, position-change-y, position-change-z, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 12.
      dt - time interval to compute prediction expressed in seconds.
      jacobianX - jacobian wrt system state. Must be 16x16.
      jacobianU - jacobian wrt control. must be 16x12.
      Returns:
      a new array containing updated system model.
      Throws:
      IllegalArgumentException - if system state array, control array or jacobians do not have proper size.

    • predictWithPositionAdjustment

      public static double[] predictWithPositionAdjustment(double[] x, double[] u, double dt)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: position-change-x, position-change-y, position-change-z, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 12.
      dt - time interval to compute prediction expressed in seconds.
      Returns:
      a new array containing updated system model.
      Throws:
      IllegalArgumentException - if system state array or control array do not have proper size.

    • predictWithRotationAdjustment

      public static void predictWithRotationAdjustment(double[] x, double[] u, double dt, double[] result, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: quaternion-change-a, quaternion-change-b, quaternion-change-c, quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 13.
      dt - time interval to compute prediction expressed in seconds.
      result - instance where updated system model will be stored. Must have length 16.
      jacobianX - jacobian wrt system state. Must be 16x16.
      jacobianU - jacobian wrt control. must be 16x13.
      Throws:
      IllegalArgumentException - if system state array, control array, result array or jacobians do not have proper size.

    • predictWithRotationAdjustment

      public static void predictWithRotationAdjustment(double[] x, double[] u, double dt, double[] result)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: quaternion-change-a, quaternion-change-b, quaternion-change-c, quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 13.
      dt - time interval to compute prediction expressed in seconds.
      result - instance where updated system model will be stored. Must have length 16.
      Throws:
      IllegalArgumentException - if system state array, control array or result array do not have proper size.

    • predictWithRotationAdjustment

      public static double[] predictWithRotationAdjustment(double[] x, double[] u, double dt, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: quaternion-change-a, quaternion-change-b, quaternion-change-c, quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 13.
      dt - time interval to compute prediction expressed in seconds.
      jacobianX - jacobian wrt system state. Must be 16x16.
      jacobianU - jacobian wrt control. must be 16x13.
      Returns:
      a new array containing updated system model.
      Throws:
      IllegalArgumentException - if system state array, control array or jacobians do not have proper size.

    • predictWithRotationAdjustment

      public static double[] predictWithRotationAdjustment(double[] x, double[] u, double dt)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: quaternion-change-a, quaternion-change-b, quaternion-change-c, quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 13.
      dt - time interval to compute prediction expressed in seconds.
      Returns:
      a new array containing updated system model.
      Throws:
      IllegalArgumentException - if system state array or control array do not have proper size.

    • predictWithPositionAndRotationAdjustment

      public static void predictWithPositionAndRotationAdjustment(double[] x, double[] u, double dt, double[] result, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: position-change-x, position-change-y, position-change-z, quaternion-change-a, quaternion-change-b, quaternion-change-c, quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 16.
      dt - time interval to compute prediction expressed in seconds.
      result - instance where updated system model will be stored. Must have length 16.
      jacobianX - jacobian wrt system state. Must be 16x16.
      jacobianU - jacobian wrt control. must be 16x16.
      Throws:
      IllegalArgumentException - if system state array, control array, result array or jacobians do not have proper size.

    • predictWithPositionAndRotationAdjustment

      public static void predictWithPositionAndRotationAdjustment(double[] x, double[] u, double dt, double[] result)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: position-change-x, position-change-y, position-change-z, quaternion-change-a, quaternion-change-b, quaternion-change-c, quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 16.
      dt - time interval to compute prediction expressed in seconds.
      result - instance where updated system model will be stored. Must have length 16.
      Throws:
      IllegalArgumentException - if system state array, control array or result array do not have proper size.

    • predictWithPositionAndRotationAdjustment

      public static double[] predictWithPositionAndRotationAdjustment(double[] x, double[] u, double dt, com.irurueta.algebra.Matrix jacobianX, com.irurueta.algebra.Matrix jacobianU)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: position-change-x, position-change-y, position-change-z, quaternion-change-a, quaternion-change-b, quaternion-change-c, quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 16.
      dt - time interval to compute prediction expressed in seconds.
      jacobianX - jacobian wrt system state. Must be 16x16.
      jacobianU - jacobian wrt control. must be 16x16.
      Returns:
      a new array containing updated system model.
      Throws:
      IllegalArgumentException - if system state array, control array or jacobians do not have proper size.

    • predictWithPositionAndRotationAdjustment

      public static double[] predictWithPositionAndRotationAdjustment(double[] x, double[] u, double dt)
      Updates the system model (position, orientation, linear velocity, linear acceleration and angular velocity) assuming a constant acceleration model when no acceleration or velocity control signal is present.
      Parameters:
      x - initial system state containing: position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c, quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z, linear-acceleration-x, linear-acceleration-y, linear-acceleration-z, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 16.
      u - perturbations or control signals: position-change-x, position-change-y, position-change-z, quaternion-change-a, quaternion-change-b, quaternion-change-c, quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, linear-acceleration-change-x, linear-acceleration-change-y, linear-acceleration-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 16.
      dt - time interval to compute prediction expressed in seconds.
      Returns:
      a new array containing updated system model.
      Throws:
      IllegalArgumentException - if system state array, or control array do not have proper size.