AbstractMultipleShooting.java

  1. /* Copyright 2002-2025 CS GROUP
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.utils;

  18. import java.util.ArrayList;
  19. import java.util.Arrays;
  20. import java.util.HashMap;
  21. import java.util.List;
  22. import java.util.Map;

  23. import org.hipparchus.exception.MathIllegalArgumentException;
  24. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  25. import org.hipparchus.linear.LUDecomposition;
  26. import org.hipparchus.linear.MatrixUtils;
  27. import org.hipparchus.linear.QRDecomposition;
  28. import org.hipparchus.linear.RealMatrix;
  29. import org.hipparchus.linear.RealVector;
  30. import org.orekit.attitudes.Attitude;
  31. import org.orekit.attitudes.AttitudeProvider;
  32. import org.orekit.propagation.SpacecraftState;
  33. import org.orekit.propagation.numerical.NumericalPropagator;
  34. import org.orekit.time.AbsoluteDate;

  35. /**
  36.  * Multiple shooting method using only constraints on state vectors of patch points (and possibly on epoch and integration time).
  37.  * @see "TRAJECTORY DESIGN AND ORBIT MAINTENANCE STRATEGIES IN MULTI-BODY DYNAMICAL REGIMES by Thomas A. Pavlak, Purdue University"
  38.  * @author William Desprats
  39.  * @author Alberto Fossà
  40.  * @since 10.2
  41.  */
  42. public abstract class AbstractMultipleShooting implements MultipleShooting {

  43.     /** Patch points along the trajectory. */
  44.     private final List<SpacecraftState> patchedSpacecraftStates;

  45.     /** List of Propagators. */
  46.     private final List<NumericalPropagator> propagatorList;

  47.     /** Duration of propagation along each arc. */
  48.     private final double[] propagationTime;

  49.     /** Free components of patch points. */
  50.     private final boolean[] freeCompsMap;

  51.     /** Free epochs of patch points. */
  52.     private final boolean[] freeEpochMap;

  53.     /** Number of free state components. */
  54.     private int nComps;

  55.     /** Number of free arc duration. */
  56.     // TODO add possibility to fix integration time span?
  57.     private final int nDuration;

  58.     /** Number of free epochs. */
  59.     private int nEpoch;

  60.     /** Scale time for update computation. */
  61.     private double scaleTime;

  62.     /** Scale length for update computation. */
  63.     private double scaleLength;

  64.     /** Patch points components which are constrained. */
  65.     private final Map<Integer, Double> mapConstraints;

  66.     /** True if the dynamical system is autonomous.
  67.      * In this case epochs and epochs constraints are omitted from the problem formulation. */
  68.     private final boolean isAutonomous;

  69.     /** Tolerance on the constraint vector. */
  70.     private final double tolerance;

  71.     /** Maximum number of iterations. */
  72.     private final int maxIter;

  73.     /** Expected name of the additional equations. */
  74.     private final String additionalName;

  75.     /** Simple Constructor.
  76.      * <p> Standard constructor for multiple shooting </p>
  77.      * @param initialGuessList initial patch points to be corrected
  78.      * @param propagatorList list of propagators associated to each patch point
  79.      * @param tolerance convergence tolerance on the constraint vector
  80.      * @param maxIter maximum number of iterations
  81.      * @param isAutonomous true if the dynamical system is autonomous (i.e. not dependent on the epoch)
  82.      * @param additionalName name of the additional equations
  83.      * @since 11.1
  84.      */
  85.     protected AbstractMultipleShooting(final List<SpacecraftState> initialGuessList,
  86.                                        final List<NumericalPropagator> propagatorList,
  87.                                        final double tolerance, final int maxIter,
  88.                                        final boolean isAutonomous, final String additionalName) {

  89.         this.patchedSpacecraftStates = initialGuessList;
  90.         this.propagatorList          = propagatorList;
  91.         this.isAutonomous            = isAutonomous;
  92.         this.additionalName          = additionalName;

  93.         // propagation duration
  94.         final int propagationNumber = initialGuessList.size() - 1;
  95.         propagationTime = new double[propagationNumber];
  96.         for (int i = 0; i < propagationNumber; i++) {
  97.             propagationTime[i] = initialGuessList.get(i + 1).getDate().durationFrom(initialGuessList.get(i).getDate());
  98.         }

  99.         // states components freedom
  100.         this.freeCompsMap = new boolean[6 * initialGuessList.size()];
  101.         Arrays.fill(freeCompsMap, true);

  102.         // epochs freedom
  103.         if (isAutonomous) {
  104.             // epochs omitted from problem formulation
  105.             this.freeEpochMap = new boolean[0];
  106.         } else {
  107.             this.freeEpochMap = new boolean[initialGuessList.size()];
  108.             Arrays.fill(freeEpochMap, true);
  109.         }

  110.         // number of free variables
  111.         this.nComps    = 6 * initialGuessList.size();
  112.         this.nDuration = propagationNumber;
  113.         this.nEpoch    = freeEpochMap.length;

  114.         // convergence criteria
  115.         this.tolerance = tolerance;
  116.         this.maxIter   = maxIter;

  117.         // scaling
  118.         this.scaleTime   = 1.0;
  119.         this.scaleLength = 1.0;

  120.         // all additional constraints must be set afterward
  121.         this.mapConstraints = new HashMap<>();
  122.     }

  123.     /** Get a patch point.
  124.      * @param i index of the patch point
  125.      * @return state of the patch point
  126.      * @since 11.1
  127.      */
  128.     protected SpacecraftState getPatchPoint(final int i) {
  129.         return patchedSpacecraftStates.get(i);
  130.     }

  131.     /** Set a component of a patch point to free or not.
  132.      * @param patchIndex Patch point index (zero-based)
  133.      * @param componentIndex Index of the component to be constrained (zero-based)
  134.      * @param isFree constraint value
  135.      */
  136.     public void setPatchPointComponentFreedom(final int patchIndex, final int componentIndex, final boolean isFree) {
  137.         if (freeCompsMap[6 * patchIndex + componentIndex] != isFree) {
  138.             freeCompsMap[6 * patchIndex + componentIndex] = isFree;
  139.             nComps += isFree ? 1 : -1;
  140.         }
  141.     }

  142.     /** Set the epoch of a patch point to free or not.
  143.      * @param patchIndex Patch point index (zero-based)
  144.      * @param isFree constraint value
  145.      */
  146.     public void setEpochFreedom(final int patchIndex, final boolean isFree) {
  147.         if (freeEpochMap[patchIndex] != isFree) {
  148.             freeEpochMap[patchIndex] = isFree;
  149.             nEpoch += isFree ? 1 : -1;
  150.         }
  151.     }

  152.     /** Set the scale time.
  153.      * @param scaleTime scale time in seconds
  154.      */
  155.     public void setScaleTime(final double scaleTime) {
  156.         this.scaleTime = scaleTime;
  157.     }

  158.     /** Set the scale length.
  159.      * @param scaleLength scale length in meters
  160.      */
  161.     public void setScaleLength(final double scaleLength) {
  162.         this.scaleLength = scaleLength;
  163.     }

  164.     /** Add a constraint on one component of one patch point.
  165.      * @param patchIndex Patch point index (zero-based)
  166.      * @param componentIndex Index of the component which is constrained (zero-based)
  167.      * @param constraintValue constraint value
  168.      */
  169.     public void addConstraint(final int patchIndex, final int componentIndex, final double constraintValue) {
  170.         mapConstraints.put(patchIndex * 6 + componentIndex, constraintValue);
  171.     }

  172.     /** {@inheritDoc} */
  173.     @Override
  174.     public List<SpacecraftState> compute() {

  175.         int iter = 0; // number of iteration
  176.         double fxNorm;

  177.         while (iter < maxIter) {

  178.             iter++;

  179.             // propagation (multi-threading see PropagatorsParallelizer)
  180.             final List<SpacecraftState> propagatedSP = propagatePatchedSpacecraftState();

  181.             // constraints computation
  182.             final RealVector fx = MatrixUtils.createRealVector(computeConstraint(propagatedSP));

  183.             // convergence check
  184.             fxNorm = fx.getNorm();
  185.             // System.out.printf(Locale.US, "Iter: %3d Error: %.16e%n", iter, fxNorm);
  186.             if (fxNorm < tolerance) {
  187.                 break;
  188.             }

  189.             // Jacobian matrix computation
  190.             final RealMatrix M = computeJacobianMatrix(propagatedSP);

  191.             // correction computation using minimum norm approach
  192.             // (i.e. minimize difference between solutions from successive iterations,
  193.             //  in other word try to stay close to initial guess. This is *not* a least-squares solution)
  194.             // see equation 3.12 in Pavlak's thesis
  195.             final RealMatrix MMt = M.multiplyTransposed(M);
  196.             RealVector dx;
  197.             try {
  198.                 dx = M.transpose().operate(new LUDecomposition(MMt, 0.0).getSolver().solve(fx));
  199.             } catch (MathIllegalArgumentException e) {
  200.                 dx = M.transpose().operate(new QRDecomposition(MMt, 0.0).getSolver().solve(fx));
  201.             }

  202.             // trajectory update
  203.             updateTrajectory(dx);
  204.         }

  205.         return patchedSpacecraftStates;
  206.     }

  207.     /** Compute the Jacobian matrix of the problem.
  208.      *  @param propagatedSP List of propagated SpacecraftStates (patch points)
  209.      *  @return jacobianMatrix Jacobian matrix
  210.      */
  211.     private RealMatrix computeJacobianMatrix(final List<SpacecraftState> propagatedSP) {

  212.         // The Jacobian matrix has the following form:
  213.         //
  214.         //          [     |     |     ]
  215.         //          [  A  |  B  |  C  ]
  216.         // DF(X) =  [     |     |     ]
  217.         //          [-----------------]
  218.         //          [  0  |  D  |  E  ]
  219.         //          [-----------------]
  220.         //          [  F  |  0  |  0  ]
  221.         //
  222.         // For a problem in which all the components of each patch points are free, A is detailed below:
  223.         //
  224.         //      [ phi1     -I                                   ]
  225.         //      [         phi2     -I                           ]
  226.         // A =  [                 ....    ....                  ]   6(n-1)x6n
  227.         //      [                         ....     ....         ]
  228.         //      [                                 phin-1    -I  ]
  229.         //
  230.         // If the duration of the propagation of each arc is the same:
  231.         //
  232.         //      [ xdot1f ]
  233.         //      [ xdot2f ]                      [  -1 ]
  234.         // B =  [  ....  ]   6(n-1)x1   and D = [ ... ]
  235.         //      [  ....  ]                      [  -1 ]
  236.         //      [xdotn-1f]
  237.         //
  238.         // Otherwise:
  239.         //
  240.         //      [ xdot1f                                ]
  241.         //      [        xdot2f                         ]
  242.         // B =  [                 ....                  ]   6(n-1)x(n-1) and D = -I
  243.         //      [                        ....           ]
  244.         //      [                             xdotn-1f  ]
  245.         //
  246.         // If the problem is not dependant on the epoch (e.g. CR3BP), the C, D and E matrices are not computed.
  247.         //
  248.         // Otherwise:
  249.         //      [ dx1f/dtau1                                           0 ]
  250.         //      [          dx2f/dtau2                                  0 ]
  251.         // C =  [                      ....                            0 ]   6(n-1)xn
  252.         //      [                              ....                    0 ]
  253.         //      [                                     dxn-1f/dtaun-1   0 ]
  254.         //
  255.         //      [ -1   1  0                 ]
  256.         //      [     -1   1  0             ]
  257.         // E =  [          ..   ..          ] n-1xn
  258.         //      [               ..   ..     ]
  259.         //      [                    -1   1 ]
  260.         //
  261.         // F is computed according to additional constraints
  262.         // (for now, closed orbit, or a component of a patch point equals to a specified value)

  263.         final int nArcs    = patchedSpacecraftStates.size() - 1;
  264.         final double scaleVel = scaleLength / scaleTime;
  265.         final double scaleAcc = scaleVel / scaleTime;
  266.         final RealMatrix M = MatrixUtils.createRealMatrix(getNumberOfConstraints(), getNumberOfFreeVariables());

  267.         int index = 0; // first column index for matrix A
  268.         int indexDuration = nComps; // first column index for matrix B
  269.         int indexEpoch = indexDuration + nDuration; // first column index for matrix C
  270.         for (int i = 0; i < nArcs; i++) {

  271.             final SpacecraftState finalState = propagatedSP.get(i);

  272.             // PV coordinates and state transition matrix at final time
  273.             final PVCoordinates pvf = finalState.getPVCoordinates();
  274.             final double[][] phi    = getStateTransitionMatrix(finalState); // already scaled

  275.             // Matrix A
  276.             for (int j = 0; j < 6; j++) { // Loop on 6 component of the patch point
  277.                 if (freeCompsMap[6 * i + j]) { // If this component is free
  278.                     for (int k = 0; k < 6; k++) { // Loop on the 6 component of the patch point constraint
  279.                         M.setEntry(6 * i + k, index, phi[k][j]);
  280.                     }
  281.                     if (i > 0) {
  282.                         M.setEntry(6 * (i - 1) + j, index, -1.0);
  283.                     }
  284.                     index++;
  285.                 }
  286.             }

  287.             // Matrix B
  288.             final double[][] pvfArray = new double[][] {
  289.                 {pvf.getVelocity().getX() / scaleVel},
  290.                 {pvf.getVelocity().getY() / scaleVel},
  291.                 {pvf.getVelocity().getZ() / scaleVel},
  292.                 {pvf.getAcceleration().getX() / scaleAcc},
  293.                 {pvf.getAcceleration().getY() / scaleAcc},
  294.                 {pvf.getAcceleration().getZ() / scaleAcc}};

  295.             M.setSubMatrix(pvfArray, 6 * i, indexDuration);
  296.             indexDuration++;

  297.             // Matrix C
  298.             // there is a typo in Pavlak's thesis, equations 3.48-3.49:
  299.             // the sign in front of the partials of the states with respect to epochs should be plus
  300.             if (!isAutonomous) {
  301.                 if (freeEpochMap[i]) { // If this component is free
  302.                     final double[] derivatives = finalState.getAdditionalState(additionalName);
  303.                     final double[][] subC = new double[6][1];
  304.                     for (int j = 0; j < 3; j++) {
  305.                         subC[j][0] = derivatives[derivatives.length - 6 + j] / scaleVel;
  306.                         subC[j + 3][0] = derivatives[derivatives.length - 3 + j] / scaleAcc;
  307.                     }
  308.                     M.setSubMatrix(subC, 6 * i, indexEpoch);
  309.                     indexEpoch++;
  310.                 }
  311.             }
  312.         }

  313.         // complete Matrix A
  314.         for (int j = 0; j < 6; j++) { // Loop on 6 component of the patch point
  315.             if (freeCompsMap[6 * nArcs + j]) { // If this component is free
  316.                 M.setEntry(6 * (nArcs - 1) + j, index, -1.0);
  317.                 index++;
  318.             }
  319.         }

  320.         // Matrices D and E
  321.         if (!isAutonomous) {
  322.             final double[][] subDE = computeEpochJacobianMatrix(propagatedSP);
  323.             M.setSubMatrix(subDE, 6 * nArcs, nComps);
  324.         }

  325.         // Matrix F
  326.         final double[][] subF = computeAdditionalJacobianMatrix(propagatedSP);
  327.         if (subF.length > 0) {
  328.             M.setSubMatrix(subF, isAutonomous ? 6 * nArcs : 7 * nArcs, 0);
  329.         }

  330.         return M;
  331.     }

  332.     /** Compute the constraint vector of the problem.
  333.      *  @param propagatedSP propagated SpacecraftState
  334.      *  @return constraint vector
  335.      */
  336.     private double[] computeConstraint(final List<SpacecraftState> propagatedSP) {

  337.         // The Constraint vector has the following form :
  338.         //
  339.         //         [ x1f - x2i ]---
  340.         //         [ x2f - x3i ]   |
  341.         // F(X) =  [    ...    ] vectors' equality for a continuous trajectory
  342.         //         [    ...    ]   |
  343.         //         [xn-1f - xni]---
  344.         //         [ d2-(d1+T) ]   continuity between epoch
  345.         //         [    ...    ]   and integration time
  346.         //         [dn-(dn-1+T)]---
  347.         //         [    ...    ] additional
  348.         //         [    ...    ] constraints

  349.         final int nPoints     = patchedSpacecraftStates.size();
  350.         final double scaleVel = scaleLength / scaleTime;
  351.         final double[] fx     = new double[getNumberOfConstraints()];

  352.         // state continuity
  353.         for (int i = 0; i < nPoints - 1; i++) {
  354.             final AbsolutePVCoordinates absPvi = patchedSpacecraftStates.get(i + 1).getAbsPVA();
  355.             final AbsolutePVCoordinates absPvf = propagatedSP.get(i).getAbsPVA();
  356.             final double[] deltaPos = absPvf.getPosition().subtract(absPvi.getPosition()).toArray();
  357.             final double[] deltaVel = absPvf.getVelocity().subtract(absPvi.getVelocity()).toArray();
  358.             for (int j = 0; j < 3; j++) {
  359.                 fx[6 * i + j]     = deltaPos[j] / scaleLength;
  360.                 fx[6 * i + 3 + j] = deltaVel[j] / scaleVel;
  361.             }
  362.         }

  363.         int index = 6 * (nPoints - 1);

  364.         // epoch constraints
  365.         if (!isAutonomous) {
  366.             for (int i = 0; i < nPoints - 1; i++) {
  367.                 final double deltaEpoch = patchedSpacecraftStates.get(i + 1).getDate()
  368.                                          .durationFrom(patchedSpacecraftStates.get(i).getDate());
  369.                 fx[index] = (deltaEpoch - propagationTime[i]) / scaleTime;
  370.                 index++;
  371.             }
  372.         }

  373.         // additional constraints
  374.         final double[] additionalConstraints = computeAdditionalConstraints(propagatedSP);
  375.         for (double constraint : additionalConstraints) {
  376.             fx[index] = constraint;
  377.             index++;
  378.         }

  379.         return fx;
  380.     }

  381.     /** Update the trajectory, and the propagation time.
  382.      *  @param dx correction on the initial vector
  383.      */
  384.     private void updateTrajectory(final RealVector dx) {
  385.         // X = [x1, ..., xn, T1, ..., Tn, d1, ..., dn]

  386.         final double scaleVel = scaleLength / scaleTime;

  387.         // Update propagation time
  388.         int indexDuration = nComps;
  389.         for (int i = 0; i < nDuration; i++) {
  390.             propagationTime[i] -= dx.getEntry(indexDuration) * scaleTime;
  391.             indexDuration++;
  392.         }

  393.         // Update patch points through SpacecraftStates
  394.         int index = 0;
  395.         int indexEpoch = nComps + nDuration;

  396.         for (int i = 0; i < patchedSpacecraftStates.size(); i++) {

  397.             // Get delta in position and velocity
  398.             final double[] deltaPV = new double[6];
  399.             for (int j = 0; j < 6; j++) { // Loop on 6 component of the patch point
  400.                 if (freeCompsMap[6 * i + j]) { // If this component is free (is to be updated)
  401.                     deltaPV[j] = dx.getEntry(index);
  402.                     index++;
  403.                 }
  404.             }
  405.             final Vector3D deltaP = new Vector3D(deltaPV[0], deltaPV[1], deltaPV[2]).scalarMultiply(scaleLength);
  406.             final Vector3D deltaV = new Vector3D(deltaPV[3], deltaPV[4], deltaPV[5]).scalarMultiply(scaleVel);

  407.             // Update the PVCoordinates of the patch point
  408.             final AbsolutePVCoordinates currentAPV = patchedSpacecraftStates.get(i).getAbsPVA();
  409.             final Vector3D position = currentAPV.getPosition().subtract(deltaP);
  410.             final Vector3D velocity = currentAPV.getVelocity().subtract(deltaV);
  411.             final PVCoordinates pv  = new PVCoordinates(position, velocity);

  412.             // Update epoch in the AbsolutePVCoordinates
  413.             AbsoluteDate epoch = currentAPV.getDate();
  414.             if (!isAutonomous) {
  415.                 if (freeEpochMap[i]) {
  416.                     epoch = epoch.shiftedBy(-dx.getEntry(indexEpoch) * scaleTime);
  417.                     indexEpoch++;
  418.                 }
  419.             } else {
  420.                 // for autonomous systems we arbitrarily fix the date of the first patch point
  421.                 if (i > 0) {
  422.                     epoch = patchedSpacecraftStates.get(i - 1).getDate().shiftedBy(propagationTime[i - 1]);
  423.                 }
  424.             }
  425.             final AbsolutePVCoordinates updatedAPV = new AbsolutePVCoordinates(currentAPV.getFrame(), epoch, pv);

  426.             // Update attitude epoch
  427.             // Last point does not have an associated propagator. The previous one is then selected.
  428.             final int iAttitude = i < getPropagatorList().size() ? i : getPropagatorList().size() - 1;
  429.             final AttitudeProvider attitudeProvider = getPropagatorList().get(iAttitude).getAttitudeProvider();
  430.             final Attitude attitude = attitudeProvider.getAttitude(updatedAPV, epoch, currentAPV.getFrame());

  431.             // Update the SpacecraftState using previously updated attitude and AbsolutePVCoordinates
  432.             patchedSpacecraftStates.set(i, new SpacecraftState(updatedAPV, attitude));
  433.         }

  434.     }

  435.     /** Propagate the patch point states.
  436.      *  @return propagatedSP propagated SpacecraftStates
  437.      */
  438.     private List<SpacecraftState> propagatePatchedSpacecraftState() {

  439.         final int nArcs = patchedSpacecraftStates.size() - 1;
  440.         final ArrayList<SpacecraftState> propagatedSP = new ArrayList<>(nArcs);

  441.         for (int i = 0; i < nArcs; i++) {

  442.             // SpacecraftState initialization
  443.             final SpacecraftState augmentedInitialState = getAugmentedInitialState(i);

  444.             // Propagator initialization
  445.             propagatorList.get(i).setInitialState(augmentedInitialState);

  446.             // Propagate trajectory
  447.             final AbsoluteDate target = augmentedInitialState.getDate().shiftedBy(propagationTime[i]);
  448.             final SpacecraftState finalState = propagatorList.get(i).propagate(target);

  449.             propagatedSP.add(finalState);
  450.         }

  451.         return propagatedSP;
  452.     }

  453.     /** Get the state transition matrix.
  454.      * @param s current spacecraft state
  455.      * @return the state transition matrix
  456.      */
  457.     private double[][] getStateTransitionMatrix(final SpacecraftState s) {
  458.         // Additional states
  459.         final DataDictionary dictionary = s.getAdditionalDataValues();
  460.         // Initialize state transition matrix
  461.         final int        dim  = 6;
  462.         final double[][] phiM = new double[dim][dim];

  463.         // Loop on entry set
  464.         for (final DataDictionary.Entry entry : dictionary.getData()) {
  465.             // Extract entry name
  466.             final String name = entry.getKey();
  467.             if (additionalName.equals(name) && entry.getValue() instanceof double[]) {
  468.                 final double[] stm = (double[]) entry.getValue();
  469.                 for (int i = 0; i < 3; i++) {
  470.                     for (int j = 0; j < 3; j++) {

  471.                         // partials of final position w.r.t. initial position
  472.                         phiM[i][j] = stm[6 * i + j];

  473.                         // partials of final position w.r.t. initial velocity
  474.                         phiM[i][j + 3] = stm[6 * i + j + 3] / scaleTime;

  475.                         // partials of final velocity w.r.t. initial position
  476.                         phiM[i + 3][j] = stm[6 * i + j + 18] * scaleTime;

  477.                         // partials of final velocity w.r.t. initial velocity
  478.                         phiM[i + 3][j + 3] = stm[6 * i + j + 21];
  479.                     }
  480.                 }
  481.             }
  482.         }

  483.         // Return state transition matrix
  484.         return phiM;
  485.     }

  486.     /** Compute a part of the Jacobian matrix with derivatives from epoch.
  487.      * The CR3BP is a time invariant problem. The derivatives w.r.t. epoch are zero.
  488.      *  @param propagatedSP propagatedSP
  489.      *  @return jacobianMatrix Jacobian sub-matrix
  490.      */
  491.     protected double[][] computeEpochJacobianMatrix(final List<SpacecraftState> propagatedSP) {

  492.         final int nRows    = patchedSpacecraftStates.size() - 1;
  493.         final double[][] M = new double[nRows][nDuration + nEpoch];

  494.         // The D and E sub-matrices have the following form:
  495.         //
  496.         // D = -I
  497.         //
  498.         //     [-1 +1  0          ]
  499.         //     [   -1 +1  0       ]
  500.         // F = [      .. ..       ]
  501.         //     [         .. ..  0 ]
  502.         //     [            -1 +1 ]

  503.         int index = nDuration;
  504.         for (int i = 0; i < nRows; i++) {

  505.             // components of D matrix
  506.             M[i][i] = -1.0;

  507.             // components of E matrix
  508.             if (freeEpochMap[i]) {
  509.                 M[i][index] = -1.0;
  510.                 index++;
  511.             }
  512.             if (freeEpochMap[i + 1]) {
  513.                 M[i][index] = 1.0;
  514.             }
  515.         }

  516.         return M;
  517.     }

  518.     /** Update the array of additional constraints.
  519.      * @param startIndex start index
  520.      * @param fxAdditional array of additional constraints
  521.      */
  522.     protected void updateAdditionalConstraints(final int startIndex, final double[] fxAdditional) {
  523.         int iter = startIndex;
  524.         final double scaleVel = scaleLength / scaleTime;
  525.         for (final Map.Entry<Integer, Double> entry : getConstraintsMap().entrySet()) {
  526.             // Extract entry values
  527.             final int    key   = entry.getKey();
  528.             final double value = entry.getValue();
  529.             final int np = key / 6;
  530.             final int nc = key % 6;
  531.             final AbsolutePVCoordinates absPv = getPatchedSpacecraftState().get(np).getAbsPVA();
  532.             if (nc < 3) {
  533.                 fxAdditional[iter] = (absPv.getPosition().toArray()[nc] - value) / scaleLength;
  534.             } else {
  535.                 fxAdditional[iter] = (absPv.getVelocity().toArray()[nc - 3] -  value) / scaleVel;
  536.             }
  537.             iter++;
  538.         }
  539.     }

  540.     /** Compute the additional constraints.
  541.      *  @param propagatedSP propagated SpacecraftState
  542.      *  @return fxAdditional additional constraints
  543.      */
  544.     protected abstract double[] computeAdditionalConstraints(List<SpacecraftState> propagatedSP);

  545.     /** Compute a part of the Jacobian matrix from additional constraints.
  546.      *  @param propagatedSP propagatedSP
  547.      *  @return jacobianMatrix Jacobian sub-matrix
  548.      */
  549.     protected abstract double[][] computeAdditionalJacobianMatrix(List<SpacecraftState> propagatedSP);

  550.     /** Compute the additional state from the additionalEquations.
  551.      *  @param i index of the state
  552.      *  @return augmentedSP SpacecraftState with the additional state within.
  553.      *  @since 11.1
  554.      */
  555.     protected abstract SpacecraftState getAugmentedInitialState(int i);

  556.     /** Get the number of free state components.
  557.      * @return number of free components
  558.      */
  559.     protected int getNumberOfFreeComponents() {
  560.         return nComps;
  561.     }

  562.     /** Get the total number of constraints.
  563.      * @return the total number of constraints
  564.      */
  565.     protected int getNumberOfConstraints() {
  566.         final int nArcs = patchedSpacecraftStates.size() - 1;
  567.         return (isAutonomous ? 6 * nArcs : 7 * nArcs) + mapConstraints.size();
  568.     }

  569.     /** Get the map of free state components.
  570.      * @return map of free state components
  571.      */
  572.     protected boolean[] getFreeCompsMap() {
  573.         return freeCompsMap;
  574.     }

  575.     /** Get the map of patch points components which are constrained.
  576.      * @return a map of patch points components which are constrained
  577.      */
  578.     protected Map<Integer, Double> getConstraintsMap() {
  579.         return mapConstraints;
  580.     }

  581.     /** Get the list of patched spacecraft states.
  582.      * @return a list of patched spacecraft states
  583.      */
  584.     protected List<SpacecraftState> getPatchedSpacecraftState() {
  585.         return patchedSpacecraftStates;
  586.     }

  587.     /** Get the list of propagators.
  588.      * @return a list of propagators
  589.      */
  590.     private List<NumericalPropagator> getPropagatorList() {
  591.         return propagatorList;
  592.     }

  593.     /** Get the number of free variables.
  594.      * @return the number of free variables
  595.      */
  596.     private int getNumberOfFreeVariables() {
  597.         return nComps + nDuration + nEpoch;
  598.     }

  599. }