Package com.irurueta.ar.slam

Class ConstantVelocityModelStatePredictor

java.lang.Object
com.irurueta.ar.slam.ConstantVelocityModelStatePredictor
public class ConstantVelocityModelStatePredictor extends Object
Utility class to predict device state (position, orientation, linear velocity and angular velocity) assuming a constant velocity model (acceleration is assumed zero under no external force).

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    static final int
    ANGULAR_SPEED_COMPONENTS
    Number of components on angular speed.
    static final int
    CONTROL_COMPONENTS
    Number of components of constant velocity model control signal.
    static final int
    CONTROL_WITH_POSITION_ADJUSTMENT_COMPONENTS
    Number of components of constant velocity model with position adjustment control signal.
    static final int
    CONTROL_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS
    Number of components of constant velocity model with position and rotation adjustment control signal.
    static final int
    CONTROL_WITH_ROTATION_ADJUSTMENT_COMPONENTS
    Number of components of constant velocity 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 velocity model state.
    static final int
    STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS
    Number of components of constant velocity model state with position adjustment.
    static final int
    STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS
    Number of components of constant velocity model state with position and rotation adjustment.
    static final int
    STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS
    Number of components of constant velocity model state with rotation adjustment.

  • Constructor Summary

    Constructors
    Modifier
    Constructor
    Description
    private
    ConstantVelocityModelStatePredictor()
    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 and angular velocity) assuming a constant velocity model (without acceleration).
    static void
    predict(double[] x, double[] u, double dt, double[] result)
    Updates the system model (position, orientation, linear velocity and angular velocity) assuming a constant velocity model (without acceleration).
    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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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 and angular velocity) assuming a constant velocity model (without acceleration).
    static double[]
    predictWithPositionAdjustment(double[] x, double[] u, double dt)
    Updates the system model (position, orientation, linear velocity and angular velocity) assuming a constant velocity model (without acceleration) when no velocity control signal is present.
    static void
    predictWithPositionAdjustment(double[] x, double[] u, double dt, double[] result)
    Updates the system model (position, orientation, linear velocity and angular velocity) assuming a constant velocity model (without acceleration) when no 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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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 and angular velocity) assuming a constant velocity model (without acceleration) when no velocity control signal is present.
    static double[]
    predictWithPositionAndRotationAdjustment(double[] x, double[] u, double dt)
    Updates the system model (position, orientation, linear velocity and angular velocity) assuming a constant velocity model (without acceleration) when no velocity control signal is present.
    static void
    predictWithPositionAndRotationAdjustment(double[] x, double[] u, double dt, double[] result)
    Updates the system model (position, orientation, linear velocity and angular velocity) assuming a constant velocity model (without acceleration) when no 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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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 and angular velocity) assuming a constant velocity model (without acceleration) when no velocity control signal is present.
    static double[]
    predictWithRotationAdjustment(double[] x, double[] u, double dt)
    Updates the system model (position, orientation, linear velocity and angular velocity) assuming a constant velocity model (without acceleration) when no velocity control signal is present.
    static void
    predictWithRotationAdjustment(double[] x, double[] u, double dt, double[] result)
    Updates the system model (position, orientation, linear velocity and angular velocity) assuming a constant velocity model (without acceleration) when no 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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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:

    • STATE_COMPONENTS

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

    • CONTROL_COMPONENTS

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

    • STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS

      public static final int STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS
      Number of components of constant velocity 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 velocity 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 velocity 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 velocity 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 velocity model state 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 velocity model with position and rotation adjustment control signal.
      See Also:

  • Constructor Details

    • ConstantVelocityModelStatePredictor

      private ConstantVelocityModelStatePredictor()
      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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear and angular velocity perturbations or controls: linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 6.
      dt - time interval to compute prediction expressed in seconds.
      result - instance where updated system model will be stored. Must have length 13.
      jacobianX - jacobian wrt system state. Must be 13x13.
      jacobianU - jacobian wrt control. Must be 13x6.
      Throws:
      IllegalArgumentException - if system state array, control array, result or jacobians do not have proper size.
      See Also:

    • predict

      public static void predict(double[] x, double[] u, double dt, double[] result)
      Updates the system model (position, orientation, linear velocity and angular velocity) assuming a constant velocity model (without acceleration).
      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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear and angular velocity perturbations or controls: linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 6.
      dt - time interval to compute prediction expressed in seconds.
      result - instance where updated system model will be stored. Must have length 13.
      Throws:
      IllegalArgumentException - if system state array or control array or result do not have proper size.
      See Also:

    • 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 and angular velocity) assuming a constant velocity model (without acceleration).
      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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear and angular velocity perturbations or controls: linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 6.
      dt - time interval to compute prediction expressed in seconds.
      jacobianX - jacobian wrt system state. Must be 13x13.
      jacobianU - jacobian wrt control. Must be 13x6.
      Returns:
      instance where updated system model will be stored.
      Throws:
      IllegalArgumentException - if system state array, control array or jacobians do not have proper size.
      See Also:

    • predict

      public static double[] predict(double[] x, double[] u, double dt)
      Updates the system model (position, orientation, linear velocity and angular velocity) assuming a constant velocity model (without acceleration).
      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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear and angular velocity perturbations or controls: linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 6.
      dt - time interval to compute prediction expressed in seconds.
      Returns:
      a new instance containing the updated system state.
      Throws:
      IllegalArgumentException - if system state array, control array or jacobians do not have proper size.
      See Also:

    • 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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear ang angular velocity perturbations or controls and position perturbations or controls: position-change-x, position-change-y, position-change-z, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-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 13.
      jacobianX - jacobian wrt system state. Must be 13x13.
      jacobianU - jacobian wrt control. Must be 13x9.
      Throws:
      IllegalArgumentException - if system state array, control array, result 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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear ang angular velocity perturbations or controls and position perturbations or controls: position-change-x, position-change-y, position-change-z, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-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 13.
      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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear ang angular velocity perturbations or controls and position perturbations or controls: position-change-x, position-change-y, position-change-z, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-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 13x13.
      jacobianU - jacobian wrt control. Must be 13x9.
      Returns:
      a new instance 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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear ang angular velocity perturbations or controls and position perturbations or controls: position-change-x, position-change-y, position-change-z, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-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 updated system model.
      Throws:
      IllegalArgumentException - if system state 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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear and angular velocity perturbations or controls, and rotation perturbations or controls: quaternion-change-a, quaternion-change-b, quaternion-change-c, quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 10.
      dt - time interval to compute prediction expressed in seconds.
      result - instance where updated system model will be stored. Must have length 13.
      jacobianX - jacobian wrt system state. Must be 13x13.
      jacobianU - jacobian wrt control. Must be 13x10.
      Throws:
      IllegalArgumentException - if system state array, control array, result 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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear and angular velocity perturbations or controls, and rotation perturbations or controls: quaternion-change-a, quaternion-change-b, quaternion-change-c, quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 10.
      dt - time interval to compute prediction expressed in seconds.
      result - instance where updated system model will be stored. Must have length 13.
      Throws:
      IllegalArgumentException - if system state array, control array or result do not have proper length.

    • 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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear and angular velocity perturbations or controls, and rotation perturbations or controls: quaternion-change-a, quaternion-change-b, quaternion-change-c, quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 10.
      dt - time interval to compute prediction expressed in seconds.
      jacobianX - jacobian wrt system state. Must be 13x13.
      jacobianU - jacobian wrt control. Must be 13x10.
      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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear and angular velocity perturbations or controls, and rotation perturbations or controls: quaternion-change-a, quaternion-change-b, quaternion-change-c, quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y, linear-velocity-change-z, angular-velocity-change-x, angular-velocity-change-y, angular-velocity-change-z. Must have length 10.
      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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear and angular velocity perturbations or controls, position perturbations or controls and rotation perturbation or control: 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, 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 13.
      jacobianX - jacobian wrt system state. Must be 13x13.
      jacobianU - jacobian wrt control. Must be 13x13.
      Throws:
      IllegalArgumentException - if system state array, control array, result 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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear and angular velocity perturbations or controls, position perturbations or controls and rotation perturbation or control: 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, 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 13.
      Throws:
      IllegalArgumentException - if system state array, control array, result or jacobians 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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear and angular velocity perturbations or controls, position perturbations or controls and rotation perturbation or control: 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, 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 13x13.
      jacobianU - jacobian wrt control. Must be 13x13.
      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 and angular velocity) assuming a constant velocity model (without acceleration) when no 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, angular-velocity-x, angular-velocity-y, angular-velocity-z. Must have length 13.
      u - linear and angular velocity perturbations or controls, position perturbations or controls and rotation perturbation or control: 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, 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. Must have length 13.
      Throws:
      IllegalArgumentException - if system state array, control array or result do not have proper size.