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.Collection;
20  import java.util.HashMap;
21  import java.util.Map;
22  
23  import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
24  import org.apache.commons.math3.geometry.euclidean.threed.FieldRotation;
25  import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
26  import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
27  import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
28  import org.apache.commons.math3.util.FastMath;
29  import org.apache.commons.math3.util.Precision;
30  import org.orekit.attitudes.Attitude;
31  import org.orekit.errors.OrekitException;
32  import org.orekit.errors.OrekitMessages;
33  import org.orekit.forces.ForceModel;
34  import org.orekit.frames.Frame;
35  import org.orekit.frames.Transform;
36  import org.orekit.orbits.CartesianOrbit;
37  import org.orekit.orbits.Orbit;
38  import org.orekit.propagation.SpacecraftState;
39  import org.orekit.time.AbsoluteDate;
40  import org.orekit.utils.PVCoordinates;
41  
42  /** Class helping implementation of partial derivatives in {@link ForceModel force models} implementations.
43   * <p>
44   * For better performances, {@link ForceModel force models} implementations should compute their
45   * partial derivatives analytically. However, in some cases, it may be difficult. This class
46   * allows to compute the derivatives by finite differences relying only on the basic acceleration.
47   * </p>
48   * @author V&eacute;ronique Pommier-Maurussane
49   * @author Luc Maisonobe
50   */
51  public class Jacobianizer {
52  
53      /** Wrapped force model instance. */
54      private final ForceModel forceModel;
55  
56      /** Central attraction coefficient (m³/s²). */
57      private final double mu;
58  
59      /** Step used for finite difference computation with respect to spacecraft position. */
60      private double hPos;
61  
62      /** Step used for finite difference computation with respect to parameters value. */
63      private final Map<String, Double> hParam;
64  
65      /** Simple constructor.
66       * @param forceModel force model instance to wrap
67       * @param mu central attraction coefficient (m³/s²)
68       * @param paramsAndSteps collection of parameters and their associated steps
69       * @param hPos step used for finite difference computation with respect to spacecraft position (m)
70       */
71      public Jacobianizer(final ForceModel forceModel, final double mu,
72                          final Collection<ParameterConfiguration> paramsAndSteps, final double hPos) {
73  
74          this.forceModel = forceModel;
75          this.mu         = mu;
76          this.hParam     = new HashMap<String, Double>();
77          this.hPos       = hPos;
78  
79          // set up parameters for jacobian computation
80          for (final ParameterConfiguration param : paramsAndSteps) {
81              final String name = param.getParameterName();
82              if (forceModel.isSupported(name)) {
83                  double step = param.getHP();
84                  if (Double.isNaN(step)) {
85                      step = FastMath.max(1.0, FastMath.abs(forceModel.getParameter(name))) *
86                             FastMath.sqrt(Precision.EPSILON);
87                  }
88                  hParam.put(name, step);
89              }
90          }
91  
92      }
93  
94      /** Compute acceleration.
95       * @param retriever acceleration retriever to use for storing acceleration
96       * @param date current date
97       * @param frame inertial reference frame for state (both orbit and attitude)
98       * @param position position of spacecraft in reference frame
99       * @param velocity velocity of spacecraft in reference frame
100      * @param rotation orientation (attitude) of the spacecraft with respect to reference frame
101      * @param mass spacecraft mass
102      * @exception OrekitException if the underlying force models cannot compute the acceleration
103      */
104     private void computeShiftedAcceleration(final AccelerationRetriever retriever,
105                                             final AbsoluteDate date, final Frame frame,
106                                             final Vector3D position, final Vector3D velocity,
107                                             final Rotation rotation, final double mass)
108         throws OrekitException {
109         final Orbit shiftedORbit = new CartesianOrbit(new PVCoordinates(position, velocity), frame, date, mu);
110         retriever.setOrbit(shiftedORbit);
111         forceModel.addContribution(new SpacecraftState(shiftedORbit,
112                                                        new Attitude(date, frame, rotation, Vector3D.ZERO, Vector3D.ZERO),
113                                                        mass),
114                                    retriever);
115     }
116 
117     /** Compute acceleration and derivatives with respect to state.
118      * @param date current date
119      * @param frame inertial reference frame for state (both orbit and attitude)
120      * @param position position of spacecraft in reference frame
121      * @param velocity velocity of spacecraft in reference frame
122      * @param rotation orientation (attitude) of the spacecraft with respect to reference frame
123      * @param mass spacecraft mass
124      * @return acceleration with derivatives
125      * @exception OrekitException if the underlying force models cannot compute the acceleration
126      */
127     public FieldVector3D<DerivativeStructure> accelerationDerivatives(final AbsoluteDate date, final Frame frame,
128                                                                       final FieldVector3D<DerivativeStructure> position,
129                                                                       final FieldVector3D<DerivativeStructure> velocity,
130                                                                       final FieldRotation<DerivativeStructure> rotation,
131                                                                       final DerivativeStructure mass)
132         throws OrekitException {
133 
134         final int parameters = mass.getFreeParameters();
135         final int order      = mass.getOrder();
136 
137         // estimate the scalar velocity step, assuming energy conservation
138         // and hence differentiating equation V = sqrt(mu (2/r - 1/a))
139         final Vector3D      p0    = position.toVector3D();
140         final Vector3D      v0    = velocity.toVector3D();
141         final double        r2    = p0.getNormSq();
142         final double        hVel  = mu * hPos / (v0.getNorm() * r2);
143 
144         // estimate mass step, applying the same relative value as position
145         final double hMass = mass.getValue() * hPos / FastMath.sqrt(r2);
146 
147         // compute nominal acceleration
148         final AccelerationRetriever nominal = new AccelerationRetriever();
149         computeShiftedAcceleration(nominal, date, frame, p0, v0, rotation.toRotation(), mass.getValue());
150         final double[] a0 = nominal.getAcceleration().toArray();
151 
152         // compute accelerations with shifted states
153         final AccelerationRetriever shifted = new AccelerationRetriever();
154 
155         // shift position by hPos alon x, y and z
156         computeShiftedAcceleration(shifted, date, frame, shift(position, 0, hPos), v0, shift(rotation, 0, hPos), shift(mass, 0, hPos));
157         final double[] derPx = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray();
158         computeShiftedAcceleration(shifted, date, frame, shift(position, 1, hPos), v0, shift(rotation, 1, hPos), shift(mass, 1, hPos));
159         final double[] derPy = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray();
160         computeShiftedAcceleration(shifted, date, frame, shift(position, 2, hPos), v0, shift(rotation, 2, hPos), shift(mass, 2, hPos));
161         final double[] derPz = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray();
162 
163         // shift velocity by hVel alon x, y and z
164         computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 3, hVel), shift(rotation, 3, hVel), shift(mass, 3, hVel));
165         final double[] derVx = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray();
166         computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 4, hVel), shift(rotation, 4, hVel), shift(mass, 4, hVel));
167         final double[] derVy = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray();
168         computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 5, hVel), shift(rotation, 5, hVel), shift(mass, 5, hVel));
169         final double[] derVz = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray();
170 
171         final double[] derM;
172         if (parameters < 7) {
173             derM = null;
174         } else {
175             // shift mass by hMass
176             computeShiftedAcceleration(shifted, date, frame, p0, v0, shift(rotation, 6, hMass), shift(mass, 6, hMass));
177             derM = new Vector3D(1 / hMass, shifted.getAcceleration(), -1 / hMass, nominal.getAcceleration()).toArray();
178 
179         }
180         final double[] derivatives = new double[1 + parameters];
181         final DerivativeStructure[] accDer = new DerivativeStructure[3];
182         for (int i = 0; i < 3; ++i) {
183 
184             // first element is value of acceleration
185             derivatives[0] = a0[i];
186 
187             // next three elements are derivatives with respect to position
188             derivatives[1] = derPx[i];
189             derivatives[2] = derPy[i];
190             derivatives[3] = derPz[i];
191 
192             // next three elements are derivatives with respect to velocity
193             derivatives[4] = derVx[i];
194             derivatives[5] = derVy[i];
195             derivatives[6] = derVz[i];
196 
197             if (derM != null) {
198                 derivatives[7] = derM[i];
199             }
200 
201             accDer[i] = new DerivativeStructure(parameters, order, derivatives);
202 
203         }
204 
205         return new FieldVector3D<DerivativeStructure>(accDer);
206 
207 
208     }
209 
210     /** Shift a vector.
211      * @param nominal nominal vector
212      * @param index index of the variable with respect to which we shift
213      * @param h shift step
214      * @return shifted vector
215      */
216     private Vector3D shift(final FieldVector3D<DerivativeStructure> nominal, final int index, final double h) {
217         final double[] delta = new double[nominal.getX().getFreeParameters()];
218         delta[index] = h;
219         return new Vector3D(nominal.getX().taylor(delta),
220                             nominal.getY().taylor(delta),
221                             nominal.getZ().taylor(delta));
222     }
223 
224     /** Shift a rotation.
225      * @param nominal nominal rotation
226      * @param index index of the variable with respect to which we shift
227      * @param h shift step
228      * @return shifted rotation
229      */
230     private Rotation shift(final FieldRotation<DerivativeStructure> nominal, final int index, final double h) {
231         final double[] delta = new double[nominal.getQ0().getFreeParameters()];
232         delta[index] = h;
233         return new Rotation(nominal.getQ0().taylor(delta),
234                             nominal.getQ1().taylor(delta),
235                             nominal.getQ2().taylor(delta),
236                             nominal.getQ3().taylor(delta),
237                             true);
238     }
239 
240     /** Shift a scalar.
241      * @param nominal nominal scalar
242      * @param index index of the variable with respect to which we shift
243      * @param h shift step
244      * @return shifted scalar
245      */
246     private double shift(final DerivativeStructure nominal, final int index, final double h) {
247         final double[] delta = new double[nominal.getFreeParameters()];
248         delta[index] = h;
249         return nominal.taylor(delta);
250     }
251 
252     /** Compute acceleration and derivatives with respect to parameter.
253      * @param s current state
254      * @param paramName parameter with respect to which derivation is desired
255      * @return acceleration with derivatives
256      * @exception OrekitException if the underlying force models cannot compute the acceleration
257      * or the parameter is not supported
258      */
259     public FieldVector3D<DerivativeStructure> accelerationDerivatives(final SpacecraftState s,
260                                                                       final String paramName)
261         throws OrekitException {
262 
263         if (!hParam.containsKey(paramName)) {
264 
265             // build the list of supported parameters
266             final StringBuilder builder = new StringBuilder();
267             for (final String available : hParam.keySet()) {
268                 if (builder.length() > 0) {
269                     builder.append(", ");
270                 }
271                 builder.append(available);
272             }
273 
274             throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
275                                       paramName, builder.toString());
276 
277         }
278         final double hP = hParam.get(paramName);
279 
280         final AccelerationRetriever nominal = new AccelerationRetriever();
281         nominal.setOrbit(s.getOrbit());
282         forceModel.addContribution(s, nominal);
283         final double nx = nominal.getAcceleration().getX();
284         final double ny = nominal.getAcceleration().getY();
285         final double nz = nominal.getAcceleration().getZ();
286 
287         final double paramValue = forceModel.getParameter(paramName);
288         forceModel.setParameter(paramName,  paramValue + hP);
289         final AccelerationRetriever shifted = new AccelerationRetriever();
290         shifted.setOrbit(s.getOrbit());
291         forceModel.addContribution(s, shifted);
292         final double sx = shifted.getAcceleration().getX();
293         final double sy = shifted.getAcceleration().getY();
294         final double sz = shifted.getAcceleration().getZ();
295 
296         forceModel.setParameter(paramName,  paramValue);
297 
298         return new FieldVector3D<DerivativeStructure>(new DerivativeStructure(1, 1, nx, (sx - nx) / hP),
299                               new DerivativeStructure(1, 1, ny, (sy - ny) / hP),
300                               new DerivativeStructure(1, 1, nz, (sz - nz) / hP));
301 
302     }
303 
304     /** Internal class for retrieving accelerations. */
305     private static class AccelerationRetriever implements TimeDerivativesEquations {
306 
307         /** Stored acceleration. */
308         private Vector3D acceleration;
309 
310         /** Current orbit. */
311         private Orbit orbit;
312 
313         /** Simple constructor.
314          */
315         protected AccelerationRetriever() {
316             acceleration = Vector3D.ZERO;
317             this.orbit   = null;
318         }
319 
320         /** Get acceleration.
321          * @return acceleration
322          */
323         public Vector3D getAcceleration() {
324             return acceleration;
325         }
326 
327         /** Set the current orbit.
328          * @param orbit current orbit
329          */
330         public void setOrbit(final Orbit orbit) {
331             acceleration = Vector3D.ZERO;
332             this.orbit   = orbit;
333         }
334 
335         /** {@inheritDoc} */
336         public void addKeplerContribution(final double mu) {
337             final Vector3D position = orbit.getPVCoordinates().getPosition();
338             final double r2         = position.getNormSq();
339             acceleration = acceleration.subtract(mu / (r2 * FastMath.sqrt(r2)), position);
340         }
341 
342         /** {@inheritDoc} */
343         public void addXYZAcceleration(final double x, final double y, final double z) {
344             acceleration = acceleration.add(new Vector3D(x, y, z));
345         }
346 
347         /** {@inheritDoc} */
348         public void addAcceleration(final Vector3D gamma, final Frame frame)
349             throws OrekitException {
350             final Transform t = frame.getTransformTo(orbit.getFrame(), orbit.getDate());
351             final Vector3D gammInRefFrame = t.transformVector(gamma);
352             addXYZAcceleration(gammInRefFrame.getX(), gammInRefFrame.getY(), gammInRefFrame.getZ());
353         }
354 
355         /** {@inheritDoc} */
356         public void addMassDerivative(final double q) {
357             // TODO: we don't compute (yet) the mass part of the Jacobian, we just ignore this
358         }
359 
360     }
361 
362 }