1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.measurements.modifiers;
18
19 import java.util.ArrayList;
20 import java.util.List;
21
22 import org.hipparchus.analysis.differentiation.DSFactory;
23 import org.hipparchus.analysis.differentiation.DerivativeStructure;
24 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.geometry.euclidean.threed.Vector3D;
27 import org.orekit.attitudes.AttitudeProvider;
28 import org.orekit.attitudes.FieldAttitude;
29 import org.orekit.models.earth.DiscreteTroposphericModel;
30 import org.orekit.orbits.FieldCartesianOrbit;
31 import org.orekit.orbits.FieldOrbit;
32 import org.orekit.propagation.FieldSpacecraftState;
33 import org.orekit.propagation.SpacecraftState;
34 import org.orekit.utils.FieldAngularCoordinates;
35 import org.orekit.utils.FieldPVCoordinates;
36 import org.orekit.utils.ParameterDriver;
37 import org.orekit.utils.TimeStampedFieldAngularCoordinates;
38 import org.orekit.utils.TimeStampedFieldPVCoordinates;
39
40
41
42
43 public class TroposphericDSConverter {
44
45
46 private final int freeStateParameters;
47
48
49 private final List<FieldSpacecraftState<DerivativeStructure>> dsStates;
50
51
52
53
54
55
56 public TroposphericDSConverter(final SpacecraftState state, final int freeStateParameters, final AttitudeProvider provider) {
57
58 this.freeStateParameters = freeStateParameters;
59
60
61 final DSFactory factory = new DSFactory(freeStateParameters, 1);
62
63
64 final Vector3D pos = state.getPVCoordinates().getPosition();
65 final FieldVector3D<DerivativeStructure> posDS = new FieldVector3D<>(factory.variable(0, pos.getX()),
66 factory.variable(1, pos.getY()),
67 factory.variable(2, pos.getZ()));
68
69
70 final Vector3D vel = state.getPVCoordinates().getVelocity();
71 final FieldVector3D<DerivativeStructure> velDS;
72 if (freeStateParameters > 3) {
73 velDS = new FieldVector3D<>(factory.variable(3, vel.getX()),
74 factory.variable(4, vel.getY()),
75 factory.variable(5, vel.getZ()));
76 } else {
77 velDS = new FieldVector3D<>(factory.constant(vel.getX()),
78 factory.constant(vel.getY()),
79 factory.constant(vel.getZ()));
80 }
81
82
83 final Vector3D acc = state.getPVCoordinates().getAcceleration();
84 final FieldVector3D<DerivativeStructure> accDS = new FieldVector3D<>(factory.constant(acc.getX()),
85 factory.constant(acc.getY()),
86 factory.constant(acc.getZ()));
87
88
89 final DerivativeStructure dsM = factory.constant(state.getMass());
90
91 final FieldOrbit<DerivativeStructure> dsOrbit =
92 new FieldCartesianOrbit<>(new TimeStampedFieldPVCoordinates<>(state.getDate(), posDS, velDS, accDS),
93 state.getFrame(), state.getMu());
94
95 final FieldAttitude<DerivativeStructure> dsAttitude;
96 if (freeStateParameters > 3) {
97
98 dsAttitude = provider.getAttitude(dsOrbit, dsOrbit.getDate(), dsOrbit.getFrame());
99 } else {
100
101 dsAttitude = new FieldAttitude<>(factory.getDerivativeField(), state.getAttitude());
102 }
103
104
105 dsStates = new ArrayList<>();
106 dsStates.add(new FieldSpacecraftState<>(dsOrbit, dsAttitude, dsM));
107
108 }
109
110
111
112
113 public int getFreeStateParameters() {
114 return freeStateParameters;
115 }
116
117
118
119
120
121 public FieldSpacecraftState<DerivativeStructure> getState(final DiscreteTroposphericModel tropoModel) {
122
123
124 int nbParams = 0;
125 for (final ParameterDriver driver : tropoModel.getParametersDrivers()) {
126 if (driver.isSelected()) {
127 ++nbParams;
128 }
129 }
130
131
132 while (dsStates.size() < nbParams + 1) {
133 dsStates.add(null);
134 }
135
136 if (dsStates.get(nbParams) == null) {
137
138
139 final DSFactory factory = new DSFactory(freeStateParameters + nbParams, 1);
140 final FieldSpacecraftState<DerivativeStructure> s0 = dsStates.get(0);
141
142
143 final FieldPVCoordinates<DerivativeStructure> pv0 = s0.getPVCoordinates();
144 final FieldOrbit<DerivativeStructure> dsOrbit =
145 new FieldCartesianOrbit<>(new TimeStampedFieldPVCoordinates<>(s0.getDate().toAbsoluteDate(),
146 extend(pv0.getPosition(), factory),
147 extend(pv0.getVelocity(), factory),
148 extend(pv0.getAcceleration(), factory)),
149 s0.getFrame(), s0.getMu());
150
151
152 final FieldAngularCoordinates<DerivativeStructure> ac0 = s0.getAttitude().getOrientation();
153 final FieldAttitude<DerivativeStructure> dsAttitude =
154 new FieldAttitude<>(s0.getAttitude().getReferenceFrame(),
155 new TimeStampedFieldAngularCoordinates<>(dsOrbit.getDate(),
156 extend(ac0.getRotation(), factory),
157 extend(ac0.getRotationRate(), factory),
158 extend(ac0.getRotationAcceleration(), factory)));
159
160
161 final DerivativeStructure dsM = extend(s0.getMass(), factory);
162
163 dsStates.set(nbParams, new FieldSpacecraftState<>(dsOrbit, dsAttitude, dsM));
164
165 }
166
167 return dsStates.get(nbParams);
168
169 }
170
171
172
173
174
175
176 private DerivativeStructure extend(final DerivativeStructure original, final DSFactory factory) {
177 final double[] originalDerivatives = original.getAllDerivatives();
178 final double[] extendedDerivatives = new double[factory.getCompiler().getSize()];
179 System.arraycopy(originalDerivatives, 0, extendedDerivatives, 0, originalDerivatives.length);
180 return factory.build(extendedDerivatives);
181 }
182
183
184
185
186
187
188 private FieldVector3D<DerivativeStructure> extend(final FieldVector3D<DerivativeStructure> original, final DSFactory factory) {
189 return new FieldVector3D<>(extend(original.getX(), factory),
190 extend(original.getY(), factory),
191 extend(original.getZ(), factory));
192 }
193
194
195
196
197
198
199 private FieldRotation<DerivativeStructure> extend(final FieldRotation<DerivativeStructure> original, final DSFactory factory) {
200 return new FieldRotation<>(extend(original.getQ0(), factory),
201 extend(original.getQ1(), factory),
202 extend(original.getQ2(), factory),
203 extend(original.getQ3(), factory),
204 false);
205 }
206
207
208
209
210
211
212 public DerivativeStructure[] getParameters(final FieldSpacecraftState<DerivativeStructure> state,
213 final DiscreteTroposphericModel tropoModel) {
214 final DSFactory factory = state.getMass().getFactory();
215 final List<ParameterDriver> drivers = tropoModel.getParametersDrivers();
216 final DerivativeStructure[] parameters = new DerivativeStructure[drivers.size()];
217 int index = freeStateParameters;
218 for (int i = 0; i < drivers.size(); ++i) {
219 parameters[i] = drivers.get(i).isSelected() ?
220 factory.variable(index++, drivers.get(i).getValue()) :
221 factory.constant(drivers.get(i).getValue());
222 }
223 return parameters;
224 }
225
226 }