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