1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.leastsquares;
18
19 import java.io.File;
20 import java.io.IOException;
21 import java.net.URISyntaxException;
22 import java.text.ParseException;
23 import java.util.ArrayList;
24 import java.util.List;
25 import java.util.NoSuchElementException;
26
27 import org.hipparchus.geometry.euclidean.threed.Vector3D;
28 import org.hipparchus.linear.RealMatrix;
29 import org.hipparchus.util.FastMath;
30 import org.junit.Assert;
31 import org.junit.Test;
32 import org.orekit.KeyValueFileParser;
33 import org.orekit.Utils;
34 import org.orekit.attitudes.AttitudeProvider;
35 import org.orekit.bodies.CelestialBody;
36 import org.orekit.bodies.OneAxisEllipsoid;
37 import org.orekit.errors.OrekitException;
38 import org.orekit.estimation.common.AbstractOrbitDetermination;
39 import org.orekit.estimation.common.ParameterKey;
40 import org.orekit.estimation.common.ResultBatchLeastSquares;
41 import org.orekit.forces.ForceModel;
42 import org.orekit.forces.drag.DragForce;
43 import org.orekit.forces.drag.DragSensitive;
44 import org.orekit.forces.empirical.AccelerationModel;
45 import org.orekit.forces.empirical.ParametricAcceleration;
46 import org.orekit.forces.empirical.PolynomialAccelerationModel;
47 import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel;
48 import org.orekit.forces.gravity.OceanTides;
49 import org.orekit.forces.gravity.Relativity;
50 import org.orekit.forces.gravity.SolidTides;
51 import org.orekit.forces.gravity.ThirdBodyAttraction;
52 import org.orekit.forces.gravity.potential.GravityFieldFactory;
53 import org.orekit.forces.gravity.potential.ICGEMFormatReader;
54 import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
55 import org.orekit.forces.radiation.KnockeRediffusedForceModel;
56 import org.orekit.forces.radiation.RadiationSensitive;
57 import org.orekit.forces.radiation.SolarRadiationPressure;
58 import org.orekit.models.earth.atmosphere.Atmosphere;
59 import org.orekit.orbits.Orbit;
60 import org.orekit.orbits.PositionAngle;
61 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
62 import org.orekit.propagation.conversion.ODEIntegratorBuilder;
63 import org.orekit.time.TimeScalesFactory;
64 import org.orekit.utils.IERSConventions;
65 import org.orekit.utils.ParameterDriver;
66 import org.orekit.utils.ParameterDriversList;
67 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
68
69 public class NumericalOrbitDeterminationTest extends AbstractOrbitDetermination<NumericalPropagatorBuilder> {
70
71
72 private NormalizedSphericalHarmonicsProvider gravityField;
73
74
75 @Override
76 protected void createGravityField(final KeyValueFileParser<ParameterKey> parser)
77 throws NoSuchElementException {
78
79 final int degree = parser.getInt(ParameterKey.CENTRAL_BODY_DEGREE);
80 final int order = FastMath.min(degree, parser.getInt(ParameterKey.CENTRAL_BODY_ORDER));
81 gravityField = GravityFieldFactory.getNormalizedProvider(degree, order);
82
83 }
84
85
86 @Override
87 protected double getMu() {
88 return gravityField.getMu();
89 }
90
91
92 @Override
93 protected NumericalPropagatorBuilder createPropagatorBuilder(final Orbit referenceOrbit,
94 final ODEIntegratorBuilder builder,
95 final double positionScale) {
96 return new NumericalPropagatorBuilder(referenceOrbit, builder, PositionAngle.MEAN,
97 positionScale);
98 }
99
100
101 @Override
102 protected void setMass(final NumericalPropagatorBuilder propagatorBuilder,
103 final double mass) {
104 propagatorBuilder.setMass(mass);
105 }
106
107
108 @Override
109 protected List<ParameterDriver> setGravity(final NumericalPropagatorBuilder propagatorBuilder,
110 final OneAxisEllipsoid body) {
111 final ForceModel gravityModel = new HolmesFeatherstoneAttractionModel(body.getBodyFrame(), gravityField);
112 propagatorBuilder.addForceModel(gravityModel);
113 return gravityModel.getParametersDrivers();
114 }
115
116
117 @Override
118 protected List<ParameterDriver> setOceanTides(final NumericalPropagatorBuilder propagatorBuilder,
119 final IERSConventions conventions,
120 final OneAxisEllipsoid body,
121 final int degree, final int order) {
122 final ForceModel tidesModel = new OceanTides(body.getBodyFrame(),
123 gravityField.getAe(), gravityField.getMu(),
124 degree, order, conventions,
125 TimeScalesFactory.getUT1(conventions, true));
126 propagatorBuilder.addForceModel(tidesModel);
127 return tidesModel.getParametersDrivers();
128 }
129
130
131 @Override
132 protected List<ParameterDriver> setSolidTides(final NumericalPropagatorBuilder propagatorBuilder,
133 final IERSConventions conventions,
134 final OneAxisEllipsoid body,
135 final CelestialBody[] solidTidesBodies) {
136 final ForceModel tidesModel = new SolidTides(body.getBodyFrame(),
137 gravityField.getAe(), gravityField.getMu(),
138 gravityField.getTideSystem(), conventions,
139 TimeScalesFactory.getUT1(conventions, true),
140 solidTidesBodies);
141 propagatorBuilder.addForceModel(tidesModel);
142 return tidesModel.getParametersDrivers();
143 }
144
145
146 @Override
147 protected List<ParameterDriver> setThirdBody(final NumericalPropagatorBuilder propagatorBuilder,
148 final CelestialBody thirdBody) {
149 final ForceModel thirdBodyModel = new ThirdBodyAttraction(thirdBody);
150 propagatorBuilder.addForceModel(thirdBodyModel);
151 return thirdBodyModel.getParametersDrivers();
152 }
153
154
155 @Override
156 protected List<ParameterDriver> setDrag(final NumericalPropagatorBuilder propagatorBuilder,
157 final Atmosphere atmosphere, final DragSensitive spacecraft) {
158 final ForceModel dragModel = new DragForce(atmosphere, spacecraft);
159 propagatorBuilder.addForceModel(dragModel);
160 return dragModel.getParametersDrivers();
161 }
162
163
164 @Override
165 protected List<ParameterDriver> setSolarRadiationPressure(final NumericalPropagatorBuilder propagatorBuilder, final CelestialBody sun,
166 final double equatorialRadius, final RadiationSensitive spacecraft) {
167 final ForceModel srpModel = new SolarRadiationPressure(sun, equatorialRadius, spacecraft);
168 propagatorBuilder.addForceModel(srpModel);
169 return srpModel.getParametersDrivers();
170 }
171
172
173 @Override
174 protected List<ParameterDriver> setAlbedoInfrared(final NumericalPropagatorBuilder propagatorBuilder,
175 final CelestialBody sun, final double equatorialRadius,
176 final double angularResolution,
177 final RadiationSensitive spacecraft) {
178 final ForceModel albedoIR = new KnockeRediffusedForceModel(sun, spacecraft, equatorialRadius, angularResolution);
179 propagatorBuilder.addForceModel(albedoIR);
180 return albedoIR.getParametersDrivers();
181 }
182
183
184 @Override
185 protected List<ParameterDriver> setRelativity(final NumericalPropagatorBuilder propagatorBuilder) {
186 final ForceModel relativityModel = new Relativity(gravityField.getMu());
187 propagatorBuilder.addForceModel(relativityModel);
188 return relativityModel.getParametersDrivers();
189 }
190
191
192 @Override
193 protected List<ParameterDriver> setPolynomialAcceleration(final NumericalPropagatorBuilder propagatorBuilder,
194 final String name, final Vector3D direction, final int degree) {
195 final AccelerationModel accModel = new PolynomialAccelerationModel(name, null, degree);
196 final ForceModel polynomialModel = new ParametricAcceleration(direction, true, accModel);
197 propagatorBuilder.addForceModel(polynomialModel);
198 return polynomialModel.getParametersDrivers();
199 }
200
201
202 @Override
203 protected void setAttitudeProvider(final NumericalPropagatorBuilder propagatorBuilder,
204 final AttitudeProvider attitudeProvider) {
205 propagatorBuilder.setAttitudeProvider(attitudeProvider);
206 }
207
208 @Test
209
210 public void testLageos2()
211 throws URISyntaxException, IllegalArgumentException, IOException,
212 OrekitException, ParseException {
213
214
215 final String inputPath = NumericalOrbitDeterminationTest.class.getClassLoader().getResource("orbit-determination/Lageos2/od_test_Lageos2.in").toURI().getPath();
216 final File input = new File(inputPath);
217
218
219 Utils.setDataRoot("orbit-determination/february-2016:potential/icgem-format");
220 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
221
222
223 ResultBatchLeastSquares odLageos2 = runBLS(input, false);
224
225
226
227 final double distanceAccuracy = 0.62;
228 final double velocityAccuracy = 1.4e-4;
229
230
231 final int numberOfIte = 5;
232 final int numberOfEval = 5;
233
234 Assert.assertEquals(numberOfIte, odLageos2.getNumberOfIteration());
235 Assert.assertEquals(numberOfEval, odLageos2.getNumberOfEvaluation());
236
237
238 final Vector3D estimatedPos = odLageos2.getEstimatedPV().getPosition();
239 final Vector3D estimatedVel = odLageos2.getEstimatedPV().getVelocity();
240
241 final Vector3D refPos = new Vector3D(7526994.072, -9646309.832, 1464110.239);
242 final Vector3D refVel = new Vector3D(3033.794, 1715.265, -4447.659);
243 Assert.assertEquals(0.0, Vector3D.distance(refPos, estimatedPos), distanceAccuracy);
244 Assert.assertEquals(0.0, Vector3D.distance(refVel, estimatedVel), velocityAccuracy);
245
246
247 final long nbRange = 95;
248 final double[] RefStatRange = { -0.756, 0.845, 0.0, 0.261 };
249 Assert.assertEquals(nbRange, odLageos2.getRangeStat().getN());
250 Assert.assertEquals(RefStatRange[0], odLageos2.getRangeStat().getMin(), distanceAccuracy);
251 Assert.assertEquals(RefStatRange[1], odLageos2.getRangeStat().getMax(), distanceAccuracy);
252 Assert.assertEquals(RefStatRange[2], odLageos2.getRangeStat().getMean(), distanceAccuracy);
253 Assert.assertEquals(RefStatRange[3], odLageos2.getRangeStat().getStandardDeviation(), distanceAccuracy);
254 }
255
256 @Test
257
258 public void testGNSS()
259 throws URISyntaxException, IllegalArgumentException, IOException,
260 OrekitException, ParseException {
261
262
263 final String inputPath = NumericalOrbitDeterminationTest.class.getClassLoader().getResource("orbit-determination/GNSS/od_test_GPS07.in").toURI().getPath();
264 final File input = new File(inputPath);
265
266
267 Utils.setDataRoot("orbit-determination/february-2016:potential/icgem-format");
268 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
269
270
271 ResultBatchLeastSquares odGNSS = runBLS(input, false);
272
273
274
275
276 final double distanceAccuracy = 1.06;
277 final double velocityAccuracy = 2.26e-3;
278
279
280 final int numberOfIte = 2;
281 final int numberOfEval = 3;
282
283 Assert.assertEquals(numberOfIte, odGNSS.getNumberOfIteration());
284 Assert.assertEquals(numberOfEval, odGNSS.getNumberOfEvaluation());
285
286
287 final Vector3D estimatedPos = odGNSS.getEstimatedPV().getPosition();
288 final Vector3D estimatedVel = odGNSS.getEstimatedPV().getVelocity();
289 final Vector3D refPos = new Vector3D(-2747606.680868164, 22572091.30648564, 13522761.402325712);
290 final Vector3D refVel = new Vector3D(-2729.5151218788005, 1142.6629459030657, -2523.9055974487947);
291 Assert.assertEquals(0.0, Vector3D.distance(refPos, estimatedPos), distanceAccuracy);
292 Assert.assertEquals(0.0, Vector3D.distance(refVel, estimatedVel), velocityAccuracy);
293
294
295 final long nbRangeInit = 8981;
296 final long nbRangeExcluded = 305;
297 final double[] RefStatRange = { -3.847, 8.307, 0.0, 0.873 };
298 Assert.assertEquals(nbRangeInit - nbRangeExcluded, odGNSS.getRangeStat().getN());
299 Assert.assertEquals(RefStatRange[0], odGNSS.getRangeStat().getMin(), 1.0e-3);
300 Assert.assertEquals(RefStatRange[1], odGNSS.getRangeStat().getMax(), 1.0e-3);
301 Assert.assertEquals(RefStatRange[2], odGNSS.getRangeStat().getMean(), 1.0e-3);
302 Assert.assertEquals(RefStatRange[3], odGNSS.getRangeStat().getStandardDeviation(), 1.0e-3);
303
304 }
305
306 @Test
307
308 public void testW3B()
309 throws URISyntaxException, IllegalArgumentException, IOException,
310 OrekitException, ParseException {
311
312
313 final String inputPath = NumericalOrbitDeterminationTest.class.getClassLoader().getResource("orbit-determination/W3B/od_test_W3.in").toURI().getPath();
314 final File input = new File(inputPath);
315
316
317 Utils.setDataRoot("orbit-determination/W3B:potential/icgem-format");
318 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
319
320
321 ResultBatchLeastSquares odsatW3 = runBLS(input, false);
322
323
324
325 final double distanceAccuracy = 0.1;
326 final double velocityAccuracy = 1e-4;
327 final double angleAccuracy = 1e-5;
328
329
330 Assert.assertTrue(odsatW3.getNumberOfIteration() < 6);
331 Assert.assertTrue(odsatW3.getNumberOfEvaluation() < 10);
332
333
334 final Vector3D estimatedPos = odsatW3.getEstimatedPV().getPosition();
335 final Vector3D estimatedVel = odsatW3.getEstimatedPV().getVelocity();
336 final Vector3D refPos = new Vector3D(-40541446.255, -9905357.41, 206777.413);
337 final Vector3D refVel = new Vector3D(759.0685, -1476.5156, 54.793);
338 Assert.assertEquals(0.0, Vector3D.distance(refPos, estimatedPos), distanceAccuracy);
339 Assert.assertEquals(0.0, Vector3D.distance(refVel, estimatedVel), velocityAccuracy);
340
341
342
343 final double dragCoef = -0.2154;
344 final ParameterDriversList propagatorParameters = odsatW3.getPropagatorParameters();
345 Assert.assertEquals(dragCoef, propagatorParameters.getDrivers().get(0).getValue(), 1e-3);
346 final Vector3D leakAcceleration0 =
347 new Vector3D(propagatorParameters.getDrivers().get(1).getValue(),
348 propagatorParameters.getDrivers().get(3).getValue(),
349 propagatorParameters.getDrivers().get(5).getValue());
350
351 Assert.assertEquals(8.002e-6, leakAcceleration0.getNorm(), 1.0e-8);
352 final Vector3D leakAcceleration1 =
353 new Vector3D(propagatorParameters.getDrivers().get(2).getValue(),
354 propagatorParameters.getDrivers().get(4).getValue(),
355 propagatorParameters.getDrivers().get(6).getValue());
356 Assert.assertEquals(3.058e-10, leakAcceleration1.getNorm(), 1.0e-12);
357
358
359 final List<DelegatingDriver> list = new ArrayList<DelegatingDriver>();
360 list.addAll(odsatW3.getMeasurementsParameters().getDrivers());
361 sortParametersChanges(list);
362
363
364 final double[] CastleAzElBias = { 0.062701342, -0.003613508 };
365 final double CastleRangeBias = 11274.4677;
366 Assert.assertEquals(CastleAzElBias[0], FastMath.toDegrees(list.get(0).getValue()), angleAccuracy);
367 Assert.assertEquals(CastleAzElBias[1], FastMath.toDegrees(list.get(1).getValue()), angleAccuracy);
368 Assert.assertEquals(CastleRangeBias, list.get(2).getValue(), distanceAccuracy);
369
370
371 final double[] FucAzElBias = { -0.053526137, 0.075483886 };
372 final double FucRangeBias = 13467.8256;
373 Assert.assertEquals(FucAzElBias[0], FastMath.toDegrees(list.get(3).getValue()), angleAccuracy);
374 Assert.assertEquals(FucAzElBias[1], FastMath.toDegrees(list.get(4).getValue()), angleAccuracy);
375 Assert.assertEquals(FucRangeBias, list.get(5).getValue(), distanceAccuracy);
376
377
378 final double[] KumAzElBias = { -0.023574208, -0.054520756 };
379 final double KumRangeBias = 13512.57594;
380 Assert.assertEquals(KumAzElBias[0], FastMath.toDegrees(list.get(6).getValue()), angleAccuracy);
381 Assert.assertEquals(KumAzElBias[1], FastMath.toDegrees(list.get(7).getValue()), angleAccuracy);
382 Assert.assertEquals(KumRangeBias, list.get(8).getValue(), distanceAccuracy);
383
384
385 final double[] PreAzElBias = { 0.030201539, 0.009747877 };
386 final double PreRangeBias = 13594.11889;
387 Assert.assertEquals(PreAzElBias[0], FastMath.toDegrees(list.get( 9).getValue()), angleAccuracy);
388 Assert.assertEquals(PreAzElBias[1], FastMath.toDegrees(list.get(10).getValue()), angleAccuracy);
389 Assert.assertEquals(PreRangeBias, list.get(11).getValue(), distanceAccuracy);
390
391
392 final double[] UraAzElBias = { 0.167814449, -0.12305252 };
393 final double UraRangeBias = 13450.26738;
394 Assert.assertEquals(UraAzElBias[0], FastMath.toDegrees(list.get(12).getValue()), angleAccuracy);
395 Assert.assertEquals(UraAzElBias[1], FastMath.toDegrees(list.get(13).getValue()), angleAccuracy);
396 Assert.assertEquals(UraRangeBias, list.get(14).getValue(), distanceAccuracy);
397
398
399 final long nbRange = 182;
400
401 final double[] RefStatRange = { -18.39149369, 12.54165259, -4.32E-05, 4.374712716 };
402 Assert.assertEquals(nbRange, odsatW3.getRangeStat().getN());
403 Assert.assertEquals(RefStatRange[0], odsatW3.getRangeStat().getMin(), distanceAccuracy);
404 Assert.assertEquals(RefStatRange[1], odsatW3.getRangeStat().getMax(), distanceAccuracy);
405 Assert.assertEquals(RefStatRange[2], odsatW3.getRangeStat().getMean(), distanceAccuracy);
406 Assert.assertEquals(RefStatRange[3], odsatW3.getRangeStat().getStandardDeviation(), distanceAccuracy);
407
408
409 final long nbAzi = 339;
410
411 final double[] RefStatAzi = { -0.043033616, 0.025297558, -1.39E-10, 0.010063041 };
412 Assert.assertEquals(nbAzi, odsatW3.getAzimStat().getN());
413 Assert.assertEquals(RefStatAzi[0], odsatW3.getAzimStat().getMin(), angleAccuracy);
414 Assert.assertEquals(RefStatAzi[1], odsatW3.getAzimStat().getMax(), angleAccuracy);
415 Assert.assertEquals(RefStatAzi[2], odsatW3.getAzimStat().getMean(), angleAccuracy);
416 Assert.assertEquals(RefStatAzi[3], odsatW3.getAzimStat().getStandardDeviation(), angleAccuracy);
417
418
419 final long nbEle = 339;
420 final double[] RefStatEle = { -0.025061971, 0.056294405, -4.10E-11, 0.011604931 };
421 Assert.assertEquals(nbEle, odsatW3.getElevStat().getN());
422 Assert.assertEquals(RefStatEle[0], odsatW3.getElevStat().getMin(), angleAccuracy);
423 Assert.assertEquals(RefStatEle[1], odsatW3.getElevStat().getMax(), angleAccuracy);
424 Assert.assertEquals(RefStatEle[2], odsatW3.getElevStat().getMean(), angleAccuracy);
425 Assert.assertEquals(RefStatEle[3], odsatW3.getElevStat().getStandardDeviation(), angleAccuracy);
426
427 RealMatrix covariances = odsatW3.getCovariances();
428 Assert.assertEquals(28, covariances.getRowDimension());
429 Assert.assertEquals(28, covariances.getColumnDimension());
430
431
432 Assert.assertEquals(0.687998, covariances.getEntry(6, 6), 1.0e-4);
433
434
435 Assert.assertEquals(2.0540e-12, covariances.getEntry(7, 7), 1.0e-16);
436
437
438 Assert.assertEquals(2.4930e-11, covariances.getEntry(9, 9), 1.0e-15);
439
440
441 Assert.assertEquals(7.6720e-11, covariances.getEntry(11, 11), 1.0e-15);
442 }
443
444 }