1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.measurements;
18
19 import org.hipparchus.analysis.UnivariateFunction;
20 import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
21 import org.hipparchus.analysis.solvers.UnivariateSolver;
22 import org.hipparchus.geometry.euclidean.threed.Vector3D;
23 import org.hipparchus.util.FastMath;
24 import org.orekit.estimation.StationDataProvider;
25 import org.orekit.frames.Frame;
26 import org.orekit.frames.Transform;
27 import org.orekit.gnss.antenna.PhaseCenterVariationFunction;
28 import org.orekit.propagation.SpacecraftState;
29 import org.orekit.time.AbsoluteDate;
30 import org.orekit.utils.Constants;
31 import org.orekit.utils.ParameterDriver;
32
33 import java.util.Arrays;
34
35 public class TwoWayRangeMeasurementCreator extends MeasurementCreator {
36
37 private final StationDataProvider provider;
38 private final Vector3D stationMeanPosition;
39 private final PhaseCenterVariationFunction stationPhaseCenterVariation;
40 private final Vector3D satelliteMeanPosition;
41 private final PhaseCenterVariationFunction satellitePhaseCenterVariation;
42 private final ObservableSatellite satellite;
43 private final double bias;
44
45 public TwoWayRangeMeasurementCreator(final StationDataProvider context) {
46 this(context, Vector3D.ZERO, null, Vector3D.ZERO, null, 0.0);
47 }
48
49 public TwoWayRangeMeasurementCreator(final StationDataProvider provider,
50 final Vector3D stationMeanPosition, final PhaseCenterVariationFunction stationPhaseCenterVariation,
51 final Vector3D satelliteMeanPosition, final PhaseCenterVariationFunction satellitePhaseCenterVariation,
52 final double bias) {
53 this.provider = provider;
54 this.stationMeanPosition = stationMeanPosition;
55 this.stationPhaseCenterVariation = stationPhaseCenterVariation;
56 this.satelliteMeanPosition = satelliteMeanPosition;
57 this.satellitePhaseCenterVariation = satellitePhaseCenterVariation;
58 this.satellite = new ObservableSatellite(0);
59 this.bias = bias;
60 }
61
62 public StationDataProvider getStationDataProvider() {
63 return provider;
64 }
65
66 public void init(SpacecraftState s0, AbsoluteDate t, double step) {
67 for (final GroundStation station : provider.getStations()) {
68 for (ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
69 station.getClockDriftDriver(),
70 station.getEastOffsetDriver(),
71 station.getNorthOffsetDriver(),
72 station.getZenithOffsetDriver(),
73 station.getPrimeMeridianOffsetDriver(),
74 station.getPrimeMeridianDriftDriver(),
75 station.getPolarOffsetXDriver(),
76 station.getPolarDriftXDriver(),
77 station.getPolarOffsetYDriver(),
78 station.getPolarDriftYDriver())) {
79 if (driver.getReferenceDate() == null) {
80 driver.setReferenceDate(s0.getDate());
81 }
82 }
83 }
84 }
85
86 public void handleStep(final SpacecraftState currentState) {
87 for (final GroundStation station : provider.getStations()) {
88 final AbsoluteDate date = currentState.getDate();
89 final Frame inertial = currentState.getFrame();
90 final Vector3D position = currentState.toStaticTransform().getInverse().transformPosition(satelliteMeanPosition);
91
92 if (station.getBaseFrame().getTrackingCoordinates(position, inertial, date).getElevation() > FastMath.toRadians(30.0)) {
93 final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
94
95 final double downLinkDelay = solver.solve(1000, new UnivariateFunction() {
96 public double value(final double x) {
97 final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(x), true);
98 final double d = Vector3D.distance(position, t.transformPosition(stationMeanPosition));
99 return d - x * Constants.SPEED_OF_LIGHT;
100 }
101 }, -1.0, 1.0);
102 final AbsoluteDate receptionDate = currentState.getDate().shiftedBy(downLinkDelay);
103 final Transform stationToInertReception = station.getOffsetToInertial(inertial, receptionDate, true);
104 final Vector3D stationAtReception = stationToInertReception.transformPosition(stationMeanPosition);
105 final double downLinkDistance = Vector3D.distance(position, stationAtReception);
106
107 final Vector3D satLosDown = currentState.toTransform().
108 transformVector(stationAtReception.subtract(position));
109 final double satPCVDown = satellitePhaseCenterVariation == null ?
110 0.0 :
111 satellitePhaseCenterVariation.value(0.5 * FastMath.PI - satLosDown.getDelta(),
112 satLosDown.getAlpha());
113 final Vector3D staLosDown = stationToInertReception.getInverse().
114 transformVector(position.subtract(stationAtReception));
115 final double staPCVDown = stationPhaseCenterVariation == null ?
116 0.0 :
117 stationPhaseCenterVariation.value(0.5 * FastMath.PI - staLosDown.getDelta(),
118 staLosDown.getAlpha());
119
120 final double correctedDownLinkDistance = downLinkDistance + satPCVDown + staPCVDown;
121
122 final double upLinkDelay = solver.solve(1000, new UnivariateFunction() {
123 public double value(final double x) {
124 final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(-x), true);
125 final double d = Vector3D.distance(position, t.transformPosition(stationMeanPosition));
126 return d - x * Constants.SPEED_OF_LIGHT;
127 }
128 }, -1.0, 1.0);
129 final AbsoluteDate emissionDate = currentState.getDate().shiftedBy(-upLinkDelay);
130 final Transform stationToInertEmission = station.getOffsetToInertial(inertial, emissionDate, true);
131 final Vector3D stationAtEmission = stationToInertEmission.transformPosition(stationMeanPosition);
132 final double upLinkDistance = Vector3D.distance(position, stationAtEmission);
133
134 final Vector3D staLosUp = stationToInertEmission.getInverse().
135 transformVector(position.subtract(stationAtEmission));
136 final double staPCVUp = stationPhaseCenterVariation == null ?
137 0.0 :
138 stationPhaseCenterVariation.value(0.5 * FastMath.PI - staLosUp.getDelta(),
139 staLosUp.getAlpha());
140 final Vector3D satLosUp = currentState.toTransform().
141 transformVector(stationAtEmission.subtract(position));
142 final double satPCVUp = satellitePhaseCenterVariation == null ?
143 0.0 :
144 satellitePhaseCenterVariation.value(0.5 * FastMath.PI - satLosUp.getDelta(),
145 satLosUp.getAlpha());
146
147 final double correctedUpLinkDistance = upLinkDistance + satPCVUp + staPCVUp;
148
149 final double clockOffset = station.getClockOffsetDriver().getValue(date);
150 addMeasurement(new Range(station, true, receptionDate.shiftedBy(clockOffset),
151 0.5 * (correctedDownLinkDistance + correctedUpLinkDistance) + bias,
152 1.0, 10, satellite));
153
154 }
155
156 }
157 }
158
159 }