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.PVCoordinates;
31 import org.orekit.utils.ParameterDriver;
32
33 import java.util.Arrays;
34
35 public class RangeRateMeasurementCreator extends MeasurementCreator {
36
37 private final StationDataProvider context;
38 private final boolean twoWay;
39 private final ObservableSatellite satellite;
40
41 public RangeRateMeasurementCreator(final StationDataProvider context, boolean twoWay,
42 final double satClockDrift) {
43 this.context = context;
44 this.twoWay = twoWay;
45 this.satellite = new ObservableSatellite(0);
46 this.satellite.getClockDriftDriver().setValue(satClockDrift);
47 }
48
49 public ObservableSatellite getSatellite() {
50 return satellite;
51 }
52
53 public void init(SpacecraftState s0, AbsoluteDate t, double step) {
54 for (final GroundStation station : context.getStations()) {
55 for (ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
56 station.getClockDriftDriver(),
57 station.getEastOffsetDriver(),
58 station.getNorthOffsetDriver(),
59 station.getZenithOffsetDriver(),
60 station.getPrimeMeridianOffsetDriver(),
61 station.getPrimeMeridianDriftDriver(),
62 station.getPolarOffsetXDriver(),
63 station.getPolarDriftXDriver(),
64 station.getPolarOffsetYDriver(),
65 station.getPolarDriftYDriver())) {
66 if (driver.getReferenceDate() == null) {
67 driver.setReferenceDate(s0.getDate());
68 }
69 }
70 }
71 if (satellite.getClockDriftDriver().getReferenceDate() == null) {
72 satellite.getClockDriftDriver().setReferenceDate(s0.getDate());
73 }
74 }
75
76 public void handleStep(final SpacecraftState currentState) {
77 for (final GroundStation station : context.getStations()) {
78 final AbsoluteDate date = currentState.getDate();
79 final Frame inertial = currentState.getFrame();
80 final Vector3D position = currentState.getPosition();
81 final Vector3D velocity = currentState.getPVCoordinates().getVelocity();
82 final double groundDft = station.getClockDriftDriver().getValue(date);
83 final double satDft = satellite.getClockDriftDriver().getValue(date);
84 final double deltaD = Constants.SPEED_OF_LIGHT * (groundDft - satDft);
85
86 if (station.getBaseFrame().getTrackingCoordinates(position, inertial, date).getElevation() > FastMath.toRadians(30.0)) {
87 final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
88
89 final double downLinkDelay = solver.solve(1000, new UnivariateFunction() {
90 public double value(final double x) {
91 final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(x), false);
92 final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
93 return d - x * Constants.SPEED_OF_LIGHT;
94 }
95 }, -1.0, 1.0);
96 final AbsoluteDate receptionDate = currentState.getDate().shiftedBy(downLinkDelay);
97 final PVCoordinates stationAtReception =
98 station.getOffsetToInertial(inertial, receptionDate, false).transformPVCoordinates(PVCoordinates.ZERO);
99
100
101 final Vector3D receptionLOS = (position.subtract(stationAtReception.getPosition())).normalize();
102
103
104 final Vector3D deltaVr = velocity.subtract(stationAtReception.getVelocity());
105
106 final double upLinkDelay = solver.solve(1000, new UnivariateFunction() {
107 public double value(final double x) {
108 final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(-x), false);
109 final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
110 return d - x * Constants.SPEED_OF_LIGHT;
111 }
112 }, -1.0, 1.0);
113 final AbsoluteDate emissionDate = currentState.getDate().shiftedBy(-upLinkDelay);
114 final PVCoordinates stationAtEmission =
115 station.getOffsetToInertial(inertial, emissionDate, false).transformPVCoordinates(PVCoordinates.ZERO);
116
117
118 final Vector3D emissionLOS = (position.subtract(stationAtEmission.getPosition())).normalize();
119
120
121 final Vector3D deltaVe = velocity.subtract(stationAtEmission.getVelocity());
122
123
124 final double rr = twoWay ?
125 0.5 * (deltaVr.dotProduct(receptionLOS) + deltaVe.dotProduct(emissionLOS)) :
126 deltaVr.dotProduct(receptionLOS) + deltaD;
127
128 addMeasurement(new RangeRate(station, receptionDate, rr, 1.0, 10, twoWay, satellite));
129 }
130
131 }
132 }
133
134 }