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.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.numerical.NumericalPropagator;
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 new StateJacobian() {
103
104 @Override
105 public double[][] value(final SpacecraftState state) {
106 final double[] tolerances =
107 NumericalPropagator.tolerances(dP, state.getOrbit(), orbitType)[0];
108 final double[][] jacobian = new double[dimension][6];
109 for (int j = 0; j < 6; ++j) {
110
111 // compute partial derivatives with respect to state component j
112 final UnivariateVectorFunction componentJ =
113 new StateComponentFunction(j, function, provider, state,
114 orbitType, positionAngleType);
115 final FiniteDifferencesDifferentiator differentiator =
116 new FiniteDifferencesDifferentiator(nbPoints, tolerances[j]);
117 final UnivariateDifferentiableVectorFunction differentiatedJ =
118 differentiator.differentiate(componentJ);
119
120 final DerivativeStructure[] c = differentiatedJ.value(FACTORY.variable(0, 0.0));
121
122 // populate the j-th column of the Jacobian
123 for (int i = 0; i < dimension; ++i) {
124 jacobian[i][j] = c[i].getPartialDerivative(1);
125 }
126
127 }
128
129 return jacobian;
130
131 }
132
133 };
134 }
135
136 /** Restriction of a {@link StateFunction} to a function of a single state component.
137 */
138 private static class StateComponentFunction implements UnivariateVectorFunction {
139
140 /** Component index in the mapped orbit array. */
141 private final int index;
142
143 /** State-dependent function. */
144 private final StateFunction f;
145
146 /** Type used to map the orbit to a one dimensional array. */
147 private final OrbitType orbitType;
148
149 /** Tpe of the position angle used for orbit mapping to array. */
150 private final PositionAngleType positionAngleType;
151
152 /** Base state, of which only one component will change. */
153 private final SpacecraftState baseState;
154
155 /** Attitude provider to use for modified states. */
156 private final AttitudeProvider provider;
157
158 /** Simple constructor.
159 * @param index component index in the mapped orbit array
160 * @param f state-dependent function
161 * @param provider attitude provider to use for modified states
162 * @param baseState base state, of which only one component will change
163 * @param orbitType type used to map the orbit to a one dimensional array
164 * @param positionAngleType type of the position angle used for orbit mapping to array
165 */
166 StateComponentFunction(final int index, final StateFunction f,
167 final AttitudeProvider provider, final SpacecraftState baseState,
168 final OrbitType orbitType, final PositionAngleType positionAngleType) {
169 this.index = index;
170 this.f = f;
171 this.provider = provider;
172 this.orbitType = orbitType;
173 this.positionAngleType = positionAngleType;
174 this.baseState = baseState;
175 }
176
177 /** {@inheritDoc} */
178 @Override
179 public double[] value(final double x) {
180 final double[] array = new double[6];
181 final double[] arrayDot = new double[6];
182 orbitType.mapOrbitToArray(baseState.getOrbit(), positionAngleType, array, arrayDot);
183 array[index] += x;
184 final Orbit orbit = orbitType.mapArrayToOrbit(array, arrayDot,
185 positionAngleType,
186 baseState.getDate(),
187 baseState.getMu(),
188 baseState.getFrame());
189 final SpacecraftState state =
190 new SpacecraftState(orbit,
191 provider.getAttitude(orbit, orbit.getDate(), orbit.getFrame()),
192 baseState.getMass());
193 return f.value(state);
194 }
195
196 }
197
198 }
199
200