/*
    * Copyright (C) 2016 Alberto Irurueta Carro (alberto@irurueta.com)
    *
    * Licensed under the Apache License, Version 2.0 (the "License");
    * you may not use this file except in compliance with the License.
    * You may obtain a copy of the License at
    *
    *         http://www.apache.org/licenses/LICENSE-2.0
    *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  package com.irurueta.ar.slam;
  
  import com.irurueta.algebra.Matrix;
  import com.irurueta.algebra.WrongSizeException;
  import com.irurueta.geometry.InhomogeneousPoint3D;
  import com.irurueta.geometry.Point3D;
  import com.irurueta.geometry.Quaternion;
  
  /**
   * 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).
   */
  @SuppressWarnings("DuplicatedCode")
  public class ConstantVelocityModelStatePredictor {
  
      /**
       * Number of components on angular speed.
       */
      public static final int ANGULAR_SPEED_COMPONENTS = 3;
  
      /**
       * Number of components of speed.
       */
      public static final int SPEED_COMPONENTS = 3;
  
      /**
       * Number of components of constant velocity model state.
       */
      public static final int STATE_COMPONENTS = 13;
  
      /**
       * Number of components of constant velocity model control signal.
       */
      public static final int CONTROL_COMPONENTS = 6;
  
      /**
       * Number of components of constant velocity model state with position
       * adjustment.
       */
      public static final int STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS = 13;
  
      /**
       * Number of components of constant velocity model with position adjustment
       * control signal.
       */
      public static final int CONTROL_WITH_POSITION_ADJUSTMENT_COMPONENTS = 9;
  
      /**
       * Number of components of constant velocity model state with rotation
       * adjustment.
       */
      public static final int STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS = 13;
  
      /**
       * Number of components of constant velocity model with rotation adjustment
       * control signal.
       */
      public static final int CONTROL_WITH_ROTATION_ADJUSTMENT_COMPONENTS = 10;
  
      /**
       * Number of components of constant velocity model state with position and
       * rotation adjustment.
       */
      public static final int STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS = 13;
  
      /**
       * Number of components of constant velocity model with position and
       * rotation adjustment control signal.
       */
      public static final int CONTROL_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS = 13;
  
      /**
       * Constructor.
       */
      private ConstantVelocityModelStatePredictor() {
      }
  
      /**
       * 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.
       *
       * @param 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.
      * @param 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.
      * @param dt        time interval to compute prediction expressed in seconds.
      * @param result    instance where updated system model will be stored. Must
      *                  have length 13.
      * @param jacobianX jacobian wrt system state. Must be 13x13.
      * @param jacobianU jacobian wrt control. Must be 13x6.
      * @throws IllegalArgumentException if system state array, control array,
      *                                  result or jacobians do not have proper size.
      * @see <a href="https://github.com/joansola/slamtb">constVel.m at https://github.com/joansola/slamtb</a>
      */
     public static void predict(
             final double[] x, final double[] u, final double dt, final double[] result, final Matrix jacobianX,
             final Matrix jacobianU) {
         if (x.length != STATE_COMPONENTS) {
             // x must have length 13
             throw new IllegalArgumentException();
         }
         if (u.length != CONTROL_COMPONENTS) {
             // u must have length 6
             throw new IllegalArgumentException();
         }
         if (result.length != STATE_COMPONENTS) {
             // result must have length 13
             throw new IllegalArgumentException();
         }
         if (jacobianX != null && (jacobianX.getRows() != STATE_COMPONENTS
                 || jacobianX.getColumns() != STATE_COMPONENTS)) {
             // jacobian wrt x must be 13x13
             throw new IllegalArgumentException();
         }
         if (jacobianU != null && (jacobianU.getRows() != STATE_COMPONENTS
                 || jacobianU.getColumns() != CONTROL_COMPONENTS)) {
             // jacobian wrt u must be 13x6
             throw new IllegalArgumentException();
         }
 
         // position
         final var r = new InhomogeneousPoint3D(x[0], x[1], x[2]);
 
         // orientation
         var q = new Quaternion(x[3], x[4], x[5], x[6]);
 
         // linear velocity
         var vx = x[7];
         var vy = x[8];
         var vz = x[9];
 
         // angular velocity
         var wx = x[10];
         var wy = x[11];
         var wz = x[12];
 
         // linear velocity change (control)
         final var uvx = u[0];
         final var uvy = u[1];
         final var uvz = u[2];
 
         // angular velocity change (control)
         final var uwx = u[3];
         final var uwy = u[4];
         final var uwz = u[5];
 
         try {
             // update position
             Matrix rr = null;
             Matrix rv = null;
             if (jacobianX != null) {
                 rr = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH,
                         Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH);
                 rv = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, SPEED_COMPONENTS);
             }
             PositionPredictor.predict(r, vx, vy, vz, dt, r, rr, rv, null);
 
             // update orientation
             Matrix qq = null;
             Matrix qw = null;
             if (jacobianX != null) {
                 qq = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
                 qw = new Matrix(Quaternion.N_PARAMS, ANGULAR_SPEED_COMPONENTS);
             }
             q = QuaternionPredictor.predict(q, wx, wy, wz, dt, true, qq, qw);
 
             // apply control signals
             vx += uvx;
             vy += uvy;
             vz += uvz;
 
             wx += uwx;
             wy += uwy;
             wz += uwz;
 
             // set new state
             result[0] = r.getInhomX();
             result[1] = r.getInhomY();
             result[2] = r.getInhomZ();
 
             result[3] = q.getA();
             result[4] = q.getB();
             result[5] = q.getC();
             result[6] = q.getD();
 
             result[7] = vx;
             result[8] = vy;
             result[9] = vz;
 
             result[10] = wx;
             result[11] = wy;
             result[12] = wz;
 
             // jacobians
             if (jacobianX != null) {
                 // [Rr   0   Rv  0  ]
                 // [0    Qq  0   Qw ]
                 // [0    0   eye 0  ]
                 // [0    0   0   eye]
                 jacobianX.initialize(0.0);
                 jacobianX.setSubmatrix(0, 0, 2, 2, rr);
 
                 jacobianX.setSubmatrix(3, 3, 6, 6, qq);
 
                 jacobianX.setSubmatrix(0, 7, 2, 9, rv);
 
                 for (int i = 7; i < STATE_COMPONENTS; i++) {
                     jacobianX.setElementAt(i, i, 1.0);
                 }
 
                 jacobianX.setSubmatrix(3, 10, 6, 12, qw);
             }
 
             if (jacobianU != null) {
                 jacobianU.initialize(0.0);
 
                 for (int i = 7, j = 0; i < STATE_COMPONENTS; i++, j++) {
                     jacobianU.setElementAt(i, j, 1.0);
                 }
             }
         } catch (final WrongSizeException ignore) {
             // never thrown
         }
     }
 
     /**
      * Updates the system model (position, orientation, linear velocity and
      * angular velocity) assuming a constant velocity model (without
      * acceleration).
      *
      * @param 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.
      * @param 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.
      * @param dt     time interval to compute prediction expressed in seconds.
      * @param 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 <a href="https://github.com/joansola/slamtb">constVel.m at https://github.com/joansola/slamtb</a>
      */
     public static void predict(final double[] x, final double[] u, final double dt, final double[] result) {
         predict(x, u, dt, result, null, null);
     }
 
     /**
      * Updates the system model (position, orientation, linear velocity and
      * angular velocity) assuming a constant velocity model (without
      * acceleration).
      *
      * @param 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.
      * @param 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.
      * @param dt        time interval to compute prediction expressed in seconds.
      * @param jacobianX jacobian wrt system state. Must be 13x13.
      * @param jacobianU jacobian wrt control. Must be 13x6.
      * @return instance where updated system model will be stored.
      * @throws IllegalArgumentException if system state array, control array or
      *                                  jacobians do not have proper size.
      * @see <a href="https://github.com/joansola/slamtb">constVel.m at https://github.com/joansola/slamtb</a>
      */
     public static double[] predict(final double[] x, final double[] u, final double dt, final Matrix jacobianX,
                                    final Matrix jacobianU) {
         final var result = new double[STATE_COMPONENTS];
         predict(x, u, dt, result, jacobianX, jacobianU);
         return result;
     }
 
     /**
      * Updates the system model (position, orientation, linear velocity and
      * angular velocity) assuming a constant velocity model (without
      * acceleration).
      *
      * @param 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.
      * @param 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.
      * @param dt time interval to compute prediction expressed in seconds.
      * @return a new instance containing the updated system state.
      * @throws IllegalArgumentException if system state array, control array or
      *                                  jacobians do not have proper size.
      * @see <a href="https://github.com/joansola/slamtb">constVel.m at https://github.com/joansola/slamtb</a>
      */
     public static double[] predict(final double[] x, final double[] u, final double dt) {
         final var result = new double[STATE_COMPONENTS];
         predict(x, u, dt, result);
         return 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.
      *
      * @param 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.
      * @param 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.
      * @param dt        time interval to compute prediction expressed in seconds.
      * @param result    instance where updated system model will be stored. Must
      *                  have length 13.
      * @param jacobianX jacobian wrt system state. Must be 13x13.
      * @param jacobianU jacobian wrt control. Must be 13x9.
      * @throws IllegalArgumentException if system state array, control array,
      *                                  result or jacobians do not have proper size.
      */
     public static void predictWithPositionAdjustment(
             final double[] x, final double[] u, final double dt, final double[] result, final Matrix jacobianX,
             final Matrix jacobianU) {
         if (x.length != STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS) {
             // x must have length 13
             throw new IllegalArgumentException();
         }
         if (u.length != CONTROL_WITH_POSITION_ADJUSTMENT_COMPONENTS) {
             // u must have length 9
             throw new IllegalArgumentException();
         }
         if (result.length != STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS) {
             // result must have length 13
             throw new IllegalArgumentException();
         }
         if (jacobianX != null && (jacobianX.getRows() != STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS ||
                 jacobianX.getColumns() != STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS)) {
             // jacobian wrt x must be 13x13
             throw new IllegalArgumentException();
         }
         if (jacobianU != null && (jacobianU.getRows() != STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS ||
                 jacobianU.getColumns() != CONTROL_WITH_POSITION_ADJUSTMENT_COMPONENTS)) {
             // jacobian wrt u must be 13x9
             throw new IllegalArgumentException();
         }
 
         // position
         final var r = new InhomogeneousPoint3D(x[0], x[1], x[2]);
 
         // orientation
         var q = new Quaternion(x[3], x[4], x[5], x[6]);
 
         // linear velocity
         var vx = x[7];
         var vy = x[8];
         var vz = x[9];
 
         // angular velocity
         var wx = x[10];
         var wy = x[11];
         var wz = x[12];
 
         // position change (control)
         final var drx = u[0];
         final var dry = u[1];
         final var drz = u[2];
 
         // linear velocity change (control)
         final var uvx = u[3];
         final var uvy = u[4];
         final var uvz = u[5];
 
         // angular velocity change (control)
         final var uwx = u[6];
         final var uwy = u[7];
         final var uwz = u[8];
 
         try {
             // update position
             Matrix rr = null;
             Matrix rv = null;
             if (jacobianX != null) {
                 rr = new Matrix(
                         Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH,
                         Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH);
                 rv = new Matrix(
                         Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH,
                         SPEED_COMPONENTS);
             }
             PositionPredictor.predictWithPositionAdjustment(r, drx, dry, drz, vx, vy, vz, 0.0, 0.0, 0.0,
                     dt, r, rr, null, rv, null);
 
             // update orientation
             Matrix qq = null;
             Matrix qw = null;
             if (jacobianX != null) {
                 qq = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
                 qw = new Matrix(Quaternion.N_PARAMS, ANGULAR_SPEED_COMPONENTS);
             }
             q = QuaternionPredictor.predict(q, wx, wy, wz, dt, true, qq, qw);
 
             // apply control signals
             vx += uvx;
             vy += uvy;
             vz += uvz;
 
             wx += uwx;
             wy += uwy;
             wz += uwz;
 
             // set new state
             result[0] = r.getInhomX();
             result[1] = r.getInhomY();
             result[2] = r.getInhomZ();
 
             result[3] = q.getA();
             result[4] = q.getB();
             result[5] = q.getC();
             result[6] = q.getD();
 
             result[7] = vx;
             result[8] = vy;
             result[9] = vz;
 
             result[10] = wx;
             result[11] = wy;
             result[12] = wz;
 
             // jacobians
             if (jacobianX != null) {
                 // [Rr   0   Rv  0  ]
                 // [0    Qq  0   Qw ]
                 // [0    0   eye 0  ]
                 // [0    0   0   eye]
                 jacobianX.initialize(0.0);
                 jacobianX.setSubmatrix(0, 0, 2, 2, rr);
 
                 jacobianX.setSubmatrix(3, 3, 6, 6, qq);
 
                 jacobianX.setSubmatrix(0, 7, 2, 9, rv);
 
                 for (int i = 7; i < STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS; i++) {
                     jacobianX.setElementAt(i, i, 1.0);
                 }
 
                 jacobianX.setSubmatrix(3, 10, 6, 12, qw);
             }
 
             if (jacobianU != null) {
                 jacobianU.initialize(0.0);
                 // variation of position
                 for (var i = 0; i < Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH; i++) {
                     jacobianU.setElementAt(i, i, 1.0);
                 }
                 // variation of linear and angular speed
                 for (int i = 7, j = Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH;
                      i < STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS; i++, j++) {
                     jacobianU.setElementAt(i, j, 1.0);
                 }
             }
         } catch (final WrongSizeException ignore) {
             // never thrown
         }
     }
 
     /**
      * 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.
      *
      * @param 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.
      * @param 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.
      * @param dt     time interval to compute prediction expressed in seconds.
      * @param 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.
      */
     public static void predictWithPositionAdjustment(
             final double[] x, final double[] u, final double dt, final double[] result) {
         predictWithPositionAdjustment(x, u, dt, result, null, null);
     }
 
     /**
      * 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.
      *
      * @param 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.
      * @param 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.
      * @param dt        time interval to compute prediction expressed in seconds.
      * @param jacobianX jacobian wrt system state. Must be 13x13.
      * @param jacobianU jacobian wrt control. Must be 13x9.
      * @return a new instance containing updated system model.
      * @throws IllegalArgumentException if system state array, control array
      *                                  or jacobians do not have proper size.
      */
     public static double[] predictWithPositionAdjustment(
             final double[] x, final double[] u, final double dt, final Matrix jacobianX, final Matrix jacobianU) {
         final var result = new double[STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS];
         predictWithPositionAdjustment(x, u, dt, result, jacobianX, jacobianU);
         return 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.
      *
      * @param 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.
      * @param 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.
      * @param dt time interval to compute prediction expressed in seconds.
      * @return a new instance containing updated system model.
      * @throws IllegalArgumentException if system state or control array do not
      *                                  have proper size.
      */
     public static double[] predictWithPositionAdjustment(final double[] x, final double[] u, final double dt) {
         final var result = new double[STATE_WITH_POSITION_ADJUSTMENT_COMPONENTS];
         predictWithPositionAdjustment(x, u, dt, result);
         return 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.
      *
      * @param 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.
      * @param 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.
      * @param dt        time interval to compute prediction expressed in seconds.
      * @param result    instance where updated system model will be stored. Must
      *                  have length 13.
      * @param jacobianX jacobian wrt system state. Must be 13x13.
      * @param jacobianU jacobian wrt control. Must be 13x10.
      * @throws IllegalArgumentException if system state array, control array,
      *                                  result or jacobians do not have proper size.
      */
     public static void predictWithRotationAdjustment(
             final double[] x, final double[] u, final double dt, final double[] result, final Matrix jacobianX,
             final Matrix jacobianU) {
         if (x.length != STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS) {
             throw new IllegalArgumentException("x must have length 13");
         }
         if (u.length != CONTROL_WITH_ROTATION_ADJUSTMENT_COMPONENTS) {
             throw new IllegalArgumentException("u must have length 10");
         }
         if (result.length != STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS) {
             throw new IllegalArgumentException("result must have length 13");
         }
         if (jacobianX != null && (jacobianX.getRows() != STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS ||
                 jacobianX.getColumns() != STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS)) {
             throw new IllegalArgumentException("jacobian wrt x must be 13x13");
         }
         if (jacobianU != null && (jacobianU.getRows() != STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS ||
                 jacobianU.getColumns() != CONTROL_WITH_ROTATION_ADJUSTMENT_COMPONENTS)) {
             throw new IllegalArgumentException("jacobian wrt u must be 13x10");
         }
 
         // position
         final var r = new InhomogeneousPoint3D(x[0], x[1], x[2]);
 
         // orientation
         var q = new Quaternion(x[3], x[4], x[5], x[6]);
 
         // linear velocity
         var vx = x[7];
         var vy = x[8];
         var vz = x[9];
 
         // linear acceleration
 
         // angular velocity
         var wx = x[10];
         var wy = x[11];
         var wz = x[12];
 
         // rotation change (control)
         final var dq = new Quaternion(u[0], u[1], u[2], u[3]);
 
         // linear velocity change (control)
         final var uvx = u[4];
         final var uvy = u[5];
         final var uvz = u[6];
 
         // angular velocity change (control)
         final var uwx = u[7];
         final var uwy = u[8];
         final var uwz = u[9];
 
         try {
             // update position
             Matrix rr = null;
             Matrix rv = null;
             if (jacobianX != null) {
                 rr = new Matrix(
                         Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH,
                         Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH);
                 rv = new Matrix(
                         Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH,
                         SPEED_COMPONENTS);
             }
             PositionPredictor.predict(r, vx, vy, vz, dt, r, rr, rv, null);
 
             // update orientation
             Matrix qq = null;
             Matrix qdq = null;
             Matrix qw = null;
             if (jacobianX != null) {
                 qq = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
                 qw = new Matrix(Quaternion.N_PARAMS, ANGULAR_SPEED_COMPONENTS);
             }
             if (jacobianU != null) {
                 qdq = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
             }
             q = QuaternionPredictor.predictWithRotationAdjustment(q, dq, wx, wy, wz, dt, qq, qdq, qw);
 
             // apply control signals
             vx += uvx;
             vy += uvy;
             vz += uvz;
 
             wx += uwx;
             wy += uwy;
             wz += uwz;
 
             // set new state
             result[0] = r.getInhomX();
             result[1] = r.getInhomY();
             result[2] = r.getInhomZ();
 
             result[3] = q.getA();
             result[4] = q.getB();
             result[5] = q.getC();
             result[6] = q.getD();
 
             result[7] = vx;
             result[8] = vy;
             result[9] = vz;
 
             result[10] = wx;
             result[11] = wy;
             result[12] = wz;
 
             // jacobians
             if (jacobianX != null) {
                 // [Rr   0   Rv  0  ]
                 // [0    Qq  0   Qw ]
                 // [0    0   eye 0  ]
                 // [0    0   0   eye]
                 jacobianX.initialize(0.0);
                 jacobianX.setSubmatrix(0, 0, 2, 2, rr);
 
                 jacobianX.setSubmatrix(3, 3, 6, 6, qq);
 
                 jacobianX.setSubmatrix(0, 7, 2, 9, rv);
 
                 for (var i = 7; i < STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS; i++) {
                     jacobianX.setElementAt(i, i, 1.0);
                 }
 
                 jacobianX.setSubmatrix(3, 10, 6, 12, qw);
             }
 
             if (jacobianU != null) {
                 jacobianU.initialize(0.0);
 
                 // variation of rotation
                 jacobianU.setSubmatrix(3, 0, 6, 3, qdq);
 
                 // variation of linear and angular speed
                 for (int i = 7, j = Quaternion.N_PARAMS;
                      i < STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS; i++, j++) {
                     jacobianU.setElementAt(i, j, 1.0);
                 }
             }
 
         } catch (final WrongSizeException ignore) {
             // never thrown
         }
     }
 
     /**
      * 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.
      *
      * @param 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.
      * @param 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.
      * @param dt     time interval to compute prediction expressed in seconds.
      * @param 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.
      */
     public static void predictWithRotationAdjustment(
             final double[] x, final double[] u, final double dt, final double[] result) {
         predictWithRotationAdjustment(x, u, dt, result, null, null);
     }
 
     /**
      * 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.
      *
      * @param 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.
      * @param 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.
      * @param dt        time interval to compute prediction expressed in seconds.
      * @param jacobianX jacobian wrt system state. Must be 13x13.
      * @param jacobianU jacobian wrt control. Must be 13x10.
      * @return a new array containing updated system model.
      * @throws IllegalArgumentException if system state array, control array
      *                                  or jacobians do not have proper size.
      */
     public static double[] predictWithRotationAdjustment(
             final double[] x, final double[] u, final double dt, final Matrix jacobianX, final Matrix jacobianU) {
         final var result = new double[STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS];
         predictWithRotationAdjustment(x, u, dt, result, jacobianX, jacobianU);
         return 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.
      *
      * @param 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.
      * @param 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.
      * @param dt time interval to compute prediction expressed in seconds.
      * @return a new array containing updated system model.
      * @throws IllegalArgumentException if system state array or control array
      *                                  do not have proper size.
      */
     public static double[] predictWithRotationAdjustment(final double[] x, final double[] u, final double dt) {
         final var result = new double[STATE_WITH_ROTATION_ADJUSTMENT_COMPONENTS];
         predictWithRotationAdjustment(x, u, dt, result);
         return 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.
      *
      * @param 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.
      * @param 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.
      * @param dt        time interval to compute prediction expressed in seconds.
      * @param result    instance where updated system model will be stored. Must
      *                  have length 13.
      * @param jacobianX jacobian wrt system state. Must be 13x13.
      * @param jacobianU jacobian wrt control. Must be 13x13.
      * @throws IllegalArgumentException if system state array, control array,
      *                                  result or jacobians do not have proper size.
      */
     public static void predictWithPositionAndRotationAdjustment(
             final double[] x, final double[] u, final double dt, final double[] result, final Matrix jacobianX,
             final Matrix jacobianU) {
         if (x.length != STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS) {
             throw new IllegalArgumentException("x must have length 13");
         }
         if (u.length != CONTROL_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS) {
             throw new IllegalArgumentException("u must have length 13");
         }
         if (result.length != STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS) {
             throw new IllegalArgumentException("result must have length 13");
         }
         if (jacobianX != null && (jacobianX.getRows() != STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS
                 || jacobianX.getColumns() != STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS)) {
             throw new IllegalArgumentException("jacobian wrt x must be 13x13");
         }
         if (jacobianU != null && (jacobianU.getRows() != STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS
                 || jacobianU.getColumns() != CONTROL_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS)) {
             throw new IllegalArgumentException("jacobian wrt u must be 13x13");
         }
 
         // position
         final var r = new InhomogeneousPoint3D(x[0], x[1], x[2]);
 
         // orientation
         var q = new Quaternion(x[3], x[4], x[5], x[6]);
 
         // linear velocity
         var vx = x[7];
         var vy = x[8];
         var vz = x[9];
 
         // angular velocity
         var wx = x[10];
         var wy = x[11];
         var wz = x[12];
 
         // position change (control)
         final var drx = u[0];
         final var dry = u[1];
         final var drz = u[2];
 
         // rotation change (control)
         final var dq = new Quaternion(u[3], u[4], u[5], u[6]);
 
         // linear velocity change (control)
         final var uvx = u[7];
         final var uvy = u[8];
         final var uvz = u[9];
 
         // angular velocity change (control)
         final var uwx = u[10];
         final var uwy = u[11];
         final var uwz = u[12];
 
         try {
             // update position
             Matrix rr = null;
             Matrix rv = null;
             if (jacobianX != null) {
                 rr = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH,
                         Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH);
                 rv = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, SPEED_COMPONENTS);
             }
             PositionPredictor.predictWithPositionAdjustment(r, drx, dry, drz, vx, vy, vz, 0.0, 0.0, 0.0,
                     dt, r, rr, null, rv, null);
 
             // update orientation
             Matrix qq = null;
             Matrix qdq = null;
             Matrix qw = null;
             if (jacobianX != null) {
                 qq = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
                 qw = new Matrix(Quaternion.N_PARAMS, ANGULAR_SPEED_COMPONENTS);
             }
             if (jacobianU != null) {
                 qdq = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
             }
             q = QuaternionPredictor.predictWithRotationAdjustment(q, dq, wx, wy, wz, dt, qq, qdq, qw);
 
             // apply control signals
             vx += uvx;
             vy += uvy;
             vz += uvz;
 
             wx += uwx;
             wy += uwy;
             wz += uwz;
 
             // set new state
             result[0] = r.getInhomX();
             result[1] = r.getInhomY();
             result[2] = r.getInhomZ();
 
             result[3] = q.getA();
             result[4] = q.getB();
             result[5] = q.getC();
             result[6] = q.getD();
 
             result[7] = vx;
             result[8] = vy;
             result[9] = vz;
 
             result[10] = wx;
             result[11] = wy;
             result[12] = wz;
 
             // jacobians
             if (jacobianX != null) {
                 // [Rr   0   Rv  0  ]
                 // [0    Qq  0   Qw ]
                 // [0    0   eye 0  ]
                 // [0    0   0   eye]
                 jacobianX.initialize(0.0);
                 jacobianX.setSubmatrix(0, 0, 2, 2, rr);
 
                 jacobianX.setSubmatrix(3, 3, 6, 6, qq);
 
                 jacobianX.setSubmatrix(0, 7, 2, 9, rv);
 
                 for (int i = 7; i < STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS; i++) {
                     jacobianX.setElementAt(i, i, 1.0);
                 }
 
                 jacobianX.setSubmatrix(3, 10, 6, 12, qw);
             }
 
             if (jacobianU != null) {
                 jacobianU.initialize(0.0);
                 // variation of position
                 for (var i = 0; i < Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH; i++) {
                     jacobianU.setElementAt(i, i, 1.0);
                 }
 
                 // variation of rotation
                 jacobianU.setSubmatrix(3, 3, 6, 6, qdq);
 
                 // variation of linear and angular speed
                 for (var i = 7; i < STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS; i++) {
                     jacobianU.setElementAt(i, i, 1.0);
                 }
             }
 
         } catch (final WrongSizeException ignore) {
             // never thrown
         }
     }
 
     /**
      * 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.
      *
      * @param 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.
      * @param 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.
      * @param dt     time interval to compute prediction expressed in seconds.
      * @param 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.
      */
     public static void predictWithPositionAndRotationAdjustment(
             final double[] x, final double[] u, final double dt, final double[] result) {
         predictWithPositionAndRotationAdjustment(x, u, dt, result, null, null);
     }
 
     /**
      * 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.
      *
      * @param 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.
      * @param 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.
      * @param dt        time interval to compute prediction expressed in seconds.
      * @param jacobianX jacobian wrt system state. Must be 13x13.
      * @param jacobianU jacobian wrt control. Must be 13x13.
      * @return a new array containing updated system model.
      * @throws IllegalArgumentException if system state array, control array
      *                                  or jacobians do not have proper size.
      */
     public static double[] predictWithPositionAndRotationAdjustment(
             final double[] x, final double[] u, final double dt, final Matrix jacobianX, final Matrix jacobianU) {
         final var result = new double[STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS];
         predictWithPositionAndRotationAdjustment(x, u, dt, result, jacobianX, jacobianU);
         return 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.
      *
      * @param 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.
      * @param 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.
      * @param dt time interval to compute prediction expressed in seconds.
      * @return 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.
      */
     public static double[] predictWithPositionAndRotationAdjustment(
             final double[] x, final double[] u, final double dt) {
         final var result = new double[STATE_WITH_POSITION_AND_ROTATION_ADJUSTMENT_COMPONENTS];
         predictWithPositionAndRotationAdjustment(x, u, dt, result);
         return result;
     }
 }