Jacobianizer.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;

  18. import java.util.Collection;
  19. import java.util.HashMap;
  20. import java.util.Map;

  21. import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
  22. import org.apache.commons.math3.geometry.euclidean.threed.FieldRotation;
  23. import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
  24. import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
  25. import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  26. import org.apache.commons.math3.util.FastMath;
  27. import org.apache.commons.math3.util.Precision;
  28. import org.orekit.attitudes.Attitude;
  29. import org.orekit.errors.OrekitException;
  30. import org.orekit.errors.OrekitMessages;
  31. import org.orekit.forces.ForceModel;
  32. import org.orekit.frames.Frame;
  33. import org.orekit.frames.Transform;
  34. import org.orekit.orbits.CartesianOrbit;
  35. import org.orekit.orbits.Orbit;
  36. import org.orekit.propagation.SpacecraftState;
  37. import org.orekit.time.AbsoluteDate;
  38. import org.orekit.utils.PVCoordinates;

  39. /** Class helping implementation of partial derivatives in {@link ForceModel force models} implementations.
  40.  * <p>
  41.  * For better performances, {@link ForceModel force models} implementations should compute their
  42.  * partial derivatives analytically. However, in some cases, it may be difficult. This class
  43.  * allows to compute the derivatives by finite differences relying only on the basic acceleration.
  44.  * </p>
  45.  * @author V&eacute;ronique Pommier-Maurussane
  46.  * @author Luc Maisonobe
  47.  */
  48. public class Jacobianizer {

  49.     /** Wrapped force model instance. */
  50.     private final ForceModel forceModel;

  51.     /** Central attraction coefficient (m³/s²). */
  52.     private final double mu;

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

  55.     /** Step used for finite difference computation with respect to parameters value. */
  56.     private final Map<String, Double> hParam;

  57.     /** Simple constructor.
  58.      * @param forceModel force model instance to wrap
  59.      * @param mu central attraction coefficient (m³/s²)
  60.      * @param paramsAndSteps collection of parameters and their associated steps
  61.      * @param hPos step used for finite difference computation with respect to spacecraft position (m)
  62.      */
  63.     public Jacobianizer(final ForceModel forceModel, final double mu,
  64.                         final Collection<ParameterConfiguration> paramsAndSteps, final double hPos) {

  65.         this.forceModel = forceModel;
  66.         this.mu         = mu;
  67.         this.hParam     = new HashMap<String, Double>();
  68.         this.hPos       = hPos;

  69.         // set up parameters for jacobian computation
  70.         for (final ParameterConfiguration param : paramsAndSteps) {
  71.             final String name = param.getParameterName();
  72.             if (forceModel.isSupported(name)) {
  73.                 double step = param.getHP();
  74.                 if (Double.isNaN(step)) {
  75.                     step = FastMath.max(1.0, FastMath.abs(forceModel.getParameter(name))) *
  76.                            FastMath.sqrt(Precision.EPSILON);
  77.                 }
  78.                 hParam.put(name, step);
  79.             }
  80.         }

  81.     }

  82.     /** Compute acceleration.
  83.      * @param retriever acceleration retriever to use for storing acceleration
  84.      * @param date current date
  85.      * @param frame inertial reference frame for state (both orbit and attitude)
  86.      * @param position position of spacecraft in reference frame
  87.      * @param velocity velocity of spacecraft in reference frame
  88.      * @param rotation orientation (attitude) of the spacecraft with respect to reference frame
  89.      * @param mass spacecraft mass
  90.      * @exception OrekitException if the underlying force models cannot compute the acceleration
  91.      */
  92.     private void computeShiftedAcceleration(final AccelerationRetriever retriever,
  93.                                             final AbsoluteDate date, final Frame frame,
  94.                                             final Vector3D position, final Vector3D velocity,
  95.                                             final Rotation rotation, final double mass)
  96.         throws OrekitException {
  97.         final Orbit shiftedORbit = new CartesianOrbit(new PVCoordinates(position, velocity), frame, date, mu);
  98.         retriever.setOrbit(shiftedORbit);
  99.         forceModel.addContribution(new SpacecraftState(shiftedORbit,
  100.                                                        new Attitude(date, frame, rotation, Vector3D.ZERO, Vector3D.ZERO),
  101.                                                        mass),
  102.                                    retriever);
  103.     }

  104.     /** Compute acceleration and derivatives with respect to state.
  105.      * @param date current date
  106.      * @param frame inertial reference frame for state (both orbit and attitude)
  107.      * @param position position of spacecraft in reference frame
  108.      * @param velocity velocity of spacecraft in reference frame
  109.      * @param rotation orientation (attitude) of the spacecraft with respect to reference frame
  110.      * @param mass spacecraft mass
  111.      * @return acceleration with derivatives
  112.      * @exception OrekitException if the underlying force models cannot compute the acceleration
  113.      */
  114.     public FieldVector3D<DerivativeStructure> accelerationDerivatives(final AbsoluteDate date, final Frame frame,
  115.                                                                       final FieldVector3D<DerivativeStructure> position,
  116.                                                                       final FieldVector3D<DerivativeStructure> velocity,
  117.                                                                       final FieldRotation<DerivativeStructure> rotation,
  118.                                                                       final DerivativeStructure mass)
  119.         throws OrekitException {

  120.         final int parameters = mass.getFreeParameters();
  121.         final int order      = mass.getOrder();

  122.         // estimate the scalar velocity step, assuming energy conservation
  123.         // and hence differentiating equation V = sqrt(mu (2/r - 1/a))
  124.         final Vector3D      p0    = position.toVector3D();
  125.         final Vector3D      v0    = velocity.toVector3D();
  126.         final double        r2    = p0.getNormSq();
  127.         final double        hVel  = mu * hPos / (v0.getNorm() * r2);

  128.         // estimate mass step, applying the same relative value as position
  129.         final double hMass = mass.getValue() * hPos / FastMath.sqrt(r2);

  130.         // compute nominal acceleration
  131.         final AccelerationRetriever nominal = new AccelerationRetriever();
  132.         computeShiftedAcceleration(nominal, date, frame, p0, v0, rotation.toRotation(), mass.getValue());
  133.         final double[] a0 = nominal.getAcceleration().toArray();

  134.         // compute accelerations with shifted states
  135.         final AccelerationRetriever shifted = new AccelerationRetriever();

  136.         // shift position by hPos alon x, y and z
  137.         computeShiftedAcceleration(shifted, date, frame, shift(position, 0, hPos), v0, shift(rotation, 0, hPos), shift(mass, 0, hPos));
  138.         final double[] derPx = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray();
  139.         computeShiftedAcceleration(shifted, date, frame, shift(position, 1, hPos), v0, shift(rotation, 1, hPos), shift(mass, 1, hPos));
  140.         final double[] derPy = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray();
  141.         computeShiftedAcceleration(shifted, date, frame, shift(position, 2, hPos), v0, shift(rotation, 2, hPos), shift(mass, 2, hPos));
  142.         final double[] derPz = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray();

  143.         // shift velocity by hVel alon x, y and z
  144.         computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 3, hVel), shift(rotation, 3, hVel), shift(mass, 3, hVel));
  145.         final double[] derVx = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray();
  146.         computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 4, hVel), shift(rotation, 4, hVel), shift(mass, 4, hVel));
  147.         final double[] derVy = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray();
  148.         computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 5, hVel), shift(rotation, 5, hVel), shift(mass, 5, hVel));
  149.         final double[] derVz = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray();

  150.         final double[] derM;
  151.         if (parameters < 7) {
  152.             derM = null;
  153.         } else {
  154.             // shift mass by hMass
  155.             computeShiftedAcceleration(shifted, date, frame, p0, v0, shift(rotation, 6, hMass), shift(mass, 6, hMass));
  156.             derM = new Vector3D(1 / hMass, shifted.getAcceleration(), -1 / hMass, nominal.getAcceleration()).toArray();

  157.         }
  158.         final double[] derivatives = new double[1 + parameters];
  159.         final DerivativeStructure[] accDer = new DerivativeStructure[3];
  160.         for (int i = 0; i < 3; ++i) {

  161.             // first element is value of acceleration
  162.             derivatives[0] = a0[i];

  163.             // next three elements are derivatives with respect to position
  164.             derivatives[1] = derPx[i];
  165.             derivatives[2] = derPy[i];
  166.             derivatives[3] = derPz[i];

  167.             // next three elements are derivatives with respect to velocity
  168.             derivatives[4] = derVx[i];
  169.             derivatives[5] = derVy[i];
  170.             derivatives[6] = derVz[i];

  171.             if (derM != null) {
  172.                 derivatives[7] = derM[i];
  173.             }

  174.             accDer[i] = new DerivativeStructure(parameters, order, derivatives);

  175.         }

  176.         return new FieldVector3D<DerivativeStructure>(accDer);


  177.     }

  178.     /** Shift a vector.
  179.      * @param nominal nominal vector
  180.      * @param index index of the variable with respect to which we shift
  181.      * @param h shift step
  182.      * @return shifted vector
  183.      */
  184.     private Vector3D shift(final FieldVector3D<DerivativeStructure> nominal, final int index, final double h) {
  185.         final double[] delta = new double[nominal.getX().getFreeParameters()];
  186.         delta[index] = h;
  187.         return new Vector3D(nominal.getX().taylor(delta),
  188.                             nominal.getY().taylor(delta),
  189.                             nominal.getZ().taylor(delta));
  190.     }

  191.     /** Shift a rotation.
  192.      * @param nominal nominal rotation
  193.      * @param index index of the variable with respect to which we shift
  194.      * @param h shift step
  195.      * @return shifted rotation
  196.      */
  197.     private Rotation shift(final FieldRotation<DerivativeStructure> nominal, final int index, final double h) {
  198.         final double[] delta = new double[nominal.getQ0().getFreeParameters()];
  199.         delta[index] = h;
  200.         return new Rotation(nominal.getQ0().taylor(delta),
  201.                             nominal.getQ1().taylor(delta),
  202.                             nominal.getQ2().taylor(delta),
  203.                             nominal.getQ3().taylor(delta),
  204.                             true);
  205.     }

  206.     /** Shift a scalar.
  207.      * @param nominal nominal scalar
  208.      * @param index index of the variable with respect to which we shift
  209.      * @param h shift step
  210.      * @return shifted scalar
  211.      */
  212.     private double shift(final DerivativeStructure nominal, final int index, final double h) {
  213.         final double[] delta = new double[nominal.getFreeParameters()];
  214.         delta[index] = h;
  215.         return nominal.taylor(delta);
  216.     }

  217.     /** Compute acceleration and derivatives with respect to parameter.
  218.      * @param s current state
  219.      * @param paramName parameter with respect to which derivation is desired
  220.      * @return acceleration with derivatives
  221.      * @exception OrekitException if the underlying force models cannot compute the acceleration
  222.      * or the parameter is not supported
  223.      */
  224.     public FieldVector3D<DerivativeStructure> accelerationDerivatives(final SpacecraftState s,
  225.                                                                       final String paramName)
  226.         throws OrekitException {

  227.         if (!hParam.containsKey(paramName)) {

  228.             // build the list of supported parameters
  229.             final StringBuilder builder = new StringBuilder();
  230.             for (final String available : hParam.keySet()) {
  231.                 if (builder.length() > 0) {
  232.                     builder.append(", ");
  233.                 }
  234.                 builder.append(available);
  235.             }

  236.             throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
  237.                                       paramName, builder.toString());

  238.         }
  239.         final double hP = hParam.get(paramName);

  240.         final AccelerationRetriever nominal = new AccelerationRetriever();
  241.         nominal.setOrbit(s.getOrbit());
  242.         forceModel.addContribution(s, nominal);
  243.         final double nx = nominal.getAcceleration().getX();
  244.         final double ny = nominal.getAcceleration().getY();
  245.         final double nz = nominal.getAcceleration().getZ();

  246.         final double paramValue = forceModel.getParameter(paramName);
  247.         forceModel.setParameter(paramName,  paramValue + hP);
  248.         final AccelerationRetriever shifted = new AccelerationRetriever();
  249.         shifted.setOrbit(s.getOrbit());
  250.         forceModel.addContribution(s, shifted);
  251.         final double sx = shifted.getAcceleration().getX();
  252.         final double sy = shifted.getAcceleration().getY();
  253.         final double sz = shifted.getAcceleration().getZ();

  254.         forceModel.setParameter(paramName,  paramValue);

  255.         return new FieldVector3D<DerivativeStructure>(new DerivativeStructure(1, 1, nx, (sx - nx) / hP),
  256.                               new DerivativeStructure(1, 1, ny, (sy - ny) / hP),
  257.                               new DerivativeStructure(1, 1, nz, (sz - nz) / hP));

  258.     }

  259.     /** Internal class for retrieving accelerations. */
  260.     private static class AccelerationRetriever implements TimeDerivativesEquations {

  261.         /** Stored acceleration. */
  262.         private Vector3D acceleration;

  263.         /** Current orbit. */
  264.         private Orbit orbit;

  265.         /** Simple constructor.
  266.          */
  267.         protected AccelerationRetriever() {
  268.             acceleration = Vector3D.ZERO;
  269.             this.orbit   = null;
  270.         }

  271.         /** Get acceleration.
  272.          * @return acceleration
  273.          */
  274.         public Vector3D getAcceleration() {
  275.             return acceleration;
  276.         }

  277.         /** Set the current orbit.
  278.          * @param orbit current orbit
  279.          */
  280.         public void setOrbit(final Orbit orbit) {
  281.             acceleration = Vector3D.ZERO;
  282.             this.orbit   = orbit;
  283.         }

  284.         /** {@inheritDoc} */
  285.         public void addKeplerContribution(final double mu) {
  286.             final Vector3D position = orbit.getPVCoordinates().getPosition();
  287.             final double r2         = position.getNormSq();
  288.             acceleration = acceleration.subtract(mu / (r2 * FastMath.sqrt(r2)), position);
  289.         }

  290.         /** {@inheritDoc} */
  291.         public void addXYZAcceleration(final double x, final double y, final double z) {
  292.             acceleration = acceleration.add(new Vector3D(x, y, z));
  293.         }

  294.         /** {@inheritDoc} */
  295.         public void addAcceleration(final Vector3D gamma, final Frame frame)
  296.             throws OrekitException {
  297.             final Transform t = frame.getTransformTo(orbit.getFrame(), orbit.getDate());
  298.             final Vector3D gammInRefFrame = t.transformVector(gamma);
  299.             addXYZAcceleration(gammInRefFrame.getX(), gammInRefFrame.getY(), gammInRefFrame.getZ());
  300.         }

  301.         /** {@inheritDoc} */
  302.         public void addMassDerivative(final double q) {
  303.             // we don't compute (yet) the mass part of the Jacobian, we just ignore this
  304.         }

  305.     }

  306. }