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;
18  
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;
24  
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;
37  
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 {
48  
49      /** Selected parameters for Jacobian computation. */
50      private NumericalPropagator propagator;
51  
52      /** Derivatives providers. */
53      private final List<ForceModel> derivativesProviders;
54  
55      /** List of parameters selected for Jacobians computation. */
56      private List<ParameterConfiguration> selectedParameters;
57  
58      /** Name. */
59      private String name;
60  
61      /** State vector dimension without additional parameters
62       * (either 6 or 7 depending on mass derivatives being included or not). */
63      private int stateDim;
64  
65      /** Parameters vector dimension. */
66      private int paramDim;
67  
68      /** Step used for finite difference computation with respect to spacecraft position. */
69      private double hPos;
70  
71      /** Boolean for force models / selected parameters consistency. */
72      private boolean dirty = false;
73  
74      /** Jacobian of acceleration with respect to spacecraft position. */
75      private transient double[][] dAccdPos;
76  
77      /** Jacobian of acceleration with respect to spacecraft velocity. */
78      private transient double[][] dAccdVel;
79  
80      /** Jacobian of acceleration with respect to spacecraft mass. */
81      private transient double[]   dAccdM;
82  
83      /** Jacobian of acceleration with respect to one force model parameter (array reused for all parameters). */
84      private transient double[]   dAccdParam;
85  
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     }
109 
110     /** {@inheritDoc} */
111     public String getName() {
112         return name;
113     }
114 
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     }
130 
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
139 
140      */
141     public void selectParameters(final String ... parameters) {
142 
143         selectedParameters.clear();
144         for (String param : parameters) {
145             selectedParameters.add(new ParameterConfiguration(param, Double.NaN));
146         }
147 
148         dirty = true;
149 
150     }
151 
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     }
166 
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     }
195 
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₁ with respect
203      * to state at some previous time t₀ (may be either 6x6 for orbit
204      * only or 7x7 for orbit and mass)
205      * @param dY1dP Jacobian of current state at time t₁ 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 {
216 
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         }
227 
228         paramDim = (dY1dP == null) ? 0 : dY1dP[0].length;
229 
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);
234 
235         // set value in propagator
236         return s1.addAdditionalState(name, p);
237 
238     }
239 
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     }
255 
256     /** {@inheritDoc} */
257     public double[] computeDerivatives(final SpacecraftState s, final double[] pDot)
258         throws OrekitException {
259 
260         final int dim = 3;
261 
262         // Lazy initialization
263         if (dirty) {
264 
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             }
269 
270              // set up derivatives providers
271             derivativesProviders.clear();
272             derivativesProviders.addAll(propagator.getForceModels());
273             derivativesProviders.add(propagator.getNewtonianAttractionForceModel());
274 
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) {
286 
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                     }
301 
302                     throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
303                                               parameterName, builder.toString());
304 
305                 }
306             }
307 
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             }
313 
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;
318 
319             dirty = false;
320 
321         }
322 
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         }
333 
334         // prepare derivation variables, 3 for position, 3 for velocity and optionally 1 for mass
335         final int nbVars = (dAccdM == null) ? 6 : 7;
336 
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()));
342 
343         // velocity corresponds three free parameters
344         final Vector3D velocity = s.getPVCoordinates().getVelocity();
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()));
348 
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());
353 
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);
362 
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         }
372 
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):
377 
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 ]
387 
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.
392 
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:
395 
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 ]
405 
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.
409 
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);
413 
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         }
425 
426         if (dAccdM != null) {
427             // set Gdot, Hdot and Idot to 0
428             Arrays.fill(pDot, 6 * stateDim, 7 * stateDim, 0.0);
429         }
430 
431         for (int k = 0; k < paramDim; ++k) {
432 
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);
441 
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 ]
453 
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.
456 
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:
459 
460             //     [ Jdot ] = [ K ]
461             //     [ Kdot ] = [ dAcc/dPos ] * [ J ] + [ dAcc/dVel ] * [ K ] + [ dAcc/dm ] * [ L ] + [ dAcc/dParam ]
462             //     [ Ldot ] = [ 0 ]
463 
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.
467 
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];
473 
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             }
484 
485             if (dAccdM != null) {
486                 // set Ldot to 0
487                 pDot[columnTop + 6 * paramDim] = 0;
488             }
489 
490         }
491 
492         // these equations have no effect of the main state itself
493         return null;
494 
495     }
496 
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) {
502 
503         if (dAccdM == null) {
504 
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);
509 
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);
514 
515         } else {
516 
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);
521 
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);
526 
527             // free parameter 6 is for mass
528             dAccdM[index]      += accelerationComponent.getPartialDerivative(0, 0, 0, 0, 0, 0, 1);
529 
530         }
531 
532     }
533 
534 }
535