1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.control.indirect.adjoint;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
21 import org.hipparchus.util.FastMath;
22 import org.hipparchus.util.MathArrays;
23 import org.orekit.control.indirect.adjoint.cost.FieldCartesianCost;
24 import org.orekit.errors.OrekitException;
25 import org.orekit.errors.OrekitMessages;
26 import org.orekit.frames.Frame;
27 import org.orekit.orbits.OrbitType;
28 import org.orekit.propagation.FieldSpacecraftState;
29 import org.orekit.propagation.integration.FieldAdditionalDerivativesProvider;
30 import org.orekit.propagation.integration.FieldCombinedDerivatives;
31 import org.orekit.time.FieldAbsoluteDate;
32 import org.orekit.utils.FieldPVCoordinates;
33
34
35
36
37
38
39
40
41
42 public class FieldCartesianAdjointDerivativesProvider<T extends CalculusFieldElement<T>> implements FieldAdditionalDerivativesProvider<T> {
43
44
45 private final CartesianAdjointEquationTerm[] adjointEquationTerms;
46
47
48 private final FieldCartesianCost<T> cost;
49
50
51
52
53
54
55 public FieldCartesianAdjointDerivativesProvider(final FieldCartesianCost<T> cost,
56 final CartesianAdjointEquationTerm... adjointEquationTerms) {
57 this.cost = cost;
58 this.adjointEquationTerms = adjointEquationTerms;
59 }
60
61
62
63
64
65 public FieldCartesianCost<T> getCost() {
66 return cost;
67 }
68
69
70
71 public String getName() {
72 return cost.getAdjointName();
73 }
74
75
76
77
78 public int getDimension() {
79 return cost.getAdjointDimension();
80 }
81
82
83 @Override
84 public void init(final FieldSpacecraftState<T> initialState, final FieldAbsoluteDate<T> target) {
85 FieldAdditionalDerivativesProvider.super.init(initialState, target);
86 if (initialState.isOrbitDefined() && initialState.getOrbit().getType() != OrbitType.CARTESIAN) {
87 throw new OrekitException(OrekitMessages.WRONG_COORDINATES_FOR_ADJOINT_EQUATION);
88 }
89 }
90
91
92 @Override
93 public FieldCombinedDerivatives<T> combinedDerivatives(final FieldSpacecraftState<T> state) {
94
95 final T mass = state.getMass();
96 final T[] adjointVariables = state.getAdditionalState(getName());
97 final int adjointDimension = getDimension();
98 final T[] additionalDerivatives = MathArrays.buildArray(mass.getField(), adjointDimension);
99 final T[] cartesianVariablesAndMass = formCartesianAndMassVector(state);
100
101
102 final T[] mainDerivativesIncrements = MathArrays.buildArray(mass.getField(), 7);
103 final FieldVector3D<T> thrustAccelerationVector = getCost().getFieldThrustAccelerationVector(adjointVariables, mass);
104 mainDerivativesIncrements[3] = thrustAccelerationVector.getX();
105 mainDerivativesIncrements[4] = thrustAccelerationVector.getY();
106 mainDerivativesIncrements[5] = thrustAccelerationVector.getZ();
107 final T thrustAccelerationNorm = thrustAccelerationVector.getNorm();
108 if (thrustAccelerationVector.getNorm().getReal() != 0.) {
109 final T thrustForceMagnitude = thrustAccelerationNorm.multiply(mass);
110 mainDerivativesIncrements[6] = thrustForceMagnitude.multiply(getCost().getMassFlowRateFactor().negate());
111 }
112
113
114 additionalDerivatives[3] = adjointVariables[0].negate();
115 additionalDerivatives[4] = adjointVariables[1].negate();
116 additionalDerivatives[5] = adjointVariables[2].negate();
117
118
119 final FieldAbsoluteDate<T> date = state.getDate();
120 final Frame propagationFrame = state.getFrame();
121 for (final CartesianAdjointEquationTerm equationTerm: adjointEquationTerms) {
122 final T[] contribution = equationTerm.getFieldRatesContribution(date, cartesianVariablesAndMass, adjointVariables,
123 propagationFrame);
124 for (int i = 0; i < FastMath.min(adjointDimension, contribution.length); i++) {
125 additionalDerivatives[i] = additionalDerivatives[i].add(contribution[i]);
126 }
127 }
128
129
130 getCost().updateFieldAdjointDerivatives(adjointVariables, mass, additionalDerivatives);
131
132 return new FieldCombinedDerivatives<>(additionalDerivatives, mainDerivativesIncrements);
133 }
134
135
136
137
138
139
140 private T[] formCartesianAndMassVector(final FieldSpacecraftState<T> state) {
141 final T mass = state.getMass();
142 final T[] cartesianVariablesAndMass = MathArrays.buildArray(mass.getField(), 7);
143 final FieldPVCoordinates<T> pvCoordinates = state.getPVCoordinates();
144 System.arraycopy(pvCoordinates.getPosition().toArray(), 0, cartesianVariablesAndMass, 0, 3);
145 System.arraycopy(pvCoordinates.getVelocity().toArray(), 0, cartesianVariablesAndMass, 3, 3);
146 cartesianVariablesAndMass[6] = mass;
147 return cartesianVariablesAndMass;
148 }
149
150
151
152
153
154
155 public T evaluateHamiltonian(final FieldSpacecraftState<T> state) {
156 final T[] cartesianAndMassVector = formCartesianAndMassVector(state);
157 final T[] adjointVariables = state.getAdditionalState(getName());
158 T hamiltonian = adjointVariables[0].multiply(cartesianAndMassVector[3]).add(adjointVariables[1].multiply(cartesianAndMassVector[4]))
159 .add(adjointVariables[2].multiply(cartesianAndMassVector[5]));
160 final FieldAbsoluteDate<T> date = state.getDate();
161 final Frame propagationFrame = state.getFrame();
162 for (final CartesianAdjointEquationTerm adjointEquationTerm : adjointEquationTerms) {
163 final T contribution = adjointEquationTerm.getFieldHamiltonianContribution(date, cartesianAndMassVector,
164 adjointVariables, propagationFrame);
165 hamiltonian = hamiltonian.add(contribution);
166 }
167 final T mass = state.getMass();
168 if (adjointVariables.length != 6) {
169 final T thrustAccelerationNorm = getCost().getFieldThrustAccelerationVector(adjointVariables, mass).getNorm();
170 final T thrustForceNorm = thrustAccelerationNorm.multiply(mass);
171 hamiltonian = hamiltonian.subtract(adjointVariables[6].multiply(getCost().getMassFlowRateFactor()).multiply(thrustForceNorm));
172 }
173 hamiltonian = hamiltonian.add(getCost().getFieldHamiltonianContribution(adjointVariables, mass));
174 return hamiltonian;
175 }
176 }