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 org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
20 import org.hipparchus.analysis.solvers.UnivariateSolver;
21 import org.hipparchus.geometry.euclidean.threed.Vector3D;
22 import org.hipparchus.util.FastMath;
23 import org.orekit.errors.OrekitException;
24 import org.orekit.estimation.Context;
25 import org.orekit.estimation.measurements.GroundStation;
26 import org.orekit.estimation.measurements.MeasurementCreator;
27 import org.orekit.estimation.measurements.ObservableSatellite;
28 import org.orekit.frames.Frame;
29 import org.orekit.frames.Transform;
30 import org.orekit.gnss.RadioWave;
31 import org.orekit.gnss.antenna.PhaseCenterVariationFunction;
32 import org.orekit.propagation.SpacecraftState;
33 import org.orekit.time.AbsoluteDate;
34 import org.orekit.utils.Constants;
35 import org.orekit.utils.ParameterDriver;
36
37 import java.util.Arrays;
38
39 public class PhaseMeasurementCreator extends MeasurementCreator {
40
41 private final Context context;
42 private final double wavelength;
43 private final int ambiguity;
44 private final Vector3D stationMeanPosition;
45 private final PhaseCenterVariationFunction stationPhaseCenterVariation;
46 private final Vector3D satelliteMeanPosition;
47 private final PhaseCenterVariationFunction satellitePhaseCenterVariation;
48 private final ObservableSatellite satellite;
49 private final AmbiguityCache cache;
50
51 public PhaseMeasurementCreator(final Context context, final RadioWave radioWave,
52 final int ambiguity, final double satClockOffset) {
53 this(context, radioWave, ambiguity, satClockOffset,
54 Vector3D.ZERO, null, Vector3D.ZERO, null);
55 }
56
57 public PhaseMeasurementCreator(final Context context, final RadioWave radioWave,
58 final int ambiguity, final double satClockOffset,
59 final Vector3D stationMeanPosition, final PhaseCenterVariationFunction stationPhaseCenterVariation,
60 final Vector3D satelliteMeanPosition, final PhaseCenterVariationFunction satellitePhaseCenterVariation) {
61 this.context = context;
62 this.wavelength = radioWave.getWavelength();
63 this.ambiguity = ambiguity;
64 this.stationMeanPosition = stationMeanPosition;
65 this.stationPhaseCenterVariation = stationPhaseCenterVariation;
66 this.satelliteMeanPosition = satelliteMeanPosition;
67 this.satellitePhaseCenterVariation = satellitePhaseCenterVariation;
68 this.satellite = new ObservableSatellite(0);
69 this.satellite.getClockOffsetDriver().setValue(satClockOffset);
70 this.cache = new AmbiguityCache();
71 }
72
73 public ObservableSatellite getSatellite() {
74 return satellite;
75 }
76
77 public void init(SpacecraftState s0, AbsoluteDate t, double step) {
78 for (final GroundStation station : context.stations) {
79 for (ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
80 station.getClockDriftDriver(),
81 station.getEastOffsetDriver(),
82 station.getNorthOffsetDriver(),
83 station.getZenithOffsetDriver(),
84 station.getPrimeMeridianOffsetDriver(),
85 station.getPrimeMeridianDriftDriver(),
86 station.getPolarOffsetXDriver(),
87 station.getPolarDriftXDriver(),
88 station.getPolarOffsetYDriver(),
89 station.getPolarDriftYDriver())) {
90 if (driver.getReferenceDate() == null) {
91 driver.setReferenceDate(s0.getDate());
92 }
93 }
94
95 }
96 if (satellite.getClockOffsetDriver().getReferenceDate() == null) {
97 satellite.getClockOffsetDriver().setReferenceDate(s0.getDate());
98 }
99 }
100
101 public void handleStep(final SpacecraftState currentState) {
102 try {
103 for (final GroundStation station : context.stations) {
104 final AbsoluteDate date = currentState.getDate();
105 final Frame inertial = currentState.getFrame();
106 final Vector3D position = currentState.toStaticTransform().getInverse().transformPosition(satelliteMeanPosition);
107
108 if (station.getBaseFrame().getTrackingCoordinates(position, inertial, date).getElevation() > FastMath.toRadians(30.0)) {
109 final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
110
111 final double downLinkDelay = solver.solve(1000, x -> {
112 final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(x), true);
113 final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
114 return d - x * Constants.SPEED_OF_LIGHT;
115 }, -1.0, 1.0);
116 final AbsoluteDate receptionDate = currentState.getDate().shiftedBy(downLinkDelay);
117 final Transform stationToInertReception = station.getOffsetToInertial(inertial, receptionDate, true);
118 final Vector3D stationAtReception = stationToInertReception.transformPosition(stationMeanPosition);
119 final double downLinkDistance = Vector3D.distance(position, stationAtReception);
120
121 final Vector3D satLosDown = currentState.toTransform().
122 transformVector(stationAtReception.subtract(position));
123 final double satPCVDown = satellitePhaseCenterVariation == null ?
124 0.0 :
125 satellitePhaseCenterVariation.value(0.5 * FastMath.PI - satLosDown.getDelta(),
126 satLosDown.getAlpha());
127 final Vector3D staLosDown = stationToInertReception.getInverse().
128 transformVector(position.subtract(stationAtReception));
129 final double staPCVDown = stationPhaseCenterVariation == null ?
130 0.0 :
131 stationPhaseCenterVariation.value(0.5 * FastMath.PI - staLosDown.getDelta(),
132 staLosDown.getAlpha());
133
134 final double groundClk = station.getClockOffsetDriver().getValue(date);
135 final double satClk = satellite.getClockOffsetDriver().getValue(date);
136 final double correctedDownLinkDistance = downLinkDistance + satPCVDown + staPCVDown +
137 (groundClk - satClk) * Constants.SPEED_OF_LIGHT;
138 final Phase phase = new Phase(station, receptionDate.shiftedBy(groundClk),
139 correctedDownLinkDistance / wavelength + ambiguity,
140 wavelength, 1.0, 10, satellite,
141 cache);
142 phase.getAmbiguityDriver().setValue(ambiguity);
143 addMeasurement(phase);
144 }
145
146 }
147 } catch (OrekitException oe) {
148 throw new OrekitException(oe);
149 }
150 }
151
152 }