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.estimation.measurements.QuadraticClockModel;
26 import org.orekit.propagation.BoundedPropagator;
27 import org.orekit.propagation.SpacecraftState;
28 import org.orekit.time.AbsoluteDate;
29 import org.orekit.time.ClockOffset;
30 import org.orekit.utils.Constants;
31 import org.orekit.utils.PVCoordinates;
32 import org.orekit.utils.ParameterDriver;
33
34 import java.util.Arrays;
35
36 public class OneWayGNSSRangeRateCreator
37 extends MeasurementCreator {
38
39 private final BoundedPropagator ephemeris;
40 private final QuadraticClockModel remoteClk;
41 private final Vector3D antennaPhaseCenter1;
42 private final Vector3D antennaPhaseCenter2;
43 private final ObservableSatellite local;
44
45 public OneWayGNSSRangeRateCreator(final BoundedPropagator ephemeris,
46 final double localClockOffset,
47 final double localClockRate,
48 final double localClockAcceleration,
49 final double remoteClockOffset,
50 final double remoteClockRate,
51 final double remoteClockAcceleration) {
52 this(ephemeris,
53 localClockOffset, localClockRate, localClockAcceleration,
54 remoteClockOffset, remoteClockRate, remoteClockAcceleration,
55 Vector3D.ZERO, Vector3D.ZERO);
56 }
57
58 public OneWayGNSSRangeRateCreator(final BoundedPropagator ephemeris,
59 final double localClockOffset,
60 final double localClockRate,
61 final double localClockAcceleration,
62 final double remoteClockOffset,
63 final double remoteClockRate,
64 final double remoteClockAcceleration,
65 final Vector3D antennaPhaseCenter1,
66 final Vector3D antennaPhaseCenter2) {
67 this.ephemeris = ephemeris;
68 this.antennaPhaseCenter1 = antennaPhaseCenter1;
69 this.antennaPhaseCenter2 = antennaPhaseCenter2;
70 this.local = new ObservableSatellite(0);
71 this.local.getClockOffsetDriver().setValue(localClockOffset);
72 this.local.getClockDriftDriver().setValue(localClockRate);
73 this.local.getClockAccelerationDriver().setValue(localClockAcceleration);
74 this.remoteClk = new QuadraticClockModel(ephemeris.getMinDate(),
75 remoteClockOffset,
76 remoteClockRate,
77 remoteClockAcceleration);
78 }
79
80 public ObservableSatellite getLocalSatellite() {
81 return local;
82 }
83
84 public void init(final SpacecraftState s0, final AbsoluteDate t, final double step) {
85 for (final ParameterDriver driver : Arrays.asList(local.getClockOffsetDriver(),
86 local.getClockDriftDriver(),
87 local.getClockAccelerationDriver())) {
88 if (driver.getReferenceDate() == null) {
89 driver.setReferenceDate(s0.getDate());
90 }
91 }
92 }
93
94 public void handleStep(final SpacecraftState currentState) {
95 try {
96 final AbsoluteDate date = currentState.getDate();
97 final PVCoordinates pv = currentState.toTransform().getInverse().
98 transformPVCoordinates(new PVCoordinates(antennaPhaseCenter1));
99 final ClockOffset localClk = local.getQuadraticClockModel().getOffset(date);
100
101 final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
102
103 final double downLinkDelay = solver.solve(1000, x -> {
104 final Vector3D other = ephemeris.
105 propagate(date.shiftedBy(-x)).
106 toTransform().
107 getInverse().
108 transformPosition(antennaPhaseCenter2);
109 final double d = Vector3D.distance(pv.getPosition(), other);
110 return d - x * Constants.SPEED_OF_LIGHT;
111 }, -1.0, 1.0);
112 final AbsoluteDate transitDate = currentState.getDate().shiftedBy(-downLinkDelay);
113 final PVCoordinates otherAtTransit = ephemeris.propagate(transitDate).
114 toTransform().
115 getInverse().
116 transformPVCoordinates(new PVCoordinates(antennaPhaseCenter2));
117 final PVCoordinates delta = new PVCoordinates(otherAtTransit, pv);
118 final double rangeRate = Vector3D.dotProduct(delta.getPosition().normalize(), delta.getVelocity()) +
119 Constants.SPEED_OF_LIGHT * (local.getQuadraticClockModel().getOffset(date).getRate() -
120 remoteClk.getOffset(transitDate).getRate());
121
122
123 addMeasurement(new OneWayGNSSRangeRate(ephemeris, remoteClk, date.shiftedBy(localClk.getOffset()), rangeRate, 1.0, 10, local));
124
125 } catch (OrekitException oe) {
126 throw new OrekitException(oe);
127 }
128 }
129
130 }