1 /* Copyright 2002-2025 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.utils;
18
19 import org.hipparchus.analysis.UnivariateFunction;
20 import org.hipparchus.analysis.UnivariateVectorFunction;
21 import org.hipparchus.analysis.differentiation.DSFactory;
22 import org.hipparchus.analysis.differentiation.DerivativeStructure;
23 import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
24 import org.hipparchus.analysis.differentiation.UnivariateDifferentiableVectorFunction;
25 import org.orekit.attitudes.AttitudeProvider;
26 import org.orekit.orbits.Orbit;
27 import org.orekit.orbits.OrbitType;
28 import org.orekit.orbits.PositionAngleType;
29 import org.orekit.propagation.SpacecraftState;
30 import org.orekit.propagation.ToleranceProvider;
31 import org.orekit.time.AbsoluteDate;
32
33 /** Utility class for differentiating various kinds of functions.
34 * @author Luc Maisonobe
35 * @since 8.0
36 */
37 public class Differentiation {
38
39 /** Factory for the DerivativeStructure instances. */
40 private static final DSFactory FACTORY = new DSFactory(1, 1);
41
42 /** Private constructor for utility class.
43 */
44 private Differentiation() {
45 }
46
47 /** Differentiate a scalar function using finite differences.
48 * @param function function to differentiate
49 * @param nbPoints number of points used for finite differences
50 * @param step step for finite differences, in <em>physical</em> units
51 * @return scalar function evaluating to the derivative of the original function
52 * @since 9.3
53 */
54 public static ParameterFunction differentiate(final ParameterFunction function,
55 final int nbPoints, final double step) {
56
57 return new ParameterFunction() {
58
59 /** Finite differences differentiator to use. */
60 private final FiniteDifferencesDifferentiator differentiator =
61 new FiniteDifferencesDifferentiator(nbPoints, step);
62
63 /** {@inheritDoc} */
64 @Override
65 public double value(final ParameterDriver driver, final AbsoluteDate date) {
66
67 final UnivariateFunction uf = new UnivariateFunction() {
68 /** {@inheritDoc} */
69 @Override
70 public double value(final double value) {
71 final double saved = driver.getValue(date);
72 driver.setValue(value, date);
73 final double functionValue = function.value(driver, date);
74 driver.setValue(saved, date);
75 return functionValue;
76 }
77 };
78
79 final DerivativeStructure dsParam = FACTORY.variable(0, driver.getValue(date));
80 final DerivativeStructure dsValue = differentiator.differentiate(uf).value(dsParam);
81 return dsValue.getPartialDerivative(1);
82
83 }
84 };
85
86 }
87
88 /** Differentiate a vector function using finite differences.
89 * @param function function to differentiate
90 * @param dimension dimension of the vector value of the function
91 * @param provider attitude provider to use for modified states
92 * @param orbitType type used to map the orbit to a one dimensional array
93 * @param positionAngleType type of the position angle used for orbit mapping to array
94 * @param dP user specified position error, used for step size computation for finite differences
95 * @param nbPoints number of points used for finite differences
96 * @return matrix function evaluating to the Jacobian of the original function
97 */
98 public static StateJacobian differentiate(final StateFunction function, final int dimension,
99 final AttitudeProvider provider,
100 final OrbitType orbitType, final PositionAngleType positionAngleType,
101 final double dP, final int nbPoints) {
102 return state -> {
103 final double[] tolerances =
104 ToleranceProvider.getDefaultToleranceProvider(dP).getTolerances(state.getOrbit(), orbitType)[0];
105 final double[][] jacobian = new double[dimension][6];
106 for (int j = 0; j < 6; ++j) {
107
108 // compute partial derivatives with respect to state component j
109 final UnivariateVectorFunction componentJ =
110 new StateComponentFunction(j, function, provider, state,
111 orbitType, positionAngleType);
112 final FiniteDifferencesDifferentiator differentiator =
113 new FiniteDifferencesDifferentiator(nbPoints, tolerances[j]);
114 final UnivariateDifferentiableVectorFunction differentiatedJ =
115 differentiator.differentiate(componentJ);
116
117 final DerivativeStructure[] c = differentiatedJ.value(FACTORY.variable(0, 0.0));
118
119 // populate the j-th column of the Jacobian
120 for (int i = 0; i < dimension; ++i) {
121 jacobian[i][j] = c[i].getPartialDerivative(1);
122 }
123
124 }
125
126 return jacobian;
127
128 };
129 }
130
131 /** Restriction of a {@link StateFunction} to a function of a single state component.
132 */
133 private static class StateComponentFunction implements UnivariateVectorFunction {
134
135 /** Component index in the mapped orbit array. */
136 private final int index;
137
138 /** State-dependent function. */
139 private final StateFunction f;
140
141 /** Type used to map the orbit to a one dimensional array. */
142 private final OrbitType orbitType;
143
144 /** Tpe of the position angle used for orbit mapping to array. */
145 private final PositionAngleType positionAngleType;
146
147 /** Base state, of which only one component will change. */
148 private final SpacecraftState baseState;
149
150 /** Attitude provider to use for modified states. */
151 private final AttitudeProvider provider;
152
153 /** Simple constructor.
154 * @param index component index in the mapped orbit array
155 * @param f state-dependent function
156 * @param provider attitude provider to use for modified states
157 * @param baseState base state, of which only one component will change
158 * @param orbitType type used to map the orbit to a one dimensional array
159 * @param positionAngleType type of the position angle used for orbit mapping to array
160 */
161 StateComponentFunction(final int index, final StateFunction f,
162 final AttitudeProvider provider, final SpacecraftState baseState,
163 final OrbitType orbitType, final PositionAngleType positionAngleType) {
164 this.index = index;
165 this.f = f;
166 this.provider = provider;
167 this.orbitType = orbitType;
168 this.positionAngleType = positionAngleType;
169 this.baseState = baseState;
170 }
171
172 /** {@inheritDoc} */
173 @Override
174 public double[] value(final double x) {
175 final double[] array = new double[6];
176 final double[] arrayDot = new double[6];
177 orbitType.mapOrbitToArray(baseState.getOrbit(), positionAngleType, array, arrayDot);
178 array[index] += x;
179 final Orbit orbit = orbitType.mapArrayToOrbit(array, arrayDot,
180 positionAngleType,
181 baseState.getDate(),
182 baseState.getOrbit().getMu(),
183 baseState.getFrame());
184 final SpacecraftState state =
185 new SpacecraftState(orbit,
186 provider.getAttitude(orbit, orbit.getDate(), orbit.getFrame())).withMass(baseState.getMass());
187 return f.value(state);
188 }
189
190 }
191
192 }
193
194