SmallManeuverAnalyticalModel.java

  1. /* Copyright 2002-2013 CS Systèmes d'Information
  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.forces.maneuvers;

  19. import java.util.Arrays;

  21. import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  22. import org.apache.commons.math3.util.FastMath;
  23. import org.orekit.errors.OrekitException;
  24. import org.orekit.frames.Frame;
  25. import org.orekit.orbits.Orbit;
  26. import org.orekit.orbits.OrbitType;
  27. import org.orekit.orbits.PositionAngle;
  28. import org.orekit.propagation.SpacecraftState;
  29. import org.orekit.propagation.analytical.AdapterPropagator;
  30. import org.orekit.time.AbsoluteDate;
  31. import org.orekit.utils.Constants;

  33. /** Analytical model for small maneuvers.
  34.  * <p>The aim of this model is to compute quickly the effect at date t<sub>1</sub>
  35.  * of a small maneuver performed at an earlier date t<sub>0</sub>. Both the
  36.  * direct effect of the maneuver and the Jacobian of this effect with respect to
  37.  * maneuver parameters are available.
  38.  * </p>
  39.  * <p>These effect are computed analytically using two Jacobian matrices:
  40.  * <ol>
  41.  *   <li>J<sub>0</sub>: Jacobian of Keplerian or equinoctial elements with respect
  42.  *   to cartesian parameters at date t<sub>0</sub></li> allows to compute
  43.  *   maneuver effect as a change in orbital elements at maneuver date t<sub>0</sub>,
  44.  *   <li>J<sub>1/0</sub>: Jacobian of Keplerian or equinoctial elements
  45.  *   at date t<sub>1</sub> with respect to Keplerian or equinoctial elements
  46.  *   at date t<sub>0</sub></li> allows to propagate the change in orbital elements
  47.  *   to final date t<sub>1</sub>.
  48.  * </ol>
  49.  * </p>
  50.  * <p>
  51.  * The second Jacobian, J<sub>1/0</sub>, is computed using a simple Keplerian
  52.  * model, i.e. it is the identity except for the mean motion row which also includes
  53.  * an off-diagonal element due to semi-major axis change.
  54.  * </p>
  55.  * <p>
  56.  * The orbital elements change at date t<sub>1</sub> can be added to orbital elements
  57.  * extracted from state, and the final elements taking account the changes are then
  58.  * converted back to appropriate type, which may be different from Keplerian or
  59.  * equinoctial elements.
  60.  * </p>
  61.  * <p>
  62.  * Note that this model takes <em>only</em> Keplerian effects into account. This means
  63.  * that using only this class to compute an inclination maneuver in Low Earth Orbit will
  64.  * <em>not</em> change ascending node drift rate despite inclination has changed (the
  65.  * same would be true for a semi-major axis change of course). In order to take this
  66.  * drift into account, an instance of {@link
  67.  * org.orekit.propagation.analytical.J2DifferentialEffect J2DifferentialEffect}
  68.  * must be used together with an instance of this class.
  69.  * </p>
  70.  * @author Luc Maisonobe
  71.  */
  72. public class SmallManeuverAnalyticalModel
  73.     implements AdapterPropagator.DifferentialEffect {

  75.     /** State at maneuver date (before maneuver occurrence). */
  76.     private final SpacecraftState state0;

  78.     /** Inertial velocity increment. */
  79.     private final Vector3D inertialDV;

  81.     /** Mass change ratio. */
  82.     private final double massRatio;

  84.     /** Type of orbit used for internal Jacobians. */
  85.     private final OrbitType type;

  87.     /** Initial Keplerian (or equinoctial) Jacobian with respect to maneuver. */
  88.     private final double[][] j0;

  90.     /** Time derivative of the initial Keplerian (or equinoctial) Jacobian with respect to maneuver. */
  91.     private double[][] j0Dot;

  93.     /** Mean anomaly change factor. */
  94.     private final double ksi;

  96.     /** Build a maneuver defined in spacecraft frame.
  97.      * @param state0 state at maneuver date, <em>before</em> the maneuver
  98.      * is performed
  99.      * @param dV velocity increment in spacecraft frame
  100.      * @param isp engine specific impulse (s)
  101.      * @exception OrekitException if spacecraft frame cannot be transformed
  102.      */
  103.     public SmallManeuverAnalyticalModel(final SpacecraftState state0,
  104.                                         final Vector3D dV, final double isp)
  105.         throws OrekitException {
  106.         this(state0, state0.getFrame(),
  107.              state0.getAttitude().getRotation().applyInverseTo(dV),
  108.              isp);
  109.     }

  111.     /** Build a maneuver defined in user-specified frame.
  112.      * @param state0 state at maneuver date, <em>before</em> the maneuver
  113.      * is performed
  114.      * @param frame frame in which velocity increment is defined
  115.      * @param dV velocity increment in specified frame
  116.      * @param isp engine specific impulse (s)
  117.      * @exception OrekitException if velocity increment frame cannot be transformed
  118.      */
  119.     public SmallManeuverAnalyticalModel(final SpacecraftState state0, final Frame frame,
  120.                                         final Vector3D dV, final double isp)
  121.         throws OrekitException {

  123.         this.state0    = state0;
  124.         this.massRatio = FastMath.exp(-dV.getNorm() / (Constants.G0_STANDARD_GRAVITY * isp));

  126.         // use equinoctial orbit type if possible, Keplerian if nearly hyperbolic orbits
  127.         type = (state0.getE() < 0.9) ? OrbitType.EQUINOCTIAL : OrbitType.KEPLERIAN;

  129.         // compute initial Jacobian
  130.         final double[][] fullJacobian = new double[6][6];
  131.         j0 = new double[6][3];
  132.         final Orbit orbit0 = type.convertType(state0.getOrbit());
  133.         orbit0.getJacobianWrtCartesian(PositionAngle.MEAN, fullJacobian);
  134.         for (int i = 0; i < j0.length; ++i) {
  135.             System.arraycopy(fullJacobian[i], 3, j0[i], 0, 3);
  136.         }

  138.         // use lazy evaluation for j0Dot, as it is used only when Jacobians are evaluated
  139.         j0Dot = null;

  141.         // compute maneuver effect on Keplerian (or equinoctial) elements
  142.         inertialDV = frame.getTransformTo(state0.getFrame(), state0.getDate()).transformVector(dV);

  144.         // compute mean anomaly change: dM(t1) = dM(t0) + ksi * da * (t1 - t0)
  145.         final double mu = state0.getMu();
  146.         final double a  = state0.getA();
  147.         ksi = -1.5 * FastMath.sqrt(mu / a) / (a * a);

  149.     }

  151.     /** Get the date of the maneuver.
  152.      * @return date of the maneuver
  153.      */
  154.     public AbsoluteDate getDate() {
  155.         return state0.getDate();
  156.     }

  158.     /** Get the inertial velocity increment of the maneuver.
  159.      * @return velocity increment in a state-dependent inertial frame
  160.      * @see #getInertialFrame()
  161.      */
  162.     public Vector3D getInertialDV() {
  163.         return inertialDV;
  164.     }

  166.     /** Get the inertial frame in which the velocity increment is defined.
  167.      * @return inertial frame in which the velocity increment is defined
  168.      * @see #getInertialDV()
  169.      */
  170.     public Frame getInertialFrame() {
  171.         return state0.getFrame();
  172.     }

  174.     /** Compute the effect of the maneuver on an orbit.
  175.      * @param orbit1 original orbit at t<sub>1</sub>, without maneuver
  176.      * @return orbit at t<sub>1</sub>, taking the maneuver
  177.      * into account if t<sub>1</sub> &gt; t<sub>0</sub>
  178.      * @see #apply(SpacecraftState)
  179.      * @see #getJacobian(Orbit, PositionAngle, double[][])
  180.      */
  181.     public Orbit apply(final Orbit orbit1) {

  183.         if (orbit1.getDate().compareTo(state0.getDate()) <= 0) {
  184.             // the maneuver has not occurred yet, don't change anything
  185.             return orbit1;
  186.         }

  188.         return updateOrbit(orbit1);

  190.     }

  192.     /** Compute the effect of the maneuver on a spacecraft state.
  193.      * @param state1 original spacecraft state at t<sub>1</sub>,
  194.      * without maneuver
  195.      * @return spacecraft state at t<sub>1</sub>, taking the maneuver
  196.      * into account if t<sub>1</sub> &gt; t<sub>0</sub>
  197.      * @see #apply(Orbit)
  198.      * @see #getJacobian(Orbit, PositionAngle, double[][])
  199.      */
  200.     public SpacecraftState apply(final SpacecraftState state1) {

  202.         if (state1.getDate().compareTo(state0.getDate()) <= 0) {
  203.             // the maneuver has not occurred yet, don't change anything
  204.             return state1;
  205.         }

  207.         return new SpacecraftState(updateOrbit(state1.getOrbit()),
  208.                                    state1.getAttitude(), updateMass(state1.getMass()));

  210.     }

  212.     /** Compute the effect of the maneuver on an orbit.
  213.      * @param orbit1 original orbit at t<sub>1</sub>, without maneuver
  214.      * @return orbit at t<sub>1</sub>, always taking the maneuver into account
  215.      */
  216.     private Orbit updateOrbit(final Orbit orbit1) {

  218.         // compute maneuver effect
  219.         final double dt = orbit1.getDate().durationFrom(state0.getDate());
  220.         final double x  = inertialDV.getX();
  221.         final double y  = inertialDV.getY();
  222.         final double z  = inertialDV.getZ();
  223.         final double[] delta = new double[6];
  224.         for (int i = 0; i < delta.length; ++i) {
  225.             delta[i] = j0[i][0] * x + j0[i][1] * y + j0[i][2] * z;
  226.         }
  227.         delta[5] += ksi * delta[0] * dt;

  229.         // convert current orbital state to Keplerian or equinoctial elements
  230.         final double[] parameters = new double[6];
  231.         type.mapOrbitToArray(type.convertType(orbit1), PositionAngle.MEAN, parameters);
  232.         for (int i = 0; i < delta.length; ++i) {
  233.             parameters[i] += delta[i];
  234.         }

  236.         // build updated orbit as Keplerian or equinoctial elements
  237.         final Orbit o = type.mapArrayToOrbit(parameters, PositionAngle.MEAN,
  238.                                              orbit1.getDate(), orbit1.getMu(),
  239.                                              orbit1.getFrame());

  241.         // convert to required type
  242.         return orbit1.getType().convertType(o);

  244.     }

  246.     /** Compute the Jacobian of the orbit with respect to maneuver parameters.
  247.      * <p>
  248.      * The Jacobian matrix is a 6x4 matrix. Element jacobian[i][j] corresponds to
  249.      * the partial derivative of orbital parameter i with respect to maneuver
  250.      * parameter j. The rows order is the same order as used in {@link
  251.      * Orbit#getJacobianWrtCartesian(PositionAngle, double[][]) Orbit.getJacobianWrtCartesian}
  252.      * method. Columns (0, 1, 2) correspond to the velocity increment coordinates
  253.      * (&Delta;V<sub>x</sub>, &Delta;V<sub>y</sub>, &Delta;V<sub>z</sub>) in the
  254.      * inertial frame returned by {@link #getInertialFrame()}, and column 3
  255.      * corresponds to the maneuver date t<sub>0</sub>.
  256.      * </p>
  257.      * @param orbit1 original orbit at t<sub>1</sub>, without maneuver
  258.      * @param positionAngle type of the position angle to use
  259.      * @param jacobian placeholder 6x4 (or larger) matrix to be filled with the Jacobian, if matrix
  260.      * is larger than 6x4, only the 6x4 upper left corner will be modified
  261.      * @see #apply(Orbit)
  262.      * @exception OrekitException if time derivative of the initial Jacobian cannot be computed
  263.      */
  264.     public void getJacobian(final Orbit orbit1, final PositionAngle positionAngle,
  265.                             final double[][] jacobian)
  266.         throws OrekitException {

  268.         final double dt = orbit1.getDate().durationFrom(state0.getDate());
  269.         if (dt < 0) {
  270.             // the maneuver has not occurred yet, Jacobian is null
  271.             for (int i = 0; i < 6; ++i) {
  272.                 Arrays.fill(jacobian[i], 0, 4, 0.0);
  273.             }
  274.             return;
  275.         }

  277.         // derivatives of Keplerian/equinoctial elements with respect to velocity increment
  278.         final double x  = inertialDV.getX();
  279.         final double y  = inertialDV.getY();
  280.         final double z  = inertialDV.getZ();
  281.         for (int i = 0; i < 6; ++i) {
  282.             System.arraycopy(j0[i], 0, jacobian[i], 0, 3);
  283.         }
  284.         for (int j = 0; j < 3; ++j) {
  285.             jacobian[5][j] += ksi * dt * j0[0][j];
  286.         }

  288.         // derivatives of Keplerian/equinoctial elements with respect to date
  289.         evaluateJ0Dot();
  290.         for (int i = 0; i < 6; ++i) {
  291.             jacobian[i][3] = j0Dot[i][0] * x + j0Dot[i][1] * y + j0Dot[i][2] * z;
  292.         }
  293.         final double da = j0[0][0] * x + j0[0][1] * y + j0[0][2] * z;
  294.         jacobian[5][3] += ksi * (jacobian[0][3] * dt - da);

  296.         if (orbit1.getType() != type || positionAngle != PositionAngle.MEAN) {

  298.             // convert to derivatives of cartesian parameters
  299.             final double[][] j2         = new double[6][6];
  300.             final double[][] pvJacobian = new double[6][4];
  301.             final Orbit updated         = updateOrbit(orbit1);
  302.             type.convertType(updated).getJacobianWrtParameters(PositionAngle.MEAN, j2);
  303.             for (int i = 0; i < 6; ++i) {
  304.                 for (int j = 0; j < 4; ++j) {
  305.                     pvJacobian[i][j] = j2[i][0] * jacobian[0][j] + j2[i][1] * jacobian[1][j] +
  306.                                        j2[i][2] * jacobian[2][j] + j2[i][3] * jacobian[3][j] +
  307.                                        j2[i][4] * jacobian[4][j] + j2[i][5] * jacobian[5][j];
  308.                 }
  309.             }

  311.             // convert to derivatives of specified parameters
  312.             final double[][] j3 = new double[6][6];
  313.             updated.getJacobianWrtCartesian(positionAngle, j3);
  314.             for (int j = 0; j < 4; ++j) {
  315.                 for (int i = 0; i < 6; ++i) {
  316.                     jacobian[i][j] = j3[i][0] * pvJacobian[0][j] + j3[i][1] * pvJacobian[1][j] +
  317.                                      j3[i][2] * pvJacobian[2][j] + j3[i][3] * pvJacobian[3][j] +
  318.                                      j3[i][4] * pvJacobian[4][j] + j3[i][5] * pvJacobian[5][j];
  319.                 }
  320.             }

  322.         }

  324.     }

  326.     /** Lazy evaluation of the initial Jacobian time derivative.
  327.      * @exception OrekitException if initial orbit cannot be shifted
  328.      */
  329.     private void evaluateJ0Dot() throws OrekitException {

  331.         if (j0Dot == null) {

  333.             j0Dot = new double[6][3];
  334.             final double dt = 1.0e-5 / state0.getKeplerianMeanMotion();
  335.             final Orbit orbit = type.convertType(state0.getOrbit());

  337.             // compute shifted Jacobians
  338.             final double[][] j0m1 = new double[6][6];
  339.             orbit.shiftedBy(-1 * dt).getJacobianWrtCartesian(PositionAngle.MEAN, j0m1);
  340.             final double[][] j0p1 = new double[6][6];
  341.             orbit.shiftedBy(+1 * dt).getJacobianWrtCartesian(PositionAngle.MEAN, j0p1);

  343.             // evaluate derivative by finite differences
  344.             for (int i = 0; i < j0Dot.length; ++i) {
  345.                 final double[] m1Row    = j0m1[i];
  346.                 final double[] p1Row    = j0p1[i];
  347.                 final double[] j0DotRow = j0Dot[i];
  348.                 for (int j = 0; j < 3; ++j) {
  349.                     j0DotRow[j] = (p1Row[j + 3] - m1Row[j + 3]) / (2 * dt);
  350.                 }
  351.             }

  353.         }

  355.     }

  357.     /** Update a spacecraft mass due to maneuver.
  358.      * @param mass masse before maneuver
  359.      * @return mass after maneuver
  360.      */
  361.     public double updateMass(final double mass) {
  362.         return massRatio * mass;
  363.     }

  365. }
Created with JaCoCo 0.6.3.201306030806