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.Field;
23 import org.hipparchus.analysis.differentiation.Gradient;
24 import org.hipparchus.analysis.differentiation.GradientField;
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.troposphere.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.propagation.integration.AbstractGradientConverter;
35 import org.orekit.utils.FieldAngularCoordinates;
36 import org.orekit.utils.FieldPVCoordinates;
37 import org.orekit.utils.ParameterDriver;
38 import org.orekit.utils.TimeStampedFieldAngularCoordinates;
39 import org.orekit.utils.TimeStampedFieldPVCoordinates;
40
41
42
43
44
45
46 public class TroposphericGradientConverter extends AbstractGradientConverter {
47
48
49 private final int freeStateParameters;
50
51
52 private final List<FieldSpacecraftState<Gradient>> gStates;
53
54
55
56
57
58
59 public TroposphericGradientConverter(final SpacecraftState state, final int freeStateParameters,
60 final AttitudeProvider provider) {
61
62 super(freeStateParameters);
63 this.freeStateParameters = freeStateParameters;
64
65
66 final Field<Gradient> field = GradientField.getField(freeStateParameters);
67
68
69 final Vector3D pos = state.getPVCoordinates().getPosition();
70 final FieldVector3D<Gradient> posG = new FieldVector3D<>(Gradient.variable(freeStateParameters, 0, pos.getX()),
71 Gradient.variable(freeStateParameters, 1, pos.getY()),
72 Gradient.variable(freeStateParameters, 2, pos.getZ()));
73
74
75 final Vector3D vel = state.getPVCoordinates().getVelocity();
76 final FieldVector3D<Gradient> velG;
77 if (freeStateParameters > 3) {
78 velG = new FieldVector3D<>(Gradient.variable(freeStateParameters, 3, vel.getX()),
79 Gradient.variable(freeStateParameters, 4, vel.getY()),
80 Gradient.variable(freeStateParameters, 5, vel.getZ()));
81 } else {
82 velG = new FieldVector3D<>(Gradient.constant(freeStateParameters, vel.getX()),
83 Gradient.constant(freeStateParameters, vel.getY()),
84 Gradient.constant(freeStateParameters, vel.getZ()));
85 }
86
87
88 final Vector3D acc = state.getPVCoordinates().getAcceleration();
89 final FieldVector3D<Gradient> accG = new FieldVector3D<>(Gradient.constant(freeStateParameters, acc.getX()),
90 Gradient.constant(freeStateParameters, acc.getY()),
91 Gradient.constant(freeStateParameters, acc.getZ()));
92
93
94 final Gradient dsM = Gradient.constant(freeStateParameters, state.getMass());
95
96 final FieldOrbit<Gradient> gOrbit =
97 new FieldCartesianOrbit<>(new TimeStampedFieldPVCoordinates<>(state.getDate(), posG, velG, accG),
98 state.getFrame(),
99 field.getZero().add(state.getMu()));
100
101 final FieldAttitude<Gradient> gAttitude;
102 if (freeStateParameters > 3) {
103
104 gAttitude = provider.getAttitude(gOrbit, gOrbit.getDate(), gOrbit.getFrame());
105 } else {
106
107 gAttitude = new FieldAttitude<>(field, state.getAttitude());
108 }
109
110
111 gStates = new ArrayList<>();
112 gStates.add(new FieldSpacecraftState<>(gOrbit, gAttitude, dsM));
113
114 }
115
116
117
118
119
120 public int getFreeStateParameters() {
121 return freeStateParameters;
122 }
123
124
125
126
127
128
129 public FieldSpacecraftState<Gradient> getState(final DiscreteTroposphericModel tropoModel) {
130
131
132 int nbParams = 0;
133 for (final ParameterDriver driver : tropoModel.getParametersDrivers()) {
134 if (driver.isSelected()) {
135 ++nbParams;
136 }
137 }
138
139
140 while (gStates.size() < nbParams + 1) {
141 gStates.add(null);
142 }
143
144 if (gStates.get(nbParams) == null) {
145
146
147 final int freeParameters = freeStateParameters + nbParams;
148 final FieldSpacecraftState<Gradient> s0 = gStates.get(0);
149
150
151 final FieldPVCoordinates<Gradient> pv0 = s0.getPVCoordinates();
152 final FieldOrbit<Gradient> gOrbit =
153 new FieldCartesianOrbit<>(new TimeStampedFieldPVCoordinates<>(s0.getDate().toAbsoluteDate(),
154 extend(pv0.getPosition(), freeParameters),
155 extend(pv0.getVelocity(), freeParameters),
156 extend(pv0.getAcceleration(), freeParameters)),
157 s0.getFrame(), extend(s0.getMu(), freeParameters));
158
159
160 final FieldAngularCoordinates<Gradient> ac0 = s0.getAttitude().getOrientation();
161 final FieldAttitude<Gradient> gAttitude =
162 new FieldAttitude<>(s0.getAttitude().getReferenceFrame(),
163 new TimeStampedFieldAngularCoordinates<>(gOrbit.getDate(),
164 extend(ac0.getRotation(), freeParameters),
165 extend(ac0.getRotationRate(), freeParameters),
166 extend(ac0.getRotationAcceleration(), freeParameters)));
167
168
169 final Gradient gM = extend(s0.getMass(), freeParameters);
170
171 gStates.set(nbParams, new FieldSpacecraftState<>(gOrbit, gAttitude, gM));
172
173 }
174
175 return gStates.get(nbParams);
176
177 }
178
179
180
181
182
183
184
185 public Gradient[] getParameters(final FieldSpacecraftState<Gradient> state,
186 final DiscreteTroposphericModel tropoModel) {
187 final int freeParameters = state.getMass().getFreeParameters();
188 final List<ParameterDriver> drivers = tropoModel.getParametersDrivers();
189 final Gradient[] parameters = new Gradient[drivers.size()];
190 int index = freeStateParameters;
191 for (int i = 0; i < drivers.size(); ++i) {
192 parameters[i] = drivers.get(i).isSelected() ?
193 Gradient.variable(freeParameters, index++, drivers.get(i).getValue()) :
194 Gradient.constant(freeParameters, drivers.get(i).getValue());
195 }
196 return parameters;
197 }
198
199 }