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