Jacobianizer.java
- /* Copyright 2010-2011 Centre National d'Études Spatiales
- * Licensed to CS Systèmes d'Information (CS) under one or more
- * contributor license agreements. See the NOTICE file distributed with
- * this work for additional information regarding copyright ownership.
- * CS licenses this file to You under the Apache License, Version 2.0
- * (the "License"); you may not use this file except in compliance with
- * the License. You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
- package org.orekit.propagation.numerical;
- import java.util.Collection;
- import java.util.HashMap;
- import java.util.Map;
- import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
- import org.apache.commons.math3.geometry.euclidean.threed.FieldRotation;
- import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
- import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
- import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
- import org.apache.commons.math3.util.FastMath;
- import org.apache.commons.math3.util.Precision;
- import org.orekit.attitudes.Attitude;
- import org.orekit.errors.OrekitException;
- import org.orekit.errors.OrekitMessages;
- import org.orekit.forces.ForceModel;
- import org.orekit.frames.Frame;
- import org.orekit.frames.Transform;
- import org.orekit.orbits.CartesianOrbit;
- import org.orekit.orbits.Orbit;
- import org.orekit.propagation.SpacecraftState;
- import org.orekit.time.AbsoluteDate;
- import org.orekit.utils.PVCoordinates;
- /** Class helping implementation of partial derivatives in {@link ForceModel force models} implementations.
- * <p>
- * For better performances, {@link ForceModel force models} implementations should compute their
- * partial derivatives analytically. However, in some cases, it may be difficult. This class
- * allows to compute the derivatives by finite differences relying only on the basic acceleration.
- * </p>
- * @author Véronique Pommier-Maurussane
- * @author Luc Maisonobe
- */
- public class Jacobianizer {
- /** Wrapped force model instance. */
- private final ForceModel forceModel;
- /** Central attraction coefficient (m³/s²). */
- private final double mu;
- /** Step used for finite difference computation with respect to spacecraft position. */
- private double hPos;
- /** Step used for finite difference computation with respect to parameters value. */
- private final Map<String, Double> hParam;
- /** Simple constructor.
- * @param forceModel force model instance to wrap
- * @param mu central attraction coefficient (m³/s²)
- * @param paramsAndSteps collection of parameters and their associated steps
- * @param hPos step used for finite difference computation with respect to spacecraft position (m)
- */
- public Jacobianizer(final ForceModel forceModel, final double mu,
- final Collection<ParameterConfiguration> paramsAndSteps, final double hPos) {
- this.forceModel = forceModel;
- this.mu = mu;
- this.hParam = new HashMap<String, Double>();
- this.hPos = hPos;
- // set up parameters for jacobian computation
- for (final ParameterConfiguration param : paramsAndSteps) {
- final String name = param.getParameterName();
- if (forceModel.isSupported(name)) {
- double step = param.getHP();
- if (Double.isNaN(step)) {
- step = FastMath.max(1.0, FastMath.abs(forceModel.getParameter(name))) *
- FastMath.sqrt(Precision.EPSILON);
- }
- hParam.put(name, step);
- }
- }
- }
- /** Compute acceleration.
- * @param retriever acceleration retriever to use for storing acceleration
- * @param date current date
- * @param frame inertial reference frame for state (both orbit and attitude)
- * @param position position of spacecraft in reference frame
- * @param velocity velocity of spacecraft in reference frame
- * @param rotation orientation (attitude) of the spacecraft with respect to reference frame
- * @param mass spacecraft mass
- * @exception OrekitException if the underlying force models cannot compute the acceleration
- */
- private void computeShiftedAcceleration(final AccelerationRetriever retriever,
- final AbsoluteDate date, final Frame frame,
- final Vector3D position, final Vector3D velocity,
- final Rotation rotation, final double mass)
- throws OrekitException {
- final Orbit shiftedORbit = new CartesianOrbit(new PVCoordinates(position, velocity), frame, date, mu);
- retriever.setOrbit(shiftedORbit);
- forceModel.addContribution(new SpacecraftState(shiftedORbit,
- new Attitude(date, frame, rotation, Vector3D.ZERO, Vector3D.ZERO),
- mass),
- retriever);
- }
- /** Compute acceleration and derivatives with respect to state.
- * @param date current date
- * @param frame inertial reference frame for state (both orbit and attitude)
- * @param position position of spacecraft in reference frame
- * @param velocity velocity of spacecraft in reference frame
- * @param rotation orientation (attitude) of the spacecraft with respect to reference frame
- * @param mass spacecraft mass
- * @return acceleration with derivatives
- * @exception OrekitException if the underlying force models cannot compute the acceleration
- */
- public FieldVector3D<DerivativeStructure> accelerationDerivatives(final AbsoluteDate date, final Frame frame,
- final FieldVector3D<DerivativeStructure> position,
- final FieldVector3D<DerivativeStructure> velocity,
- final FieldRotation<DerivativeStructure> rotation,
- final DerivativeStructure mass)
- throws OrekitException {
- final int parameters = mass.getFreeParameters();
- final int order = mass.getOrder();
- // estimate the scalar velocity step, assuming energy conservation
- // and hence differentiating equation V = sqrt(mu (2/r - 1/a))
- final Vector3D p0 = position.toVector3D();
- final Vector3D v0 = velocity.toVector3D();
- final double r2 = p0.getNormSq();
- final double hVel = mu * hPos / (v0.getNorm() * r2);
- // estimate mass step, applying the same relative value as position
- final double hMass = mass.getValue() * hPos / FastMath.sqrt(r2);
- // compute nominal acceleration
- final AccelerationRetriever nominal = new AccelerationRetriever();
- computeShiftedAcceleration(nominal, date, frame, p0, v0, rotation.toRotation(), mass.getValue());
- final double[] a0 = nominal.getAcceleration().toArray();
- // compute accelerations with shifted states
- final AccelerationRetriever shifted = new AccelerationRetriever();
- // shift position by hPos alon x, y and z
- computeShiftedAcceleration(shifted, date, frame, shift(position, 0, hPos), v0, shift(rotation, 0, hPos), shift(mass, 0, hPos));
- final double[] derPx = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray();
- computeShiftedAcceleration(shifted, date, frame, shift(position, 1, hPos), v0, shift(rotation, 1, hPos), shift(mass, 1, hPos));
- final double[] derPy = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray();
- computeShiftedAcceleration(shifted, date, frame, shift(position, 2, hPos), v0, shift(rotation, 2, hPos), shift(mass, 2, hPos));
- final double[] derPz = new Vector3D(1 / hPos, shifted.getAcceleration(), -1 / hPos, nominal.getAcceleration()).toArray();
- // shift velocity by hVel alon x, y and z
- computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 3, hVel), shift(rotation, 3, hVel), shift(mass, 3, hVel));
- final double[] derVx = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray();
- computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 4, hVel), shift(rotation, 4, hVel), shift(mass, 4, hVel));
- final double[] derVy = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray();
- computeShiftedAcceleration(shifted, date, frame, p0, shift(velocity, 5, hVel), shift(rotation, 5, hVel), shift(mass, 5, hVel));
- final double[] derVz = new Vector3D(1 / hVel, shifted.getAcceleration(), -1 / hVel, nominal.getAcceleration()).toArray();
- final double[] derM;
- if (parameters < 7) {
- derM = null;
- } else {
- // shift mass by hMass
- computeShiftedAcceleration(shifted, date, frame, p0, v0, shift(rotation, 6, hMass), shift(mass, 6, hMass));
- derM = new Vector3D(1 / hMass, shifted.getAcceleration(), -1 / hMass, nominal.getAcceleration()).toArray();
- }
- final double[] derivatives = new double[1 + parameters];
- final DerivativeStructure[] accDer = new DerivativeStructure[3];
- for (int i = 0; i < 3; ++i) {
- // first element is value of acceleration
- derivatives[0] = a0[i];
- // next three elements are derivatives with respect to position
- derivatives[1] = derPx[i];
- derivatives[2] = derPy[i];
- derivatives[3] = derPz[i];
- // next three elements are derivatives with respect to velocity
- derivatives[4] = derVx[i];
- derivatives[5] = derVy[i];
- derivatives[6] = derVz[i];
- if (derM != null) {
- derivatives[7] = derM[i];
- }
- accDer[i] = new DerivativeStructure(parameters, order, derivatives);
- }
- return new FieldVector3D<DerivativeStructure>(accDer);
- }
- /** Shift a vector.
- * @param nominal nominal vector
- * @param index index of the variable with respect to which we shift
- * @param h shift step
- * @return shifted vector
- */
- private Vector3D shift(final FieldVector3D<DerivativeStructure> nominal, final int index, final double h) {
- final double[] delta = new double[nominal.getX().getFreeParameters()];
- delta[index] = h;
- return new Vector3D(nominal.getX().taylor(delta),
- nominal.getY().taylor(delta),
- nominal.getZ().taylor(delta));
- }
- /** Shift a rotation.
- * @param nominal nominal rotation
- * @param index index of the variable with respect to which we shift
- * @param h shift step
- * @return shifted rotation
- */
- private Rotation shift(final FieldRotation<DerivativeStructure> nominal, final int index, final double h) {
- final double[] delta = new double[nominal.getQ0().getFreeParameters()];
- delta[index] = h;
- return new Rotation(nominal.getQ0().taylor(delta),
- nominal.getQ1().taylor(delta),
- nominal.getQ2().taylor(delta),
- nominal.getQ3().taylor(delta),
- true);
- }
- /** Shift a scalar.
- * @param nominal nominal scalar
- * @param index index of the variable with respect to which we shift
- * @param h shift step
- * @return shifted scalar
- */
- private double shift(final DerivativeStructure nominal, final int index, final double h) {
- final double[] delta = new double[nominal.getFreeParameters()];
- delta[index] = h;
- return nominal.taylor(delta);
- }
- /** Compute acceleration and derivatives with respect to parameter.
- * @param s current state
- * @param paramName parameter with respect to which derivation is desired
- * @return acceleration with derivatives
- * @exception OrekitException if the underlying force models cannot compute the acceleration
- * or the parameter is not supported
- */
- public FieldVector3D<DerivativeStructure> accelerationDerivatives(final SpacecraftState s,
- final String paramName)
- throws OrekitException {
- if (!hParam.containsKey(paramName)) {
- // build the list of supported parameters
- final StringBuilder builder = new StringBuilder();
- for (final String available : hParam.keySet()) {
- if (builder.length() > 0) {
- builder.append(", ");
- }
- builder.append(available);
- }
- throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
- paramName, builder.toString());
- }
- final double hP = hParam.get(paramName);
- final AccelerationRetriever nominal = new AccelerationRetriever();
- nominal.setOrbit(s.getOrbit());
- forceModel.addContribution(s, nominal);
- final double nx = nominal.getAcceleration().getX();
- final double ny = nominal.getAcceleration().getY();
- final double nz = nominal.getAcceleration().getZ();
- final double paramValue = forceModel.getParameter(paramName);
- forceModel.setParameter(paramName, paramValue + hP);
- final AccelerationRetriever shifted = new AccelerationRetriever();
- shifted.setOrbit(s.getOrbit());
- forceModel.addContribution(s, shifted);
- final double sx = shifted.getAcceleration().getX();
- final double sy = shifted.getAcceleration().getY();
- final double sz = shifted.getAcceleration().getZ();
- forceModel.setParameter(paramName, paramValue);
- return new FieldVector3D<DerivativeStructure>(new DerivativeStructure(1, 1, nx, (sx - nx) / hP),
- new DerivativeStructure(1, 1, ny, (sy - ny) / hP),
- new DerivativeStructure(1, 1, nz, (sz - nz) / hP));
- }
- /** Internal class for retrieving accelerations. */
- private static class AccelerationRetriever implements TimeDerivativesEquations {
- /** Stored acceleration. */
- private Vector3D acceleration;
- /** Current orbit. */
- private Orbit orbit;
- /** Simple constructor.
- */
- protected AccelerationRetriever() {
- acceleration = Vector3D.ZERO;
- this.orbit = null;
- }
- /** Get acceleration.
- * @return acceleration
- */
- public Vector3D getAcceleration() {
- return acceleration;
- }
- /** Set the current orbit.
- * @param orbit current orbit
- */
- public void setOrbit(final Orbit orbit) {
- acceleration = Vector3D.ZERO;
- this.orbit = orbit;
- }
- /** {@inheritDoc} */
- public void addKeplerContribution(final double mu) {
- final Vector3D position = orbit.getPVCoordinates().getPosition();
- final double r2 = position.getNormSq();
- acceleration = acceleration.subtract(mu / (r2 * FastMath.sqrt(r2)), position);
- }
- /** {@inheritDoc} */
- public void addXYZAcceleration(final double x, final double y, final double z) {
- acceleration = acceleration.add(new Vector3D(x, y, z));
- }
- /** {@inheritDoc} */
- public void addAcceleration(final Vector3D gamma, final Frame frame)
- throws OrekitException {
- final Transform t = frame.getTransformTo(orbit.getFrame(), orbit.getDate());
- final Vector3D gammInRefFrame = t.transformVector(gamma);
- addXYZAcceleration(gammInRefFrame.getX(), gammInRefFrame.getY(), gammInRefFrame.getZ());
- }
- /** {@inheritDoc} */
- public void addMassDerivative(final double q) {
- // we don't compute (yet) the mass part of the Jacobian, we just ignore this
- }
- }
- }