1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.measurements.generation;
18
19 import java.util.SortedSet;
20
21 import org.hipparchus.linear.MatrixUtils;
22 import org.hipparchus.linear.RealMatrix;
23 import org.hipparchus.random.CorrelatedRandomVectorGenerator;
24 import org.hipparchus.random.GaussianRandomGenerator;
25 import org.hipparchus.random.RandomGenerator;
26 import org.hipparchus.random.Well19937a;
27 import org.hipparchus.util.FastMath;
28 import org.junit.Assert;
29 import org.junit.Before;
30 import org.junit.Test;
31 import org.orekit.estimation.Context;
32 import org.orekit.estimation.EstimationTestUtils;
33 import org.orekit.estimation.Force;
34 import org.orekit.estimation.measurements.ObservableSatellite;
35 import org.orekit.estimation.measurements.ObservedMeasurement;
36 import org.orekit.estimation.measurements.Position;
37 import org.orekit.estimation.measurements.modifiers.Bias;
38 import org.orekit.orbits.OrbitType;
39 import org.orekit.orbits.PositionAngle;
40 import org.orekit.propagation.Propagator;
41 import org.orekit.propagation.SpacecraftState;
42 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
43 import org.orekit.time.AbsoluteDate;
44 import org.orekit.time.BurstSelector;
45 import org.orekit.time.TimeScalesFactory;
46
47 public class PositionBuilderTest {
48
49 private static final double SIGMA = 10.0;
50 private static final double BIAS = 5.0;
51
52 private MeasurementBuilder<Position> getBuilder(final RandomGenerator random, final ObservableSatellite satellite) {
53 final RealMatrix covariance = MatrixUtils.createRealDiagonalMatrix(new double[] {
54 SIGMA * SIGMA, SIGMA * SIGMA, SIGMA * SIGMA
55 });
56 MeasurementBuilder<Position> pb =
57 new PositionBuilder(random == null ? null : new CorrelatedRandomVectorGenerator(covariance,
58 1.0e-10,
59 new GaussianRandomGenerator(random)),
60 SIGMA, 1.0, satellite);
61 pb.addModifier(new Bias<>(new String[] { "pxBias", "pyBias", "pzBias" },
62 new double[] { BIAS, BIAS, BIAS },
63 new double[] { 1.0, 1.0, 1.0 },
64 new double[] { Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY },
65 new double[] { Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY }));
66 return pb;
67 }
68
69 @Test
70 public void testForward() {
71 doTest(0x292b6e87436fe4c7l, 0.0, 1.2, 3.3 * SIGMA);
72 }
73
74 @Test
75 public void testBackward() {
76 doTest(0x2f3285aa70b83c47l, 0.0, -1.0, 3.3 * SIGMA);
77 }
78
79 private Propagator buildPropagator() {
80 return EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
81 }
82
83 private void doTest(long seed, double startPeriod, double endPeriod, double tolerance) {
84 Generator generator = new Generator();
85 final int maxBurstSize = 10;
86 final double highRateStep = 5.0;
87 final double burstPeriod = 300.0;
88
89 generator.addPropagator(buildPropagator());
90 generator.addPropagator(buildPropagator());
91 ObservableSatellite satellite = generator.addPropagator(buildPropagator());
92 generator.addPropagator(buildPropagator());
93 generator.addScheduler(new ContinuousScheduler<>(getBuilder(new Well19937a(seed), satellite),
94 new BurstSelector(maxBurstSize, highRateStep, burstPeriod,
95 TimeScalesFactory.getUTC())));
96 final double period = context.initialOrbit.getKeplerianPeriod();
97 AbsoluteDate t0 = context.initialOrbit.getDate().shiftedBy(startPeriod * period);
98 AbsoluteDate t1 = context.initialOrbit.getDate().shiftedBy(endPeriod * period);
99 SortedSet<ObservedMeasurement<?>> measurements = generator.generate(t0, t1);
100 Propagator propagator = buildPropagator();
101 double maxError = 0;
102 AbsoluteDate previous = null;
103 AbsoluteDate tInf = t0.compareTo(t1) < 0 ? t0 : t1;
104 AbsoluteDate tSup = t0.compareTo(t1) < 0 ? t1 : t0;
105 int count = 0;
106 for (ObservedMeasurement<?> measurement : measurements) {
107 AbsoluteDate date = measurement.getDate();
108 double[] m = measurement.getObservedValue();
109 Assert.assertTrue(date.compareTo(tInf) >= 0);
110 Assert.assertTrue(date.compareTo(tSup) <= 0);
111 if (previous != null) {
112
113
114
115 final double expected = (count % maxBurstSize == 0) ?
116 burstPeriod - (maxBurstSize - 1) * highRateStep :
117 highRateStep;
118 Assert.assertEquals(expected, date.durationFrom(previous), 1.0e-10 * expected);
119 }
120 previous = date;
121 ++count;
122 SpacecraftState state = propagator.propagate(date);
123 double[] e = measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue();
124 for (int i = 0; i < m.length; ++i) {
125 maxError = FastMath.max(maxError, FastMath.abs(e[i] - m[i]));
126 }
127 }
128 Assert.assertEquals(0.0, maxError, tolerance);
129 }
130
131 @Before
132 public void setUp() {
133 context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
134
135 propagatorBuilder = context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
136 1.0e-6, 300.0, 0.001, Force.POTENTIAL,
137 Force.THIRD_BODY_SUN, Force.THIRD_BODY_MOON);
138 }
139
140 Context context;
141 NumericalPropagatorBuilder propagatorBuilder;
142
143 }