1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.measurements;
18
19 import org.hipparchus.stat.descriptive.StreamingStatistics;
20 import org.hipparchus.util.FastMath;
21 import org.junit.jupiter.api.Assertions;
22 import org.junit.jupiter.api.Test;
23 import org.orekit.estimation.Context;
24 import org.orekit.estimation.EstimationTestUtils;
25 import org.orekit.orbits.OrbitType;
26 import org.orekit.orbits.PositionAngleType;
27 import org.orekit.propagation.Propagator;
28 import org.orekit.propagation.SpacecraftState;
29 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
30 import org.orekit.time.AbsoluteDate;
31 import org.orekit.utils.Differentiation;
32 import org.orekit.utils.ParameterDriver;
33 import org.orekit.utils.ParameterFunction;
34
35 import java.util.List;
36
37 public class FDOATest {
38
39
40 private static final double CENTRE_FREQUENCY = 2.3e9;
41
42
43
44
45
46 @Test
47 public void testValues() {
48
49 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
50
51
52 final NumericalPropagatorBuilder propagatorBuilder =
53 context.createBuilder(OrbitType.EQUINOCTIAL, PositionAngleType.TRUE, false,
54 1.0e-6, 60.0, 0.001);
55 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
56 propagatorBuilder);
57 final List<ObservedMeasurement<?>> measurements =
58 EstimationTestUtils.createMeasurements(propagator,
59 new FDOAMeasurementCreator(context, CENTRE_FREQUENCY),
60 1.0, 3.0, 300.0);
61 propagator.clearStepHandlers();
62
63
64 final StreamingStatistics diffStat = new StreamingStatistics();
65
66 for (final ObservedMeasurement<?> measurement : measurements) {
67
68
69 SpacecraftState state = propagator.propagate(measurement.getDate());
70
71
72 final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
73
74
75 diffStat.addValue(FastMath.abs(estimated.getEstimatedValue()[0] - measurement.getObservedValue()[0]));
76 }
77
78
79 Assertions.assertEquals(0.0, diffStat.getMean(), 1e-3);
80 Assertions.assertEquals(0.0, diffStat.getStandardDeviation(), 1e-3);
81
82
83 Assertions.assertEquals(FDOA.MEASUREMENT_TYPE, measurements.get(0).getMeasurementType());
84 }
85
86
87
88
89
90 @Test
91 public void testStateDerivatives() {
92
93 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
94
95
96 final NumericalPropagatorBuilder propagatorBuilder =
97 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
98 1.0e-6, 60.0, 0.001);
99 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
100 propagatorBuilder);
101 final List<ObservedMeasurement<?>> measurements =
102 EstimationTestUtils.createMeasurements(propagator,
103 new FDOAMeasurementCreator(context, CENTRE_FREQUENCY),
104 1.0, 3.0, 300.0);
105 propagator.clearStepHandlers();
106
107 double maxRelativeError = 0;
108 for (final ObservedMeasurement<?> measurement : measurements) {
109
110 final AbsoluteDate date = measurement.getDate().shiftedBy(1);
111 final SpacecraftState state = propagator.propagate(date);
112
113 final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
114 Assertions.assertEquals(3, estimated.getParticipants().length);
115 final double[][] jacobian = estimated.getStateDerivatives(0);
116
117 final double[][] finiteDifferencesJacobian =
118 Differentiation.differentiate(state1 -> measurement.
119 estimate(0, 0, new SpacecraftState[] { state1 }).
120 getEstimatedValue(), 1, propagator.getAttitudeProvider(),
121 OrbitType.CARTESIAN, PositionAngleType.TRUE, 15.0, 3).
122 value(state);
123
124 Assertions.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
125 Assertions.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
126
127 for (int i = 0; i < jacobian.length; ++i) {
128 for (int j = 0; j < jacobian[i].length; ++j) {
129
130 maxRelativeError = FastMath.max(maxRelativeError,
131 FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) /
132 finiteDifferencesJacobian[i][j]));
133 }
134 }
135 }
136
137 Assertions.assertEquals(0, maxRelativeError, 5.4e-6);
138
139 }
140
141
142
143
144
145 @Test
146 public void testParameterDerivatives() {
147
148 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
149
150
151 final NumericalPropagatorBuilder propagatorBuilder =
152 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
153 1.0e-6, 60.0, 0.001);
154 final FDOAMeasurementCreator creator = new FDOAMeasurementCreator(context, CENTRE_FREQUENCY);
155 final GroundStation primary = context.TDOAstations.getKey();
156 primary.getEastOffsetDriver().setSelected(true);
157 primary.getNorthOffsetDriver().setSelected(true);
158 primary.getZenithOffsetDriver().setSelected(true);
159 final double clockOffset = 4.8e-9;
160 final GroundStation secondary = context.TDOAstations.getValue();
161 secondary.getClockOffsetDriver().setValue(clockOffset);
162 secondary.getClockOffsetDriver().setSelected(true);
163 secondary.getEastOffsetDriver().setSelected(true);
164 secondary.getNorthOffsetDriver().setSelected(true);
165 secondary.getZenithOffsetDriver().setSelected(true);
166 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
167 propagatorBuilder);
168 final List<ObservedMeasurement<?>> measurements =
169 EstimationTestUtils.createMeasurements(propagator,
170 creator,
171 1.0, 3.0, 300.0);
172 propagator.clearStepHandlers();
173
174 double maxRelativeError = 0;
175 for (final ObservedMeasurement<?> measurement : measurements) {
176
177
178 final GroundStation primeParameter = ((FDOA) measurement).getPrimeStation();
179 final GroundStation secondParameter = ((FDOA) measurement).getSecondStation();
180
181
182
183
184
185
186
187
188 final double delay = measurement.getObservedValue()[0];
189 final AbsoluteDate date = measurement.getDate().shiftedBy(delay);
190 final SpacecraftState state = propagator.propagate(date);
191 final ParameterDriver[] drivers = new ParameterDriver[] {
192 primeParameter.getEastOffsetDriver(),
193 primeParameter.getNorthOffsetDriver(),
194 primeParameter.getZenithOffsetDriver(),
195 secondParameter.getClockOffsetDriver(),
196 secondParameter.getEastOffsetDriver(),
197 secondParameter.getNorthOffsetDriver(),
198 secondParameter.getZenithOffsetDriver(),
199 };
200 for (int i = 0; i < drivers.length; ++i) {
201 final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i], new AbsoluteDate());
202 Assertions.assertEquals(1, measurement.getDimension());
203 Assertions.assertEquals(1, gradient.length);
204
205 final ParameterFunction dMkdP =
206 Differentiation.differentiate(new ParameterFunction() {
207
208 @Override
209 public double value(final ParameterDriver parameterDriver, AbsoluteDate date) {
210 return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0];
211 }
212 }, 3, 20.0 * drivers[i].getScale());
213 final double ref = dMkdP.value(drivers[i], date);
214 if (ref != 0.0) {
215 maxRelativeError = FastMath.max(maxRelativeError, FastMath.abs((ref - gradient[0]) / ref));
216 }
217 }
218 }
219
220 Assertions.assertEquals(0, maxRelativeError, 3.4e-8);
221
222 }
223
224 }