1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.measurements.gnss;
18
19 import java.util.SortedSet;
20
21 import org.hipparchus.geometry.euclidean.threed.Vector3D;
22 import org.hipparchus.util.FastMath;
23 import org.hipparchus.util.MathUtils;
24 import org.junit.jupiter.api.AfterEach;
25 import org.junit.jupiter.api.Assertions;
26 import org.junit.jupiter.api.BeforeEach;
27 import org.junit.jupiter.api.Test;
28 import org.orekit.Utils;
29 import org.orekit.attitudes.AttitudeProvider;
30 import org.orekit.bodies.CelestialBodyFactory;
31 import org.orekit.bodies.GeodeticPoint;
32 import org.orekit.bodies.OneAxisEllipsoid;
33 import org.orekit.estimation.EstimationTestUtils;
34 import org.orekit.estimation.measurements.EstimatedMeasurementBase;
35 import org.orekit.estimation.measurements.GroundStation;
36 import org.orekit.estimation.measurements.ObservableSatellite;
37 import org.orekit.estimation.measurements.generation.EventBasedScheduler;
38 import org.orekit.estimation.measurements.generation.GatheringSubscriber;
39 import org.orekit.estimation.measurements.generation.Generator;
40 import org.orekit.estimation.measurements.generation.SignSemantic;
41 import org.orekit.frames.FramesFactory;
42 import org.orekit.frames.TopocentricFrame;
43 import org.orekit.gnss.PredefinedGnssSignal;
44 import org.orekit.gnss.SatelliteSystem;
45 import org.orekit.gnss.attitude.GPSBlockIIA;
46 import org.orekit.gnss.attitude.GPSBlockIIR;
47 import org.orekit.orbits.CartesianOrbit;
48 import org.orekit.orbits.Orbit;
49 import org.orekit.propagation.Propagator;
50 import org.orekit.propagation.SpacecraftState;
51 import org.orekit.propagation.analytical.KeplerianPropagator;
52 import org.orekit.propagation.events.handlers.ContinueOnEvent;
53 import org.orekit.time.AbsoluteDate;
54 import org.orekit.time.FixedStepSelector;
55 import org.orekit.time.GNSSDate;
56 import org.orekit.time.TimeScalesFactory;
57 import org.orekit.utils.Constants;
58 import org.orekit.utils.IERSConventions;
59 import org.orekit.utils.TimeStampedPVCoordinates;
60
61 public class WindUpTest {
62
63 @Test
64 public void testYawSteering() {
65
66
67
68 doTest(new CartesianOrbit(new TimeStampedPVCoordinates(new GNSSDate(1206, 307052.670, SatelliteSystem.GPS).getDate(),
69 new Vector3D( 8759594.455119, 12170903.262908, 21973798.932235),
70 new Vector3D(-2957.570165356, 2478.252315039, -263.042027935)),
71 FramesFactory.getGCRF(),
72 Constants.EIGEN5C_EARTH_MU),
73 new GPSBlockIIA(GPSBlockIIA.getDefaultYawRate(17), GPSBlockIIA.DEFAULT_YAW_BIAS,
74 AbsoluteDate.PAST_INFINITY, AbsoluteDate.FUTURE_INFINITY,
75 CelestialBodyFactory.getSun(), FramesFactory.getGCRF()),
76 SatelliteSystem.GPS, 17,
77 new GroundStation(new TopocentricFrame(earth,
78 new GeodeticPoint(FastMath.toRadians(55.0 + ( 1.0 + 10.0 / 60.0) / 60.0),
79 FastMath.toRadians(82.0 + (55.0 + 22.0 / 60.0) / 60.0),
80 160.0),
81 "Новосибирск")),
82 0.476338, 0.500405);
83 }
84
85 @Test
86 public void testMidnightTurn() {
87
88
89
90
91
92
93
94 doTest(new CartesianOrbit(new TimeStampedPVCoordinates(new GNSSDate(1218, 287890.543, SatelliteSystem.GPS).getDate(),
95 new Vector3D(-17920092.444521, -11889104.443797, -15318905.173501),
96 new Vector3D( 231.983556337, -3232.849996931, 2163.378049467)),
97 FramesFactory.getGCRF(),
98 Constants.EIGEN5C_EARTH_MU),
99 new GPSBlockIIA(GPSBlockIIA.getDefaultYawRate(7), GPSBlockIIA.DEFAULT_YAW_BIAS,
100 AbsoluteDate.PAST_INFINITY, AbsoluteDate.FUTURE_INFINITY,
101 CelestialBodyFactory.getSun(), FramesFactory.getGCRF()),
102 SatelliteSystem.GPS, 7,
103 new GroundStation(new TopocentricFrame(earth,
104 new GeodeticPoint(FastMath.toRadians( -(25.0 + 4.0 / 60.0)),
105 FastMath.toRadians(-(130.0 + 6.0 / 60.0)),
106 0.0),
107 "Adamstown")),
108 -1.853178, -0.361900);
109 }
110
111 @Test
112 public void testNoonTurn() {
113
114
115
116 doTest(new CartesianOrbit(new TimeStampedPVCoordinates(new GNSSDate(1225, 509000.063, SatelliteSystem.GPS).getDate(),
117 new Vector3D( 2297608.196826, 20928500.842189, 16246321.092008),
118 new Vector3D(-2810.598090399, 1819.511241767, -1939.009527296)),
119 FramesFactory.getGCRF(),
120 Constants.EIGEN5C_EARTH_MU),
121 new GPSBlockIIR(GPSBlockIIR.DEFAULT_YAW_RATE,
122 AbsoluteDate.PAST_INFINITY, AbsoluteDate.FUTURE_INFINITY,
123 CelestialBodyFactory.getSun(), FramesFactory.getGCRF()),
124 SatelliteSystem.GPS, 11,
125 new GroundStation(new TopocentricFrame(earth,
126 new GeodeticPoint(FastMath.toRadians( 19.0 + (49.0 + 20.0 / 60.0) / 60.0),
127 FastMath.toRadians(-(155.0 + (28.0 + 30.0 / 60.0) / 60.0)),
128 4205.0),
129 "Mauna Kea")),
130 -0.105573, 0.351411);
131 }
132
133 private void doTest(final Orbit orbit, final AttitudeProvider attitudeProvider,
134 final SatelliteSystem system, final int prn, final GroundStation station,
135 final double expectedMin, final double expectedMax) {
136
137
138 Generator generator = new Generator();
139 ObservableSatellite obsSat = generator.addPropagator(new KeplerianPropagator(orbit, attitudeProvider));
140 PhaseBuilder builder = new PhaseBuilder(null, station,
141 PredefinedGnssSignal.G01.getWavelength(),
142 0.01 * PredefinedGnssSignal.G01.getWavelength(),
143 1.0, obsSat,
144 new AmbiguityCache());
145 generator.addScheduler(new EventBasedScheduler<>(builder,
146 new FixedStepSelector(60.0, TimeScalesFactory.getUTC()),
147 generator.getPropagator(obsSat),
148 EstimationTestUtils.getElevationDetector(station.getBaseFrame(),
149 FastMath.toRadians(5.0)).
150 withHandler(new ContinueOnEvent()),
151 SignSemantic.FEASIBLE_MEASUREMENT_WHEN_POSITIVE));
152 final GatheringSubscriber gatherer = new GatheringSubscriber();
153 generator.addSubscriber(gatherer);
154 generator.generate(orbit.getDate(), orbit.getDate().shiftedBy(7200));
155 SortedSet<EstimatedMeasurementBase<?>> measurements = gatherer.getGeneratedMeasurements();
156 Assertions.assertEquals(120, measurements.size());
157
158 WindUp windUp = new WindUpFactory().getWindUp(system, prn, Dipole.CANONICAL_I_J, station.getBaseFrame().getName());
159 Propagator propagator = new KeplerianPropagator(orbit, attitudeProvider);
160 double min = Double.POSITIVE_INFINITY;
161 double max = Double.NEGATIVE_INFINITY;
162 for (EstimatedMeasurementBase<?> m : measurements) {
163 Phase phase = (Phase) m.getObservedMeasurement();
164 @SuppressWarnings("unchecked")
165 EstimatedMeasurementBase<Phase> estimated = (EstimatedMeasurementBase<Phase>) m.
166 getObservedMeasurement().
167 estimateWithoutDerivatives(new SpacecraftState[] {
168 propagator.propagate(phase.getDate())
169 });
170 final double original = estimated.getEstimatedValue()[0];
171 windUp.modifyWithoutDerivatives(estimated);
172 final double modified = estimated.getEstimatedValue()[0];
173 final double correction = modified - original;
174 Assertions.assertEquals(correction, windUp.getAngularWindUp() / MathUtils.TWO_PI, 1.0e-5);
175 min = FastMath.min(min, correction);
176 max = FastMath.max(max, correction);
177 Assertions.assertEquals(1,
178 estimated.getAppliedEffects().entrySet().stream().
179 filter(e -> e.getKey().getEffectName().equals("wind-up")).count());
180 }
181 Assertions.assertEquals(expectedMin, min, 1.0e-5);
182 Assertions.assertEquals(expectedMax, max, 1.0e-5);
183
184 }
185
186 @BeforeEach
187 public void setUp() {
188 Utils.setDataRoot("regular-data");
189 earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
190 Constants.WGS84_EARTH_FLATTENING,
191 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
192 }
193
194 @AfterEach
195 public void tearDown() {
196 earth = null;
197 }
198
199 private OneAxisEllipsoid earth;
200
201 }