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