1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation.analytical.gnss;
18
19 import org.hipparchus.analysis.UnivariateFunction;
20 import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
21 import org.hipparchus.analysis.differentiation.Gradient;
22 import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
23 import org.hipparchus.analysis.differentiation.UnivariateDifferentiableFunction;
24 import org.hipparchus.linear.RealMatrix;
25 import org.hipparchus.util.FastMath;
26 import org.junit.jupiter.api.Assertions;
27 import org.junit.jupiter.api.BeforeEach;
28 import org.junit.jupiter.api.Test;
29 import org.orekit.Utils;
30 import org.orekit.data.DataContext;
31 import org.orekit.gnss.SatelliteSystem;
32 import org.orekit.orbits.OrbitType;
33 import org.orekit.orbits.PositionAngleType;
34 import org.orekit.propagation.FieldSpacecraftState;
35 import org.orekit.propagation.MatricesHarvester;
36 import org.orekit.propagation.SpacecraftState;
37 import org.orekit.propagation.analytical.gnss.data.CommonGnssData;
38 import org.orekit.propagation.analytical.gnss.data.GNSSOrbitalElements;
39 import org.orekit.propagation.analytical.gnss.data.GPSLegacyNavigationMessage;
40 import org.orekit.propagation.analytical.gnss.data.GalileoNavigationMessage;
41 import org.orekit.time.AbsoluteDate;
42 import org.orekit.utils.DoubleArrayDictionary;
43 import org.orekit.utils.FieldPVCoordinates;
44 import org.orekit.utils.ParameterDriver;
45
46 import java.util.function.BiConsumer;
47 import java.util.function.ToDoubleFunction;
48
49 class GnssGradientConverterTest {
50
51 private GNSSPropagator propagator;
52
53 @BeforeEach
54 public void setUp() {
55 Utils.setDataRoot("regular-data");
56 final GalileoNavigationMessage goe = new GalileoNavigationMessage(DataContext.getDefault().getTimeScales(), SatelliteSystem.GALILEO);
57 goe.setPRN(4);
58 goe.setWeek(1024);
59 goe.setTime(293400.0);
60 goe.setSqrtA(5440.602949142456);
61 goe.setDeltaN0(3.7394414770330066E-9);
62 goe.setE(2.4088891223073006E-4);
63 goe.setI0(0.9531656087278083);
64 goe.setIDot(-2.36081262303612E-10);
65 goe.setOmega0(-0.36639513583951266);
66 goe.setOmegaDot(-5.7695260382035525E-9);
67 goe.setPa(-1.6870064194345724);
68 goe.setM0(-0.38716557650888);
69 goe.setCuc(-8.903443813323975E-7);
70 goe.setCus(6.61797821521759E-6);
71 goe.setCrc(194.0625);
72 goe.setCrs(-18.78125);
73 goe.setCic(3.166496753692627E-8);
74 goe.setCis(-1.862645149230957E-8);
75 propagator = new GNSSPropagatorBuilder(goe, DataContext.getDefault().getFrames()).build();
76 }
77
78 @Test
79 void testInitialStateStmNoSelectedParameters() {
80 final FieldGnssPropagator<Gradient> gPropagator = new GnssGradientConverter(propagator).getPropagator();
81 Assertions.assertEquals(9, gPropagator.getParametersDrivers().size());
82 Assertions.assertEquals(0, gPropagator.getParametersDrivers().stream().filter(ParameterDriver::isSelected).count());
83 Assertions.assertEquals(6, gPropagator.getInitialState().getOrbit().getA().getFreeParameters());
84 checkUnitaryInitialSTM(gPropagator.getInitialState());
85 }
86
87 @Test
88 void testInitialStateStmAllParametersSelected() {
89 propagator.getOrbitalElements().getParametersDrivers().forEach(p -> p.setSelected(true));
90 final FieldGnssPropagator<Gradient> gPropagator = new GnssGradientConverter(propagator).getPropagator();
91 Assertions.assertEquals(9, gPropagator.getParametersDrivers().size());
92 Assertions.assertEquals( 9, gPropagator.getParametersDrivers().stream().filter(ParameterDriver::isSelected).count());
93 Assertions.assertEquals(15, gPropagator.getInitialState().getOrbit().getA().getFreeParameters());
94 checkUnitaryInitialSTM(gPropagator.getInitialState());
95 }
96
97 @Test
98 void testStmAndJacobian() {
99
100 final GPSLegacyNavigationMessage goe = new GPSLegacyNavigationMessage(DataContext.getDefault().getTimeScales(),
101 SatelliteSystem.GPS);
102 goe.setPRN(7);
103 goe.setWeek(0);
104 goe.setTime(288000);
105 goe.setSqrtA(5153.599830627441);
106 goe.setE(0.012442796607501805);
107 goe.setDeltaN0(4.419469802942352E-9);
108 goe.setI0(0.9558937988021613);
109 goe.setIDot(-2.4608167886110235E-10);
110 goe.setOmega0(1.0479401362158658);
111 goe.setOmegaDot(-7.967117576712062E-9);
112 goe.setPa(-2.4719019944000538);
113 goe.setM0(-1.0899023379614294);
114 goe.setCuc(4.3995678424835205E-6);
115 goe.setCus(1.002475619316101E-5);
116 goe.setCrc(183.40625);
117 goe.setCrs(87.03125);
118 goe.setCic(3.203749656677246E-7);
119 goe.setCis(4.0978193283081055E-8);
120 GNSSPropagator propagator = goe.getPropagator();
121
122
123 Assertions.assertEquals(9, propagator.getOrbitalElements().getParameters().length);
124 propagator.getOrbitalElements().getParameterDriver(CommonGnssData.RADIUS_SINE).setSelected(true);
125 propagator.getOrbitalElements().getParameterDriver(CommonGnssData.RADIUS_COSINE).setSelected(true);
126 final DoubleArrayDictionary initialJacobianColumns = new DoubleArrayDictionary();
127 initialJacobianColumns.put(CommonGnssData.RADIUS_SINE, new double[6]);
128 initialJacobianColumns.put(CommonGnssData.RADIUS_COSINE, new double[6]);
129 final MatricesHarvester harvester = propagator.setupMatricesComputation("stm", null, initialJacobianColumns);
130
131
132 Assertions.assertEquals(2, harvester.getJacobiansColumnsNames().size());
133 Assertions.assertEquals("Span" + CommonGnssData.RADIUS_COSINE + "0", harvester.getJacobiansColumnsNames().get(0));
134 Assertions.assertEquals("Span" + CommonGnssData.RADIUS_SINE + "0", harvester.getJacobiansColumnsNames().get(1));
135
136
137 final SpacecraftState state = propagator.propagate(goe.getDate().shiftedBy(3600.0));
138
139
140 final RealMatrix stm = harvester.getStateTransitionMatrix(state);
141 final double hP = 100000.0;
142 final double hV = 100.0;
143 double maxErrorPP = 0.0;
144 double maxErrorPV = 0.0;
145 double maxErrorVP = 0.0;
146 double maxErrorVV = 0.0;
147 for (int i = 0; i < 6; i++) {
148 for (int j = 0; j < 6; j++) {
149 final double h = j < 3 ? hP : hV;
150 final double error = differentiate(propagator, state.getDate(), h, i, j) - stm.getEntry(i, j);
151 if (i < 3) {
152 if (j < 3) {
153 maxErrorPP = FastMath.max(maxErrorPP, error);
154 } else {
155 maxErrorPV = FastMath.max(maxErrorPV, error);
156 }
157 } else {
158 if (j < 3) {
159 maxErrorVP = FastMath.max(maxErrorVP, error);
160 } else {
161 maxErrorVV = FastMath.max(maxErrorVV, error);
162 }
163 }
164 }
165 }
166 Assertions.assertEquals(0.0, maxErrorPP, 6.5e-13);
167 Assertions.assertEquals(0.0, maxErrorPV, 8.2e-10);
168 Assertions.assertEquals(0.0, maxErrorVP, 8.4e-17);
169 Assertions.assertEquals(0.0, maxErrorVV, 3.8e-13);
170
171
172 final RealMatrix jacobian = harvester.getParametersJacobian(state);
173 final double h = 100000.0;
174 double maxErrorP = 0.0;
175 double maxErrorV = 0.0;
176 for (int i = 0; i < 6; i++) {
177 for (int j = 0; j < 2; j++) {
178 final ToDoubleFunction<GNSSOrbitalElements<?>> getter;
179 final BiConsumer<GNSSOrbitalElements<?>, Double> setter;
180 if (j == 0) {
181 getter = GNSSOrbitalElements::getCrc;
182 setter = GNSSOrbitalElements::setCrc;
183 } else {
184 getter = GNSSOrbitalElements::getCrs;
185 setter = GNSSOrbitalElements::setCrs;
186 }
187 final double error = differentiate(propagator, state.getDate(), getter, setter, h, i) -
188 jacobian.getEntry(i, j);
189 if (i < 3) {
190 maxErrorP = FastMath.max(maxErrorP, error);
191 } else {
192 maxErrorV = FastMath.max(maxErrorV, error);
193 }
194 }
195 }
196 Assertions.assertEquals(0.0, maxErrorP, 6.9e-14);
197 Assertions.assertEquals(0.0, maxErrorV, 1.7e-17);
198
199 }
200
201 private void checkUnitaryInitialSTM(final FieldSpacecraftState<Gradient> initialState) {
202 final FieldPVCoordinates<Gradient> pv0 = initialState.getPVCoordinates();
203 checkUnitary(pv0.getPosition().getX().getGradient(), 0, 4.0e-13, 2.0e-8);
204 checkUnitary(pv0.getPosition().getY().getGradient(), 1, 4.0e-13, 2.0e-8);
205 checkUnitary(pv0.getPosition().getZ().getGradient(), 2, 4.0e-13, 2.0e-8);
206 checkUnitary(pv0.getVelocity().getX().getGradient(), 3, 2.0e-12, 2.0e-12);
207 checkUnitary(pv0.getVelocity().getY().getGradient(), 4, 2.0e-12, 2.0e-12);
208 checkUnitary(pv0.getVelocity().getZ().getGradient(), 5, 2.0e-12, 2.0e-12);
209 }
210
211 private void checkUnitary(final double[] gradient, final int index,
212 final double tolDiag, final double tolNonDiag) {
213
214
215
216 for (int i = 0; i < 6; i++) {
217 if (i == index) {
218
219 Assertions.assertEquals(1.0, gradient[i], tolDiag);
220 } else {
221
222 Assertions.assertEquals(0.0, gradient[i], tolNonDiag);
223 }
224 }
225 }
226
227 private double differentiate(final GNSSPropagator basePropagator, final AbsoluteDate target,
228 final double step, final int outIndex, final int inIndex) {
229
230
231
232 final UnivariateFunction f = h -> {
233
234
235 final SpacecraftState original = basePropagator.getInitialState();
236
237
238 final double[] in = new double[6];
239 OrbitType.CARTESIAN.mapOrbitToArray(original.getOrbit(), PositionAngleType.MEAN, in, null);
240 in[inIndex] += h;
241
242
243 final SpacecraftState shiftedState =
244 new SpacecraftState(OrbitType.CARTESIAN.mapArrayToOrbit(in, null, PositionAngleType.MEAN,
245 original.getDate(),
246 original.getOrbit().getMu(), original.getFrame()),
247 original.getAttitude(), original.getMass());
248
249
250 final GNSSPropagator shiftedPropagator = new GNSSPropagator(shiftedState,
251 basePropagator.getOrbitalElements(),
252 basePropagator.getECEF(),
253 basePropagator.getAttitudeProvider(),
254 shiftedState.getMass());
255
256
257 final SpacecraftState outState = shiftedPropagator.propagate(target);
258
259
260 final double[] out = new double[6];
261 OrbitType.CARTESIAN.mapOrbitToArray(outState.getOrbit(), PositionAngleType.MEAN, out, null);
262 return out[outIndex];
263
264 };
265
266 final FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, step);
267 final UnivariateDifferentiableFunction df = differentiator.differentiate(f);
268
269 return df.value(new UnivariateDerivative1(0.0, 1.0)).getFirstDerivative();
270
271 }
272
273 private double differentiate(final GNSSPropagator basePropagator, final AbsoluteDate target,
274 final ToDoubleFunction<GNSSOrbitalElements<?>> getter,
275 final BiConsumer<GNSSOrbitalElements<?>, Double> setter,
276 final double step, final int outIndex) {
277
278
279
280 final UnivariateFunction f = h -> {
281
282
283 final double initialValue = getter.applyAsDouble(basePropagator.getOrbitalElements());
284
285
286 setter.accept(basePropagator.getOrbitalElements(), initialValue + h);
287
288
289 final SpacecraftState outState = basePropagator.propagate(target);
290
291
292 setter.accept(basePropagator.getOrbitalElements(), initialValue);
293
294
295 final double[] out = new double[6];
296 OrbitType.CARTESIAN.mapOrbitToArray(outState.getOrbit(), PositionAngleType.MEAN, out, null);
297 return out[outIndex];
298
299 };
300
301 final FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, step);
302 final UnivariateDifferentiableFunction df = differentiator.differentiate(f);
303
304 return df.value(new UnivariateDerivative1(0.0, 1.0)).getFirstDerivative();
305
306 }
307
308 }