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.propagation.SpacecraftState;
28 import org.orekit.time.AbsoluteDate;
29 import org.orekit.utils.Constants;
30 import org.orekit.utils.ParameterDriver;
31
32 import java.util.Arrays;
33
34 public class RangeMeasurementCreator extends MeasurementCreator {
35
36 private final StationDataProvider provider;
37 private final Vector3D antennaPhaseCenter;
38 private final ObservableSatellite satellite;
39 private final double bias;
40
41 public RangeMeasurementCreator(final StationDataProvider context) {
42 this(context, 0.0);
43 }
44
45 public RangeMeasurementCreator(final StationDataProvider context, final double bias) {
46 this(context, Vector3D.ZERO, bias);
47 }
48
49 public RangeMeasurementCreator(final StationDataProvider provider, final Vector3D antennaPhaseCenter) {
50 this(provider, antennaPhaseCenter, 0.0);
51 }
52
53 public StationDataProvider getStationDataProvider() {
54 return provider;
55 }
56
57 public RangeMeasurementCreator(final StationDataProvider provider, final Vector3D antennaPhaseCenter, final double bias) {
58 this.provider = provider;
59 this.antennaPhaseCenter = antennaPhaseCenter;
60 this.satellite = new ObservableSatellite(0);
61 this.bias = bias;
62 }
63
64 public void init(SpacecraftState s0, AbsoluteDate t, double step) {
65 for (final GroundStation station : provider.getStations()) {
66 for (ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
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 public void handleStep(final SpacecraftState currentState) {
84 for (final GroundStation station : provider.getStations()) {
85 final AbsoluteDate date = currentState.getDate();
86 final Frame inertial = currentState.getFrame();
87 final Vector3D position = currentState.toTransform().getInverse().transformPosition(antennaPhaseCenter);
88
89 if (station.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0)) {
90 final double clockOffset = station.getClockOffsetDriver().getValue();
91 final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
92
93 final double downLinkDelay = solver.solve(1000, new UnivariateFunction() {
94 public double value(final double x) {
95 final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(clockOffset + x));
96 final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
97 return d - x * Constants.SPEED_OF_LIGHT;
98 }
99 }, -1.0, 1.0);
100 final AbsoluteDate receptionDate = currentState.getDate().shiftedBy(downLinkDelay);
101 final Vector3D stationAtReception =
102 station.getOffsetToInertial(inertial, receptionDate.shiftedBy(clockOffset)).transformPosition(Vector3D.ZERO);
103 final double downLinkDistance = Vector3D.distance(position, stationAtReception);
104
105 final double upLinkDelay = solver.solve(1000, new UnivariateFunction() {
106 public double value(final double x) {
107 final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(clockOffset - x));
108 final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
109 return d - x * Constants.SPEED_OF_LIGHT;
110 }
111 }, -1.0, 1.0);
112 final AbsoluteDate emissionDate = currentState.getDate().shiftedBy(-upLinkDelay);
113 final Vector3D stationAtEmission =
114 station.getOffsetToInertial(inertial, emissionDate.shiftedBy(clockOffset)).transformPosition(Vector3D.ZERO);
115 final double upLinkDistance = Vector3D.distance(position, stationAtEmission);
116 addMeasurement(new Range(station, true, receptionDate.shiftedBy(clockOffset),
117 0.5 * (downLinkDistance + upLinkDistance) + bias, 1.0, 10, satellite));
118 }
119
120 }
121 }
122
123 }