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