SmallManeuverAnalyticalModel.java

  1. /* Copyright 2002-2024 CS GROUP
  2.  * Licensed to CS GROUP (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.hipparchus.geometry.euclidean.threed.Vector3D;
  22. import org.hipparchus.util.FastMath;
  23. import org.orekit.frames.Frame;
  24. import org.orekit.orbits.Orbit;
  25. import org.orekit.orbits.OrbitType;
  26. import org.orekit.orbits.PositionAngleType;
  27. import org.orekit.propagation.SpacecraftState;
  28. import org.orekit.propagation.analytical.AdapterPropagator;
  29. import org.orekit.time.AbsoluteDate;
  30. import org.orekit.utils.Constants;

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

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

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

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

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

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

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

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

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

  108.     /** Build a maneuver defined in user-specified frame.
  109.      * @param state0 state at maneuver date, <em>before</em> the maneuver
  110.      * is performed
  111.      * @param frame frame in which velocity increment is defined
  112.      * @param dV velocity increment in specified frame
  113.      * @param isp engine specific impulse (s)
  114.      */
  115.     public SmallManeuverAnalyticalModel(final SpacecraftState state0, final Frame frame,
  116.                                         final Vector3D dV, final double isp) {

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

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

  124.         // compute initial Jacobian
  125.         final double[][] fullJacobian = new double[6][6];
  126.         j0 = new double[6][3];
  127.         final Orbit orbit0 = type.convertType(state0.getOrbit());
  128.         orbit0.getJacobianWrtCartesian(PositionAngleType.MEAN, fullJacobian);
  129.         for (int i = 0; i < j0.length; ++i) {
  130.             System.arraycopy(fullJacobian[i], 3, j0[i], 0, 3);
  131.         }

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

  136.         // compute maneuver effect on Keplerian (or equinoctial) elements
  137.         inertialDV = frame.getStaticTransformTo(state0.getFrame(), state0.getDate())
  138.                 .transformVector(dV);

  140.         // compute mean anomaly change: dM(t1) = dM(t0) + ksi * da * (t1 - t0)
  141.         final double mu = state0.getMu();
  142.         final double a  = state0.getA();
  143.         ksi = -1.5 * FastMath.sqrt(mu / a) / (a * a);

  145.     }

  147.     /** Get the date of the maneuver.
  148.      * @return date of the maneuver
  149.      */
  150.     public AbsoluteDate getDate() {
  151.         return state0.getDate();
  152.     }

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

  162.     /** Get the inertial frame in which the velocity increment is defined.
  163.      * @return inertial frame in which the velocity increment is defined
  164.      * @see #getInertialDV()
  165.      */
  166.     public Frame getInertialFrame() {
  167.         return state0.getFrame();
  168.     }

  170.     /** Compute the effect of the maneuver on an orbit.
  171.      * @param orbit1 original orbit at t₁, without maneuver
  172.      * @return orbit at t₁, taking the maneuver
  173.      * into account if t₁ &gt; t₀
  174.      * @see #apply(SpacecraftState)
  175.      * @see #getJacobian(Orbit, PositionAngleType, double[][])
  176.      */
  177.     public Orbit apply(final Orbit orbit1) {

  179.         if (orbit1.getDate().compareTo(state0.getDate()) <= 0) {
  180.             // the maneuver has not occurred yet, don't change anything
  181.             return orbit1;
  182.         }

  184.         return orbit1.getType().convertType(updateOrbit(orbit1));

  186.     }

  188.     /** Compute the effect of the maneuver on a spacecraft state.
  189.      * @param state1 original spacecraft state at t₁,
  190.      * without maneuver
  191.      * @return spacecraft state at t₁, taking the maneuver
  192.      * into account if t₁ &gt; t₀
  193.      * @see #apply(Orbit)
  194.      * @see #getJacobian(Orbit, PositionAngleType, double[][])
  195.      */
  196.     public SpacecraftState apply(final SpacecraftState state1) {

  198.         if (state1.getDate().compareTo(state0.getDate()) <= 0) {
  199.             // the maneuver has not occurred yet, don't change anything
  200.             return state1;
  201.         }

  203.         return new SpacecraftState(state1.getOrbit().getType().convertType(updateOrbit(state1.getOrbit())),
  204.                                    state1.getAttitude(), updateMass(state1.getMass()));

  206.     }

  208.     /** Compute the effect of the maneuver on an orbit.
  209.      * @param orbit1 original orbit at t₁, without maneuver
  210.      * @return orbit at t₁, always taking the maneuver into account, always in the internal type
  211.      */
  212.     private Orbit updateOrbit(final Orbit orbit1) {

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

  225.         // convert current orbital state to Keplerian or equinoctial elements
  226.         final double[] parameters    = new double[6];
  227.         type.mapOrbitToArray(type.convertType(orbit1), PositionAngleType.MEAN, parameters, null);
  228.         for (int i = 0; i < delta.length; ++i) {
  229.             parameters[i] += delta[i];
  230.         }

  232.         // build updated orbit as Keplerian or equinoctial elements
  233.         return type.mapArrayToOrbit(parameters, null, PositionAngleType.MEAN,
  234.                                     orbit1.getDate(), orbit1.getMu(), orbit1.getFrame());

  236.     }

  238.     /** Compute the Jacobian of the orbit with respect to maneuver parameters.
  239.      * <p>
  240.      * The Jacobian matrix is a 6x4 matrix. Element jacobian[i][j] corresponds to
  241.      * the partial derivative of orbital parameter i with respect to maneuver
  242.      * parameter j. The rows order is the same order as used in {@link
  243.      * Orbit#getJacobianWrtCartesian(PositionAngleType, double[][]) Orbit.getJacobianWrtCartesian}
  244.      * method. Columns (0, 1, 2) correspond to the velocity increment coordinates
  245.      * (ΔV<sub>x</sub>, ΔV<sub>y</sub>, ΔV<sub>z</sub>) in the
  246.      * inertial frame returned by {@link #getInertialFrame()}, and column 3
  247.      * corresponds to the maneuver date t₀.
  248.      * </p>
  249.      * @param orbit1 original orbit at t₁, without maneuver
  250.      * @param positionAngleType type of the position angle to use
  251.      * @param jacobian placeholder 6x4 (or larger) matrix to be filled with the Jacobian, if matrix
  252.      * is larger than 6x4, only the 6x4 upper left corner will be modified
  253.      * @see #apply(Orbit)
  254.      */
  255.     public void getJacobian(final Orbit orbit1, final PositionAngleType positionAngleType,
  256.                             final double[][] jacobian) {

  258.         final double dt = orbit1.getDate().durationFrom(state0.getDate());
  259.         if (dt < 0) {
  260.             // the maneuver has not occurred yet, Jacobian is null
  261.             for (int i = 0; i < 6; ++i) {
  262.                 Arrays.fill(jacobian[i], 0, 4, 0.0);
  263.             }
  264.             return;
  265.         }

  267.         // derivatives of Keplerian/equinoctial elements with respect to velocity increment
  268.         final double x  = inertialDV.getX();
  269.         final double y  = inertialDV.getY();
  270.         final double z  = inertialDV.getZ();
  271.         for (int i = 0; i < 6; ++i) {
  272.             System.arraycopy(j0[i], 0, jacobian[i], 0, 3);
  273.         }
  274.         for (int j = 0; j < 3; ++j) {
  275.             jacobian[5][j] += ksi * dt * j0[0][j];
  276.         }

  278.         // derivatives of Keplerian/equinoctial elements with respect to date
  279.         evaluateJ0Dot();
  280.         for (int i = 0; i < 6; ++i) {
  281.             jacobian[i][3] = j0Dot[i][0] * x + j0Dot[i][1] * y + j0Dot[i][2] * z;
  282.         }
  283.         final double da = j0[0][0] * x + j0[0][1] * y + j0[0][2] * z;
  284.         jacobian[5][3] += ksi * (jacobian[0][3] * dt - da);

  286.         if (orbit1.getType() != type || positionAngleType != PositionAngleType.MEAN) {

  288.             // convert to derivatives of Cartesian parameters
  289.             final double[][] j2         = new double[6][6];
  290.             final double[][] pvJacobian = new double[6][4];
  291.             final Orbit updated         = updateOrbit(orbit1);
  292.             updated.getJacobianWrtParameters(PositionAngleType.MEAN, j2);
  293.             for (int i = 0; i < 6; ++i) {
  294.                 for (int j = 0; j < 4; ++j) {
  295.                     pvJacobian[i][j] = j2[i][0] * jacobian[0][j] + j2[i][1] * jacobian[1][j] +
  296.                                        j2[i][2] * jacobian[2][j] + j2[i][3] * jacobian[3][j] +
  297.                                        j2[i][4] * jacobian[4][j] + j2[i][5] * jacobian[5][j];
  298.                 }
  299.             }

  301.             // convert to derivatives of specified parameters
  302.             final double[][] j3 = new double[6][6];
  303.             orbit1.getType().convertType(updated).getJacobianWrtCartesian(positionAngleType, j3);
  304.             for (int j = 0; j < 4; ++j) {
  305.                 for (int i = 0; i < 6; ++i) {
  306.                     jacobian[i][j] = j3[i][0] * pvJacobian[0][j] + j3[i][1] * pvJacobian[1][j] +
  307.                                      j3[i][2] * pvJacobian[2][j] + j3[i][3] * pvJacobian[3][j] +
  308.                                      j3[i][4] * pvJacobian[4][j] + j3[i][5] * pvJacobian[5][j];
  309.                 }
  310.             }

  312.         }

  314.     }

  316.     /** Lazy evaluation of the initial Jacobian time derivative.
  317.      */
  318.     private void evaluateJ0Dot() {

  320.         if (j0Dot == null) {

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

  326.             // compute shifted Jacobians
  327.             final double[][] j0m1 = new double[6][6];
  328.             orbit.shiftedBy(-1 * dt).getJacobianWrtCartesian(PositionAngleType.MEAN, j0m1);
  329.             final double[][] j0p1 = new double[6][6];
  330.             orbit.shiftedBy(+1 * dt).getJacobianWrtCartesian(PositionAngleType.MEAN, j0p1);

  332.             // evaluate derivative by finite differences
  333.             for (int i = 0; i < j0Dot.length; ++i) {
  334.                 final double[] m1Row    = j0m1[i];
  335.                 final double[] p1Row    = j0p1[i];
  336.                 final double[] j0DotRow = j0Dot[i];
  337.                 for (int j = 0; j < 3; ++j) {
  338.                     j0DotRow[j] = (p1Row[j + 3] - m1Row[j + 3]) / (2 * dt);
  339.                 }
  340.             }

  342.         }

  344.     }

  346.     /** Update a spacecraft mass due to maneuver.
  347.      * @param mass masse before maneuver
  348.      * @return mass after maneuver
  349.      */
  350.     public double updateMass(final double mass) {
  351.         return massRatio * mass;
  352.     }

  354. }
Created with JaCoCo 0.8.11.202310140853