TriggerDate.java

/* Copyright 2002-2025 CS GROUP
 * Licensed to CS GROUP (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.forces.maneuvers.jacobians;

import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.QRDecomposition;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.orekit.forces.ForceModel;
import org.orekit.forces.maneuvers.Maneuver;
import org.orekit.forces.maneuvers.trigger.ManeuverTriggersResetter;
import org.orekit.propagation.AdditionalDataProvider;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.integration.AdditionalDerivativesProvider;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.TimeSpanMap;

/** Generator for one column of a Jacobian matrix for special case of trigger dates.
 * <p>
 * Typical use cases for this are estimation of maneuver start and stop date during
 * either orbit determination or maneuver optimization.
 * </p>
 * <p>
 * Let \((t_0, y_0)\) be the state at propagation start, \((t_1, y_1)\) be the state at
 * maneuver trigger time, \((t_t, y_t)\) be the state at any arbitrary time \(t\) during
 * propagation, and \(f_m(t, y)\) be the contribution of the maneuver to the global
 * ODE \(\frac{dy}{dt} = f(t, y)\). We are interested in the Jacobian column
 * \(\frac{\partial y_t}{\partial t_1}\).
 * </p>
 * <p>
 * There are two parts in this Jacobian: the primary part corresponds to the full contribution
 * of the jump in the dynamics due to the maneuver as it is delayed by a small amount \(dt_1\), whereas
 * the secondary part corresponds to change of acceleration after maneuver start as the mass
 * depletion is delayed and therefore the spacecraft mass is different from the mass for nominal
 * start time.
 * This second part is already contained in the first one when the mass is included in the transition matrix
 * (7x7 instead of 6x6).
 * </p>
 * <p>
 * The primary part is computed as follows. After trigger time \(t_1\) (according to propagation direction),
 * \[\frac{\partial y_t}{\partial t_1} = \pm \frac{\partial y_t}{\partial y_1} f_m(t_1, y_1)\]
 * where the sign depends on \(t_1\) being a start or stop trigger and propagation being forward
 * or backward.
 * </p>
 * <p>
 * We don't have \(\frac{\partial y_t}{\partial y_1}\) available if \(t_1 \neq t_0\), but we
 * have \(\frac{\partial y_t}{\partial y_0}\) at any time since it can be computed by integrating
 * variational equations for numerical propagation or by other closed form expressions for analytical
 * propagators. We use the classical composition rule to recover the state transition matrix with
 * respect to intermediate time \(t_1\):
 * \[\frac{\partial y_t}{\partial y_0} = \frac{\partial y_t}{\partial y_1} \frac{\partial y_1}{\partial y_0}\]
 * We deduce
 * \[\frac{\partial y_t}{\partial y_1} = \frac{\partial y_t}{\partial y_0} \left(\frac{\partial y_1}{\partial y_0}\right)^{-1}\]
 * </p>
 * <p>
 * The contribution of the primary part to the Jacobian column can therefore be computed using the following
 * closed-form expression:
 * \[\frac{\partial y_t}{\partial t_1}
 * = \pm \frac{\partial y_t}{\partial y_0} \left(\frac{\partial y_1}{\partial y_0}\right)^{-1} f_m(t_1, y_1)
 * = \frac{\partial y_t}{\partial y_0} c_1\]
 * where \(c_1\) is the signed contribution of maneuver at \(t_1\) and is computed at trigger time
 * by solving \(\frac{\partial y_1}{\partial y_0} c_1 = \pm f_m(t_1, y_1)\).
 * </p>
 * <p>
 * As the primary part of the column is generated using a closed-form expression, this generator
 * implements the {@link AdditionalDataProvider} interface and stores the column directly
 * in the primary state during propagation.
 * </p>
 * <p>
 * As the closed-form expression requires picking \(c_1\) at trigger time \(t_1\), it works only
 * if propagation starts outside of the maneuver and passes over \(t_1\) during integration.
 * </p>
 * <p>
 * The secondary part, if needed (as it is not required if the mass is already included the state transition matrix
 * i.e. when the latter is 7x7), is computed as follows. Let m be the mass and m_s its value at switching time t_s.
 * Let (x,y,z) be the position vector, (vx, vy, vz) the velocity
 * and (ax, ay, az) the total acceleration, we have \(\dot \frac{\partial x} {\partial \partial m_s} = \frac{\partial vx }{\partial m_s}))
 * and similar expressions for y and z. Furthermore, \(\dot \frac{\partial vx}{ \partial \partial m_s} = \frac{\partial ax }{\partial m}
 * . \frac{\partial m }{\partial m_s} \), and symmetric equations for vy and vy. The fact is that \( \frac{\partial m}{ \partial m_s} = 1 \)
 * assuming the mass rate q only depends on time. On the other hand, \( \frac{\partial m_s}{ \partial t_s }= q(t_s) \)/
 * By the chain rule of derivation, one gets the contribution due to the mass depletion delay.
 * </p>
 * <p>
 * The contribution of the secondary part to the Jacobian column can therefore be computed by integrating
 * the partial derivative of the acceleration, to get the partial derivative of the position.
 * </p>
 * <p>
 * As the secondary part of the column is generated using a differential equation, a separate
 * underlying generator implementing the {@link AdditionalDerivativesProvider} interface is set up to
 * perform the integration during propagation.
 * </p>
 * <p>
 * This generator takes care to sum up the primary and secondary parts so the full column of the Jacobian
 * is computed.
 * </p>
 * <p>
 * The implementation takes care to <em>not</em> resetting \(c_1\) at propagation start.
 * This allows to get proper Jacobian if we interrupt propagation in the middle of a maneuver
 * and restart propagation where it left.
 * </p>
 * @author Luc Maisonobe
 * @since 11.1
 * @see MedianDate
 * @see Duration
 */
public class TriggerDate implements ManeuverTriggersResetter, AdditionalDataProvider<double[]> {

    /** Threshold for decomposing state transition matrix at trigger time. */
    private static final double DECOMPOSITION_THRESHOLD = 1.0e-10;

    /** Name of the state for State Transition Matrix. */
    private final String stmName;

    /** Name of the parameter corresponding to the column. */
    private final String triggerName;

    /** Mass depletion effect. */
    private final MassDepletionDelay massDepletionDelay;

    /** Start/stop management flag. */
    private final boolean manageStart;

    /** Maneuver force model. */
    private final Maneuver maneuver;

    /** Event detector threshold. */
    private final double threshold;

    /** State dimension. */
    private final int stateDimension;

    /** Signed contribution of maneuver at trigger time ±(∂y₁/∂y₀)⁻¹ fₘ(t₁, y₁). */
    private TimeSpanMap<double[]> contribution;

    /** Trigger date. */
    private AbsoluteDate trigger;

    /** Indicator for forward propagation. */
    private boolean forward;

    /** Mass rate at trigger (sign depends on propagation direction). Set to zero until the maneuver has actually happened during a propagation. */
    private double signedMassRateAtTrigger = 0.;

    /** Constructor without mass as state variable in transition matrix.
     * @param stmName name of State Transition Matrix state
     * @param triggerName name of the parameter corresponding to the trigger date column
     * @param manageStart if true, we compute derivatives with respect to maneuver start
     * @param maneuver maneuver force model
     * @param threshold event detector threshold
     * @param nonGravitationalForces list of non-gravitational forces, used only if mass is not in STM
     */
    public TriggerDate(final String stmName, final String triggerName, final boolean manageStart,
                       final Maneuver maneuver, final double threshold, final ForceModel... nonGravitationalForces) {
        this(stmName, triggerName, manageStart, maneuver, threshold, false, nonGravitationalForces);
    }

    /** Constructor.
     * @param stmName name of State Transition Matrix state
     * @param triggerName name of the parameter corresponding to the trigger date column
     * @param manageStart if true, we compute derivatives with respect to maneuver start
     * @param maneuver maneuver force model
     * @param threshold event detector threshold
     * @param isMassInStm flag on mass inclusion as state variable in STM
     * @param nonGravitationalForces list of non-gravitational forces, used only if mass is not in STM
     * @since 13.1
     */
    public TriggerDate(final String stmName, final String triggerName, final boolean manageStart,
                       final Maneuver maneuver, final double threshold, final boolean isMassInStm,
                       final ForceModel... nonGravitationalForces) {
        this.stmName            = stmName;
        this.triggerName        = triggerName;
        this.massDepletionDelay = isMassInStm ? null : new MassDepletionDelay(triggerName, manageStart, maneuver, nonGravitationalForces);
        this.manageStart        = manageStart;
        this.maneuver           = maneuver;
        this.threshold          = threshold;
        this.stateDimension     = isMassInStm ? 7 : 6;
        this.contribution       = null;
        this.trigger            = null;
        this.forward            = true;
    }

    /** {@inheritDoc} */
    @Override
    public String getName() {
        return triggerName;
    }

    /** {@inheritDoc}
     * <p>
     * The column state can be computed only if the State Transition Matrix state is available.
     * </p>
     */
    @Override
    public boolean yields(final SpacecraftState state) {
        if (massDepletionDelay == null) {
            return !state.hasAdditionalData(stmName);
        } else {
            return !(state.hasAdditionalData(stmName) && state.hasAdditionalData(massDepletionDelay.getName()));
        }
    }

    /** Get the mass depletion effect processor. Can be null.
     * @return mass depletion effect processor
     */
    public MassDepletionDelay getMassDepletionDelay() {
        return massDepletionDelay;
    }

    /** {@inheritDoc} */
    @Override
    public void init(final SpacecraftState initialState, final AbsoluteDate target) {

        // note that we reset contribution or triggered ONLY at start or if we change
        // propagation direction
        // this allows to get proper Jacobian if we interrupt propagation
        // in the middle of a maneuver and restart propagation where it left
        final boolean newForward = target.isAfterOrEqualTo(initialState);
        if (contribution == null || (forward ^ newForward)) {
            contribution = new TimeSpanMap<>(null);
            trigger      = null;
            signedMassRateAtTrigger = 0.;
        }

        forward = newForward;

    }

    /** {@inheritDoc} */
    @Override
    public double[] getAdditionalData(final SpacecraftState state) {
        // we check contribution rather than triggered because this method
        // is called after maneuverTriggered and before resetState,
        // when preparing the old state to be reset
        final double[] c = contribution == null ? null : contribution.get(state.getDate());
        if (c == null) {
            // no thrust, no effect
            return new double[stateDimension];
        } else {

            // primary effect: full maneuver contribution at (delayed) trigger date
            final double[] effect = getStm(state).operate(c);

            if (massDepletionDelay != null) {
                // secondary effect: maneuver change throughout thrust as mass depletion is delayed (only needed when mass is not in the STM)
                final double[] secondary = state.getAdditionalState(massDepletionDelay.getName());

                // cumulate both effects
                for (int i = 0; i < effect.length; ++i) {
                    effect[i] += secondary[i] * signedMassRateAtTrigger;
                }
            }

            return effect;

        }
    }

    /** {@inheritDoc}*/
    @Override
    public void maneuverTriggered(final SpacecraftState state, final boolean start) {
        trigger = (start == manageStart) ? state.getDate() : null;
    }

    /** {@inheritDoc}*/
    @Override
    public SpacecraftState resetState(final SpacecraftState state) {

        if (trigger == null) {
            // this is not the maneuver trigger we expected (start vs. stop)
            return state;
        }

        // get the acceleration near trigger time
        final double[] parameters = maneuver.getParameters(state.getDate());
        final SpacecraftState stateWhenFiring = state.shiftedBy((manageStart ? 2 : -2) * threshold);
        final Vector3D        acceleration    = maneuver.acceleration(stateWhenFiring, parameters);

        // initialize derivatives computation
        final double     sign = (forward == manageStart) ? -1 : +1;
        final RealVector rhs  = MatrixUtils.createRealVector(stateDimension);
        rhs.setEntry(3, sign * acceleration.getX());
        rhs.setEntry(4, sign * acceleration.getY());
        rhs.setEntry(5, sign * acceleration.getZ());
        signedMassRateAtTrigger = sign * maneuver.getPropulsionModel().getMassDerivatives(state, parameters);
        if (stateDimension == 7) {
            rhs.setEntry(6, signedMassRateAtTrigger);
        }

        // get State Transition Matrix with respect to Cartesian parameters at trigger time
        final RealMatrix dY1dY0 = getStm(state);

        // store contribution factor for derivatives scm = ±(∂y₁/∂y₀)⁻¹ fₘ(t₁, y₁)
        final double[] c = new QRDecomposition(dY1dY0, DECOMPOSITION_THRESHOLD).getSolver().solve(rhs).toArray();
        if (forward) {
            contribution.addValidAfter(c, state.getDate(), false);
        } else {
            contribution.addValidBefore(c, state.getDate(), false);
        }

        // return unchanged state
        return state;

    }

    /** Extract State Transition Matrix with respect to Cartesian parameters.
     * @param state state containing the State Transition Matrix
     * @return State Transition Matrix
     */
    private RealMatrix getStm(final SpacecraftState state) {
        final double[] p = state.getAdditionalState(stmName);
        final RealMatrix dYdY0 = MatrixUtils.createRealMatrix(stateDimension, stateDimension);
        int index = 0;
        for (int i = 0; i < stateDimension; ++i) {
            for (int j = 0; j < stateDimension; ++j) {
                dYdY0.setEntry(i, j, p[index++]);
            }
        }
        return dYdY0;
    }

}