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 java.util.Arrays;
20
21 import org.hipparchus.analysis.UnivariateFunction;
22 import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
23 import org.hipparchus.analysis.solvers.UnivariateSolver;
24 import org.hipparchus.geometry.euclidean.threed.Vector3D;
25 import org.hipparchus.util.FastMath;
26 import org.orekit.estimation.Context;
27 import org.orekit.frames.Frame;
28 import org.orekit.frames.Transform;
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 public class RangeMeasurementCreator extends MeasurementCreator {
35
36 private final Context context;
37 private final Vector3D antennaPhaseCenter;
38 private final ObservableSatellite satellite;
39
40 public RangeMeasurementCreator(final Context context) {
41 this(context, Vector3D.ZERO);
42 }
43
44 public RangeMeasurementCreator(final Context context, final Vector3D antennaPhaseCenter) {
45 this.context = context;
46 this.antennaPhaseCenter = antennaPhaseCenter;
47 this.satellite = new ObservableSatellite(0);
48 }
49
50 public void init(SpacecraftState s0, AbsoluteDate t, double step) {
51 for (final GroundStation station : context.stations) {
52 for (ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
53 station.getEastOffsetDriver(),
54 station.getNorthOffsetDriver(),
55 station.getZenithOffsetDriver(),
56 station.getPrimeMeridianOffsetDriver(),
57 station.getPrimeMeridianDriftDriver(),
58 station.getPolarOffsetXDriver(),
59 station.getPolarDriftXDriver(),
60 station.getPolarOffsetYDriver(),
61 station.getPolarDriftYDriver())) {
62 if (driver.getReferenceDate() == null) {
63 driver.setReferenceDate(s0.getDate());
64 }
65 }
66 }
67 }
68
69 public void handleStep(final SpacecraftState currentState, final boolean isLast) {
70 for (final GroundStation station : context.stations) {
71 final AbsoluteDate date = currentState.getDate();
72 final Frame inertial = currentState.getFrame();
73 final Vector3D position = currentState.toTransform().getInverse().transformPosition(antennaPhaseCenter);
74
75 if (station.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0)) {
76 final double clockOffset = station.getClockOffsetDriver().getValue();
77 final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
78
79 final double downLinkDelay = solver.solve(1000, new UnivariateFunction() {
80 public double value(final double x) {
81 final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(clockOffset + x));
82 final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
83 return d - x * Constants.SPEED_OF_LIGHT;
84 }
85 }, -1.0, 1.0);
86 final AbsoluteDate receptionDate = currentState.getDate().shiftedBy(downLinkDelay);
87 final Vector3D stationAtReception =
88 station.getOffsetToInertial(inertial, receptionDate.shiftedBy(clockOffset)).transformPosition(Vector3D.ZERO);
89 final double downLinkDistance = Vector3D.distance(position, stationAtReception);
90
91 final double upLinkDelay = solver.solve(1000, new UnivariateFunction() {
92 public double value(final double x) {
93 final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(clockOffset - x));
94 final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
95 return d - x * Constants.SPEED_OF_LIGHT;
96 }
97 }, -1.0, 1.0);
98 final AbsoluteDate emissionDate = currentState.getDate().shiftedBy(-upLinkDelay);
99 final Vector3D stationAtEmission =
100 station.getOffsetToInertial(inertial, emissionDate.shiftedBy(clockOffset)).transformPosition(Vector3D.ZERO);
101 final double upLinkDistance = Vector3D.distance(position, stationAtEmission);
102 addMeasurement(new Range(station, true, receptionDate.shiftedBy(clockOffset),
103 0.5 * (downLinkDistance + upLinkDistance), 1.0, 10, satellite));
104 }
105
106 }
107 }
108
109 }