1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.measurements.gnss;
18
19 import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
20 import org.hipparchus.analysis.solvers.UnivariateSolver;
21 import org.hipparchus.geometry.euclidean.threed.Vector3D;
22 import org.orekit.errors.OrekitException;
23 import org.orekit.estimation.measurements.MeasurementCreator;
24 import org.orekit.estimation.measurements.ObservableSatellite;
25 import org.orekit.propagation.BoundedPropagator;
26 import org.orekit.propagation.SpacecraftState;
27 import org.orekit.time.AbsoluteDate;
28 import org.orekit.time.ClockOffset;
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 InterSatellitesOneWayRangeRateMeasurementCreator
36 extends MeasurementCreator {
37
38 private final BoundedPropagator ephemeris;
39 private final Vector3D antennaPhaseCenter1;
40 private final Vector3D antennaPhaseCenter2;
41 private final ObservableSatellite local;
42 private final ObservableSatellite remote;
43
44 public InterSatellitesOneWayRangeRateMeasurementCreator(final BoundedPropagator ephemeris,
45 final double localClockOffset,
46 final double localClockRate,
47 final double localClockAcceleration,
48 final double remoteClockOffset,
49 final double remoteClockRate,
50 final double remoteClockAcceleration) {
51 this(ephemeris,
52 localClockOffset, localClockRate, localClockAcceleration,
53 remoteClockOffset, remoteClockRate, remoteClockAcceleration,
54 Vector3D.ZERO, Vector3D.ZERO);
55 }
56
57 public InterSatellitesOneWayRangeRateMeasurementCreator(final BoundedPropagator ephemeris,
58 final double localClockOffset,
59 final double localClockRate,
60 final double localClockAcceleration,
61 final double remoteClockOffset,
62 final double remoteClockRate,
63 final double remoteClockAcceleration,
64 final Vector3D antennaPhaseCenter1,
65 final Vector3D antennaPhaseCenter2) {
66 this.ephemeris = ephemeris;
67 this.antennaPhaseCenter1 = antennaPhaseCenter1;
68 this.antennaPhaseCenter2 = antennaPhaseCenter2;
69 this.local = new ObservableSatellite(0);
70 this.local.getClockOffsetDriver().setValue(localClockOffset);
71 this.local.getClockDriftDriver().setValue(localClockRate);
72 this.local.getClockAccelerationDriver().setValue(localClockAcceleration);
73 this.remote = new ObservableSatellite(1);
74 this.remote.getClockOffsetDriver().setValue(remoteClockOffset);
75 this.remote.getClockDriftDriver().setValue(remoteClockRate);
76 this.remote.getClockAccelerationDriver().setValue(remoteClockAcceleration);
77 }
78
79 public ObservableSatellite getLocalSatellite() {
80 return local;
81 }
82
83 public ObservableSatellite getRemoteSatellite() {
84 return remote;
85 }
86
87 public void init(final SpacecraftState s0, final AbsoluteDate t, final double step) {
88 for (final ParameterDriver driver : Arrays.asList(local.getClockOffsetDriver(),
89 local.getClockDriftDriver(),
90 local.getClockAccelerationDriver(),
91 remote.getClockOffsetDriver(),
92 remote.getClockDriftDriver(),
93 remote.getClockAccelerationDriver())) {
94 if (driver.getReferenceDate() == null) {
95 driver.setReferenceDate(s0.getDate());
96 }
97 }
98 }
99
100 public void handleStep(final SpacecraftState currentState) {
101 try {
102 final AbsoluteDate date = currentState.getDate();
103 final PVCoordinates pv = currentState.toTransform().getInverse().
104 transformPVCoordinates(new PVCoordinates(antennaPhaseCenter1));
105 final ClockOffset localClk = local.getQuadraticClockModel().getOffset(date);
106
107 final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
108
109 final double downLinkDelay = solver.solve(1000, x -> {
110 final Vector3D other = ephemeris.
111 propagate(date.shiftedBy(-x)).
112 toTransform().
113 getInverse().
114 transformPosition(antennaPhaseCenter2);
115 final double d = Vector3D.distance(pv.getPosition(), other);
116 return d - x * Constants.SPEED_OF_LIGHT;
117 }, -1.0, 1.0);
118 final AbsoluteDate transitDate = currentState.getDate().shiftedBy(-downLinkDelay);
119 final PVCoordinates otherAtTransit =
120 ephemeris.propagate(transitDate).
121 toTransform().
122 getInverse().
123 transformPVCoordinates(new PVCoordinates(antennaPhaseCenter2));
124 final PVCoordinates delta = new PVCoordinates(otherAtTransit, pv);
125 final double rangeRate = Vector3D.dotProduct(delta.getPosition().normalize(), delta.getVelocity()) +
126 Constants.SPEED_OF_LIGHT * (local.getQuadraticClockModel().getOffset(date).getRate() -
127 remote.getQuadraticClockModel().getOffset(transitDate).getRate());
128
129
130 final InterSatellitesOneWayRangeRate phase = new InterSatellitesOneWayRangeRate(local, remote,
131 date.shiftedBy(localClk.getOffset()),
132 rangeRate,
133 1.0, 10);
134 addMeasurement(phase);
135
136 } catch (OrekitException oe) {
137 throw new OrekitException(oe);
138 }
139 }
140
141 }