PartialDerivativesEquations.java

  1. /* Copyright 2010-2011 Centre National d'Études Spatiales
  2.  * Licensed to CS Systèmes d'Information (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.propagation.numerical;

  19. import java.util.ArrayList;
  20. import java.util.Arrays;
  21. import java.util.List;
  22. import java.util.SortedSet;
  23. import java.util.TreeSet;

  25. import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
  26. import org.apache.commons.math3.geometry.euclidean.threed.FieldRotation;
  27. import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
  28. import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
  29. import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  30. import org.apache.commons.math3.util.FastMath;
  31. import org.apache.commons.math3.util.Precision;
  32. import org.orekit.errors.OrekitException;
  33. import org.orekit.errors.OrekitMessages;
  34. import org.orekit.forces.ForceModel;
  35. import org.orekit.propagation.SpacecraftState;
  36. import org.orekit.propagation.integration.AdditionalEquations;

  38. /** Set of {@link AdditionalEquations additional equations} computing the partial derivatives
  39.  * of the state (orbit) with respect to initial state and force models parameters.
  40.  * <p>
  41.  * This set of equations are automatically added to a {@link NumericalPropagator numerical propagator}
  42.  * in order to compute partial derivatives of the orbit along with the orbit itself. This is
  43.  * useful for example in orbit determination applications.
  44.  * </p>
  45.  * @author V&eacute;ronique Pommier-Maurussane
  46.  */
  47. public class PartialDerivativesEquations implements AdditionalEquations {

  49.     /** Selected parameters for Jacobian computation. */
  50.     private NumericalPropagator propagator;

  52.     /** Derivatives providers. */
  53.     private final List<ForceModel> derivativesProviders;

  55.     /** List of parameters selected for Jacobians computation. */
  56.     private List<ParameterConfiguration> selectedParameters;

  58.     /** Name. */
  59.     private String name;

  61.     /** State vector dimension without additional parameters
  62.      * (either 6 or 7 depending on mass derivatives being included or not). */
  63.     private int stateDim;

  65.     /** Parameters vector dimension. */
  66.     private int paramDim;

  68.     /** Step used for finite difference computation with respect to spacecraft position. */
  69.     private double hPos;

  71.     /** Boolean for force models / selected parameters consistency. */
  72.     private boolean dirty = false;

  74.     /** Jacobian of acceleration with respect to spacecraft position. */
  75.     private transient double[][] dAccdPos;

  77.     /** Jacobian of acceleration with respect to spacecraft velocity. */
  78.     private transient double[][] dAccdVel;

  80.     /** Jacobian of acceleration with respect to spacecraft mass. */
  81.     private transient double[]   dAccdM;

  83.     /** Jacobian of acceleration with respect to one force model parameter (array reused for all parameters). */
  84.     private transient double[]   dAccdParam;

  86.     /** Simple constructor.
  87.      * <p>
  88.      * Upon construction, this set of equations is <em>automatically</em> added to
  89.      * the propagator by calling its {@link
  90.      * NumericalPropagator#addAdditionalEquations(AdditionalEquations)} method. So
  91.      * there is no need to call this method explicitly for these equations.
  92.      * </p>
  93.      * @param name name of the partial derivatives equations
  94.      * @param propagator the propagator that will handle the orbit propagation
  95.      * @exception OrekitException if a set of equations with the same name is already present
  96.      */
  97.     public PartialDerivativesEquations(final String name, final NumericalPropagator propagator)
  98.         throws OrekitException {
  99.         this.name = name;
  100.         derivativesProviders = new ArrayList<ForceModel>();
  101.         dirty = true;
  102.         this.propagator = propagator;
  103.         selectedParameters = new ArrayList<ParameterConfiguration>();
  104.         stateDim = -1;
  105.         paramDim = -1;
  106.         hPos     = Double.NaN;
  107.         propagator.addAdditionalEquations(this);
  108.     }

  110.     /** {@inheritDoc} */
  111.     public String getName() {
  112.         return name;
  113.     }

  115.     /** Get the names of the available parameters in the propagator.
  116.      * <p>
  117.      * The names returned depend on the force models set up in the propagator,
  118.      * including the Newtonian attraction from the central body.
  119.      * </p>
  120.      * @return available parameters
  121.      */
  122.     public List<String> getAvailableParameters() {
  123.         final List<String> available = new ArrayList<String>();
  124.         available.addAll(propagator.getNewtonianAttractionForceModel().getParametersNames());
  125.         for (final ForceModel model : propagator.getForceModels()) {
  126.             available.addAll(model.getParametersNames());
  127.         }
  128.         return available;
  129.     }

  131.     /** Select the parameters to consider for Jacobian processing.
  132.      * <p>Parameters names have to be consistent with some
  133.      * {@link ForceModel} added elsewhere.</p>
  134.      * @param parameters parameters to consider for Jacobian processing
  135.      * @see NumericalPropagator#addForceModel(ForceModel)
  136.      * @see #setInitialJacobians(SpacecraftState, double[][], double[][])
  137.      * @see ForceModel
  138.      * @see org.apache.commons.math3.ode.Parameterizable

  140.      */
  141.     public void selectParameters(final String ... parameters) {

  143.         selectedParameters.clear();
  144.         for (String param : parameters) {
  145.             selectedParameters.add(new ParameterConfiguration(param, Double.NaN));
  146.         }

  148.         dirty = true;

  150.     }

  152.     /** Select the parameters to consider for Jacobian processing.
  153.      * <p>Parameters names have to be consistent with some
  154.      * {@link ForceModel} added elsewhere.</p>
  155.      * @param parameter parameter to consider for Jacobian processing
  156.      * @param hP step to use for computing Jacobian column with respect to the specified parameter
  157.      * @see NumericalPropagator#addForceModel(ForceModel)
  158.      * @see #setInitialJacobians(SpacecraftState, double[][], double[][])
  159.      * @see ForceModel
  160.      * @see org.apache.commons.math3.ode.Parameterizable
  161.      */
  162.     public void selectParamAndStep(final String parameter, final double hP) {
  163.         selectedParameters.add(new ParameterConfiguration(parameter, hP));
  164.         dirty = true;
  165.     }

  167.     /** Set the initial value of the Jacobian with respect to state and parameter.
  168.      * <p>
  169.      * This method is equivalent to call {@link #setInitialJacobians(SpacecraftState,
  170.      * double[][], double[][])} with dYdY0 set to the identity matrix and dYdP set
  171.      * to a zero matrix.
  172.      * </p>
  173.      * <p>
  174.      * The returned state must be added to the propagator (it is not done
  175.      * automatically, as the user may need to add more states to it).
  176.      * </p>
  177.      * @param s0 initial state
  178.      * @param stateDimension state dimension, must be either 6 for orbit only or 7 for orbit and mass
  179.      * @param paramDimension parameters dimension
  180.      * @return state with initial Jacobians added
  181.      * @exception OrekitException if the partial equation has not been registered in
  182.      * the propagator or if matrices dimensions are incorrect
  183.      * @see #selectedParameters
  184.      * @see #selectParamAndStep(String, double)
  185.      */
  186.     public SpacecraftState setInitialJacobians(final SpacecraftState s0, final int stateDimension, final int paramDimension)
  187.         throws OrekitException {
  188.         final double[][] dYdY0 = new double[stateDimension][stateDimension];
  189.         final double[][] dYdP  = new double[stateDimension][paramDimension];
  190.         for (int i = 0; i < stateDimension; ++i) {
  191.             dYdY0[i][i] = 1.0;
  192.         }
  193.         return setInitialJacobians(s0, dYdY0, dYdP);
  194.     }

  196.     /** Set the initial value of the Jacobian with respect to state and parameter.
  197.      * <p>
  198.      * The returned state must be added to the propagator (it is not done
  199.      * automatically, as the user may need to add more states to it).
  200.      * </p>
  201.      * @param s1 current state
  202.      * @param dY1dY0 Jacobian of current state at time t<sub>1</sub> with respect
  203.      * to state at some previous time t<sub>0</sub> (may be either 6x6 for orbit
  204.      * only or 7x7 for orbit and mass)
  205.      * @param dY1dP Jacobian of current state at time t<sub>1</sub> with respect
  206.      * to parameters (may be null if no parameters are selected)
  207.      * @return state with initial Jacobians added
  208.      * @exception OrekitException if the partial equation has not been registered in
  209.      * the propagator or if matrices dimensions are incorrect
  210.      * @see #selectedParameters
  211.      * @see #selectParamAndStep(String, double)
  212.      */
  213.     public SpacecraftState setInitialJacobians(final SpacecraftState s1,
  214.                                                final double[][] dY1dY0, final double[][] dY1dP)
  215.         throws OrekitException {

  217.         // Check dimensions
  218.         stateDim = dY1dY0.length;
  219.         if ((stateDim < 6) || (stateDim > 7) || (stateDim != dY1dY0[0].length)) {
  220.             throw new OrekitException(OrekitMessages.STATE_JACOBIAN_NEITHER_6X6_NOR_7X7,
  221.                                       stateDim, dY1dY0[0].length);
  222.         }
  223.         if ((dY1dP != null) && (stateDim != dY1dP.length)) {
  224.             throw new OrekitException(OrekitMessages.STATE_AND_PARAMETERS_JACOBIANS_ROWS_MISMATCH,
  225.                                       stateDim, dY1dP.length);
  226.         }

  228.         paramDim = (dY1dP == null) ? 0 : dY1dP[0].length;

  230.         // store the matrices as a single dimension array
  231.         final JacobiansMapper mapper = getMapper();
  232.         final double[] p = new double[mapper.getAdditionalStateDimension()];
  233.         mapper.setInitialJacobians(s1, dY1dY0, dY1dP, p);

  235.         // set value in propagator
  236.         return s1.addAdditionalState(name, p);

  238.     }

  240.     /** Get a mapper between two-dimensional Jacobians and one-dimensional additional state.
  241.      * @return a mapper between two-dimensional Jacobians and one-dimensional additional state,
  242.      * with the same name as the instance
  243.      * @exception OrekitException if the initial Jacobians have not been initialized yet
  244.      * @see #setInitialJacobians(SpacecraftState, int, int)
  245.      * @see #setInitialJacobians(SpacecraftState, double[][], double[][])
  246.      */
  247.     public JacobiansMapper getMapper() throws OrekitException {
  248.         if (stateDim < 0) {
  249.             throw new OrekitException(OrekitMessages.STATE_JACOBIAN_NOT_INITIALIZED);
  250.         }
  251.         return new JacobiansMapper(name, stateDim, paramDim,
  252.                                    propagator.getOrbitType(),
  253.                                    propagator.getPositionAngleType());
  254.     }

  256.     /** {@inheritDoc} */
  257.     public double[] computeDerivatives(final SpacecraftState s, final double[] pDot)
  258.         throws OrekitException {

  260.         final int dim = 3;

  262.         // Lazy initialization
  263.         if (dirty) {

  265.             // if step has not been set by user, set a default value
  266.             if (Double.isNaN(hPos)) {
  267.                 hPos = FastMath.sqrt(Precision.EPSILON) * s.getPVCoordinates().getPosition().getNorm();
  268.             }

  270.              // set up derivatives providers
  271.             derivativesProviders.clear();
  272.             derivativesProviders.addAll(propagator.getForceModels());
  273.             derivativesProviders.add(propagator.getNewtonianAttractionForceModel());

  275.             // check all parameters are handled by at least one Jacobian provider
  276.             for (final ParameterConfiguration param : selectedParameters) {
  277.                 final String parameterName = param.getParameterName();
  278.                 boolean found = false;
  279.                 for (final ForceModel provider : derivativesProviders) {
  280.                     if (provider.isSupported(parameterName)) {
  281.                         param.setProvider(provider);
  282.                         found = true;
  283.                     }
  284.                 }
  285.                 if (!found) {

  287.                     // build the list of supported parameters, avoiding duplication
  288.                     final SortedSet<String> set = new TreeSet<String>();
  289.                     for (final ForceModel provider : derivativesProviders) {
  290.                         for (final String forceModelParameter : provider.getParametersNames()) {
  291.                             set.add(forceModelParameter);
  292.                         }
  293.                     }
  294.                     final StringBuilder builder = new StringBuilder();
  295.                     for (final String forceModelParameter : set) {
  296.                         if (builder.length() > 0) {
  297.                             builder.append(", ");
  298.                         }
  299.                         builder.append(forceModelParameter);
  300.                     }

  302.                     throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
  303.                                               parameterName, builder.toString());

  305.                 }
  306.             }

  308.             // check the numbers of parameters and matrix size agree
  309.             if (selectedParameters.size() != paramDim) {
  310.                 throw new OrekitException(OrekitMessages.INITIAL_MATRIX_AND_PARAMETERS_NUMBER_MISMATCH,
  311.                                           paramDim, selectedParameters.size());
  312.             }

  314.             dAccdParam = new double[dim];
  315.             dAccdPos   = new double[dim][dim];
  316.             dAccdVel   = new double[dim][dim];
  317.             dAccdM     = (stateDim > 6) ? new double[dim] : null;

  319.             dirty = false;

  321.         }

  323.         // initialize acceleration Jacobians to zero
  324.         for (final double[] row : dAccdPos) {
  325.             Arrays.fill(row, 0.0);
  326.         }
  327.         for (final double[] row : dAccdVel) {
  328.             Arrays.fill(row, 0.0);
  329.         }
  330.         if (dAccdM != null) {
  331.             Arrays.fill(dAccdM, 0.0);
  332.         }

  334.         // prepare derivation variables, 3 for position, 3 for velocity and optionally 1 for mass
  335.         final int nbVars = (dAccdM == null) ? 6 : 7;

  337.         // position corresponds three free parameters
  338.         final Vector3D position = s.getPVCoordinates().getPosition();
  339.         final FieldVector3D<DerivativeStructure> dsP = new FieldVector3D<DerivativeStructure>(new DerivativeStructure(nbVars, 1, 0, position.getX()),
  340.                                               new DerivativeStructure(nbVars, 1, 1, position.getY()),
  341.                                               new DerivativeStructure(nbVars, 1, 2, position.getZ()));

  343.         // velocity corresponds three free parameters
  344.         final Vector3D velocity = s.getPVCoordinates().getPosition();
  345.         final FieldVector3D<DerivativeStructure> dsV = new FieldVector3D<DerivativeStructure>(new DerivativeStructure(nbVars, 1, 3, velocity.getX()),
  346.                                               new DerivativeStructure(nbVars, 1, 4, velocity.getY()),
  347.                                               new DerivativeStructure(nbVars, 1, 5, velocity.getZ()));

  349.         // mass corresponds either to a constant or to one free parameter
  350.         final DerivativeStructure dsM = (dAccdM == null) ?
  351.                                         new DerivativeStructure(nbVars, 1,    s.getMass()) :
  352.                                         new DerivativeStructure(nbVars, 1, 6, s.getMass());

  354.         // TODO:  we should compute attitude partial derivatives with respect to position/velocity
  355.         final Rotation rotation = s.getAttitude().getRotation();
  356.         final FieldRotation<DerivativeStructure> dsR =
  357.                 new FieldRotation<DerivativeStructure>(new DerivativeStructure(nbVars, 1, rotation.getQ0()),
  358.                                new DerivativeStructure(nbVars, 1, rotation.getQ1()),
  359.                                new DerivativeStructure(nbVars, 1, rotation.getQ2()),
  360.                                new DerivativeStructure(nbVars, 1, rotation.getQ3()),
  361.                                false);

  363.         // compute acceleration Jacobians
  364.         for (final ForceModel derivativesProvider : derivativesProviders) {
  365.             final FieldVector3D<DerivativeStructure> acceleration =
  366.                     derivativesProvider.accelerationDerivatives(s.getDate(), s.getFrame(),
  367.                                                                 dsP, dsV, dsR, dsM);
  368.             addToRow(acceleration.getX(), 0);
  369.             addToRow(acceleration.getY(), 1);
  370.             addToRow(acceleration.getZ(), 2);
  371.         }

  373.         // the variational equations of the complete state Jacobian matrix have the
  374.         // following form for 7x7, i.e. when mass partial derivatives are also considered
  375.         // (when mass is not considered, only the A, B, D and E matrices are used along
  376.         // with their derivatives):

  378.         // [       |        |       ]   [                 |                  |               ]   [    |     |    ]
  379.         // [ Adot  |  Bdot  |  Cdot ]   [  dVel/dPos = 0  |  dVel/dVel = Id  |   dVel/dm = 0 ]   [ A  |  B  |  C ]
  380.         // [       |        |       ]   [                 |                  |               ]   [    |     |    ]
  381.         // --------+--------+--- ----   ------------------+------------------+----------------   -----+-----+-----
  382.         // [       |        |       ]   [                 |                  |               ]   [    |     |    ]
  383.         // [ Ddot  |  Edot  |  Fdot ] = [    dAcc/dPos    |     dAcc/dVel    |    dAcc/dm    ] * [ D  |  E  |  F ]
  384.         // [       |        |       ]   [                 |                  |               ]   [    |     |    ]
  385.         // --------+--------+--- ----   ------------------+------------------+----------------   -----+-----+-----
  386.         // [ Gdot  |  Hdot  |  Idot ]   [ dmDot/dPos = 0  |  dmDot/dVel = 0  |  dmDot/dm = 0 ]   [ G  |  H  |  I ]

  388.         // The A, B, D and E sub-matrices and their derivatives (Adot ...) are 3x3 matrices,
  389.         // the C and F sub-matrices and their derivatives (Cdot ...) are 3x1 matrices,
  390.         // the G and H sub-matrices and their derivatives (Gdot ...) are 1x3 matrices,
  391.         // the I sub-matrix and its derivative (Idot) is a 1x1 matrix.

  393.         // The expanded multiplication above can be rewritten to take into account
  394.         // the fixed values found in the sub-matrices in the left factor. This leads to:

  396.         //     [ Adot ] = [ D ]
  397.         //     [ Bdot ] = [ E ]
  398.         //     [ Cdot ] = [ F ]
  399.         //     [ Ddot ] = [ dAcc/dPos ] * [ A ] + [ dAcc/dVel ] * [ D ] + [ dAcc/dm ] * [ G ]
  400.         //     [ Edot ] = [ dAcc/dPos ] * [ B ] + [ dAcc/dVel ] * [ E ] + [ dAcc/dm ] * [ H ]
  401.         //     [ Fdot ] = [ dAcc/dPos ] * [ C ] + [ dAcc/dVel ] * [ F ] + [ dAcc/dm ] * [ I ]
  402.         //     [ Gdot ] = [ 0 ]
  403.         //     [ Hdot ] = [ 0 ]
  404.         //     [ Idot ] = [ 0 ]

  406.         // The following loops compute these expressions taking care of the mapping of the
  407.         // (A, B, ... I) matrices into the single dimension array p and of the mapping of the
  408.         // (Adot, Bdot, ... Idot) matrices into the single dimension array pDot.

  410.         // copy D, E and F into Adot, Bdot and Cdot
  411.         final double[] p = s.getAdditionalState(getName());
  412.         System.arraycopy(p, dim * stateDim, pDot, 0, dim * stateDim);

  414.         // compute Ddot, Edot and Fdot
  415.         for (int i = 0; i < dim; ++i) {
  416.             final double[] dAdPi = dAccdPos[i];
  417.             final double[] dAdVi = dAccdVel[i];
  418.             for (int j = 0; j < stateDim; ++j) {
  419.                 pDot[(dim + i) * stateDim + j] =
  420.                     dAdPi[0] * p[j]                + dAdPi[1] * p[j +     stateDim] + dAdPi[2] * p[j + 2 * stateDim] +
  421.                     dAdVi[0] * p[j + 3 * stateDim] + dAdVi[1] * p[j + 4 * stateDim] + dAdVi[2] * p[j + 5 * stateDim] +
  422.                     ((dAccdM == null) ? 0.0 : dAccdM[i] * p[j + 6 * stateDim]);
  423.             }
  424.         }

  426.         if (dAccdM != null) {
  427.             // set Gdot, Hdot and Idot to 0
  428.             Arrays.fill(pDot, 6 * stateDim, 7 * stateDim, 0.0);
  429.         }

  431.         for (int k = 0; k < paramDim; ++k) {

  433.             // compute the acceleration gradient with respect to current parameter
  434.             final ParameterConfiguration param = selectedParameters.get(k);
  435.             final ForceModel provider = param.getProvider();
  436.             final FieldVector3D<DerivativeStructure> accDer =
  437.                     provider.accelerationDerivatives(s, param.getParameterName());
  438.             dAccdParam[0] = accDer.getX().getPartialDerivative(1);
  439.             dAccdParam[1] = accDer.getY().getPartialDerivative(1);
  440.             dAccdParam[2] = accDer.getZ().getPartialDerivative(1);

  442.             // the variational equations of the parameters Jacobian matrix are computed
  443.             // one column at a time, they have the following form:
  444.             // [      ]   [                 |                  |               ]   [   ]   [                  ]
  445.             // [ Jdot ]   [  dVel/dPos = 0  |  dVel/dVel = Id  |   dVel/dm = 0 ]   [ J ]   [  dVel/dParam = 0 ]
  446.             // [      ]   [                 |                  |               ]   [   ]   [                  ]
  447.             // --------   ------------------+------------------+----------------   -----   --------------------
  448.             // [      ]   [                 |                  |               ]   [   ]   [                  ]
  449.             // [ Kdot ] = [    dAcc/dPos    |     dAcc/dVel    |    dAcc/dm    ] * [ K ] + [    dAcc/dParam   ]
  450.             // [      ]   [                 |                  |               ]   [   ]   [                  ]
  451.             // --------   ------------------+------------------+----------------   -----   --------------------
  452.             // [ Ldot ]   [ dmDot/dPos = 0  |  dmDot/dVel = 0  |  dmDot/dm = 0 ]   [ L ]   [ dmDot/dParam = 0 ]

  454.             // The J and K sub-columns and their derivatives (Jdot ...) are 3 elements columns,
  455.             // the L sub-colums and its derivative (Ldot) are 1 elements columns.

  457.             // The expanded multiplication and addition above can be rewritten to take into
  458.             // account the fixed values found in the sub-matrices in the left factor. This leads to:

  460.             //     [ Jdot ] = [ K ]
  461.             //     [ Kdot ] = [ dAcc/dPos ] * [ J ] + [ dAcc/dVel ] * [ K ] + [ dAcc/dm ] * [ L ] + [ dAcc/dParam ]
  462.             //     [ Ldot ] = [ 0 ]

  464.             // The following loops compute these expressions taking care of the mapping of the
  465.             // (J, K, L) columns into the single dimension array p and of the mapping of the
  466.             // (Jdot, Kdot, Ldot) columns into the single dimension array pDot.

  468.             // copy K into Jdot
  469.             final int columnTop = stateDim * stateDim + k;
  470.             pDot[columnTop]                = p[columnTop + 3 * paramDim];
  471.             pDot[columnTop +     paramDim] = p[columnTop + 4 * paramDim];
  472.             pDot[columnTop + 2 * paramDim] = p[columnTop + 5 * paramDim];

  474.             // compute Kdot
  475.             for (int i = 0; i < dim; ++i) {
  476.                 final double[] dAdPi = dAccdPos[i];
  477.                 final double[] dAdVi = dAccdVel[i];
  478.                 pDot[columnTop + (dim + i) * paramDim] =
  479.                     dAccdParam[i] +
  480.                     dAdPi[0] * p[columnTop]                + dAdPi[1] * p[columnTop +     paramDim] + dAdPi[2] * p[columnTop + 2 * paramDim] +
  481.                     dAdVi[0] * p[columnTop + 3 * paramDim] + dAdVi[1] * p[columnTop + 4 * paramDim] + dAdVi[2] * p[columnTop + 5 * paramDim] +
  482.                     ((dAccdM == null) ? 0.0 : dAccdM[i] * p[columnTop + 6 * paramDim]);
  483.             }

  485.             if (dAccdM != null) {
  486.                 // set Ldot to 0
  487.                 pDot[columnTop + 6 * paramDim] = 0;
  488.             }

  490.         }

  492.         // these equations have no effect of the main state itself
  493.         return null;

  495.     }

  497.     /** Fill Jacobians rows when mass is needed.
  498.      * @param accelerationComponent component of acceleration (along either x, y or z)
  499.      * @param index component index (0 for x, 1 for y, 2 for z)
  500.      */
  501.     private void addToRow(final DerivativeStructure accelerationComponent, final int index) {

  503.         if (dAccdM == null) {

  505.             // free parameters 0, 1, 2 are for position
  506.             dAccdPos[index][0] += accelerationComponent.getPartialDerivative(1, 0, 0, 0, 0, 0);
  507.             dAccdPos[index][1] += accelerationComponent.getPartialDerivative(0, 1, 0, 0, 0, 0);
  508.             dAccdPos[index][2] += accelerationComponent.getPartialDerivative(0, 0, 1, 0, 0, 0);

  510.             // free parameters 3, 4, 5 are for velocity
  511.             dAccdVel[index][0] += accelerationComponent.getPartialDerivative(0, 0, 0, 1, 0, 0);
  512.             dAccdVel[index][1] += accelerationComponent.getPartialDerivative(0, 0, 0, 0, 1, 0);
  513.             dAccdVel[index][2] += accelerationComponent.getPartialDerivative(0, 0, 0, 0, 0, 1);

  515.         } else {

  517.             // free parameters 0, 1, 2 are for position
  518.             dAccdPos[index][0] += accelerationComponent.getPartialDerivative(1, 0, 0, 0, 0, 0, 0);
  519.             dAccdPos[index][1] += accelerationComponent.getPartialDerivative(0, 1, 0, 0, 0, 0, 0);
  520.             dAccdPos[index][2] += accelerationComponent.getPartialDerivative(0, 0, 1, 0, 0, 0, 0);

  522.             // free parameters 3, 4, 5 are for velocity
  523.             dAccdVel[index][0] += accelerationComponent.getPartialDerivative(0, 0, 0, 1, 0, 0, 0);
  524.             dAccdVel[index][1] += accelerationComponent.getPartialDerivative(0, 0, 0, 0, 1, 0, 0);
  525.             dAccdVel[index][2] += accelerationComponent.getPartialDerivative(0, 0, 0, 0, 0, 1, 0);

  527.             // free parameter 6 is for mass
  528.             dAccdM[index]      += accelerationComponent.getPartialDerivative(0, 0, 0, 0, 0, 0, 1);

  530.         }

  532.     }

  534. }
Created with JaCoCo 0.6.3.201306030806