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