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.PVCoordinates;
33 import org.orekit.utils.ParameterDriver;
34
35 public class RangeRateMeasurementCreator extends MeasurementCreator {
36
37 private final Context context;
38 private final boolean twoWay;
39 private final ObservableSatellite satellite;
40
41 public RangeRateMeasurementCreator(final Context 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.stations) {
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, final boolean isLast) {
77 for (final GroundStation station : context.stations) {
78 final double groundDft = station.getClockDriftDriver().getValue();
79 final double satDft = satellite.getClockDriftDriver().getValue();
80 final double deltaD = Constants.SPEED_OF_LIGHT * (groundDft - satDft);
81 final AbsoluteDate date = currentState.getDate();
82 final Frame inertial = currentState.getFrame();
83 final Vector3D position = currentState.getPVCoordinates().getPosition();
84 final Vector3D velocity = currentState.getPVCoordinates().getVelocity();
85
86 if (station.getBaseFrame().getElevation(position, inertial, date) > 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));
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).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));
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).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,
129 rr,
130 1.0, 10, twoWay, satellite));
131 }
132
133 }
134 }
135
136 }