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.gnss.RadioWave;
27 import org.orekit.propagation.BoundedPropagator;
28 import org.orekit.propagation.SpacecraftState;
29 import org.orekit.time.AbsoluteDate;
30 import org.orekit.utils.Constants;
31
32 public class OneWayGNSSPhaseCreator extends MeasurementCreator {
33
34 private final BoundedPropagator ephemeris;
35 private final String remoteName;
36 private final QuadraticClockModel remoteClk;
37 private final double wavelength;
38 private final Vector3D antennaPhaseCenter1;
39 private final Vector3D antennaPhaseCenter2;
40 private final ObservableSatellite local;
41 private final AmbiguityCache cache;
42 private final AmbiguityDriver ambiguityDriver;
43
44 public OneWayGNSSPhaseCreator(final BoundedPropagator ephemeris,
45 final String remoteName,
46 final RadioWave radioWave,
47 final int ambiguity,
48 final double localClockOffset,
49 final double remoteClockOffset) {
50 this(ephemeris, remoteName, radioWave, ambiguity,
51 localClockOffset, remoteClockOffset, Vector3D.ZERO, Vector3D.ZERO);
52 }
53
54 public OneWayGNSSPhaseCreator(final BoundedPropagator ephemeris,
55 final String remoteName,
56 final RadioWave radioWave,
57 final int ambiguity,
58 final double localClockOffset,
59 final double remoteClockOffset,
60 final Vector3D antennaPhaseCenter1,
61 final Vector3D antennaPhaseCenter2) {
62 this.ephemeris = ephemeris;
63 this.remoteName = remoteName;
64 this.remoteClk = new QuadraticClockModel(ephemeris.getMinDate(),
65 remoteClockOffset, 0.0, 0.0);
66 this.antennaPhaseCenter1 = antennaPhaseCenter1;
67 this.antennaPhaseCenter2 = antennaPhaseCenter2;
68 this.wavelength = radioWave.getWavelength();
69 this.local = new ObservableSatellite(0);
70 this.local.getClockOffsetDriver().setValue(localClockOffset);
71 this.cache = new AmbiguityCache();
72 this.ambiguityDriver = cache.
73 getAmbiguity(remoteName,
74 "sat-" + local.getPropagatorIndex(),
75 wavelength);
76 ambiguityDriver.setValue(ambiguity);
77 }
78
79 public ObservableSatellite getLocalSatellite() {
80 return local;
81 }
82
83 public void init(final SpacecraftState s0, final AbsoluteDate t, final double step) {
84 if (local.getClockOffsetDriver().getReferenceDate() == null) {
85 local.getClockOffsetDriver().setReferenceDate(s0.getDate());
86 }
87 }
88
89 public void handleStep(final SpacecraftState currentState) {
90 try {
91 final double n = ambiguityDriver.getValue(currentState.getDate());
92 final double localClk = local.getClockOffsetDriver().getValue(currentState.getDate());
93 final double deltaD = Constants.SPEED_OF_LIGHT * (localClk -
94 remoteClk.getOffset(currentState.getDate()).getOffset());
95 final AbsoluteDate date = currentState.getDate();
96 final Vector3D position = currentState.toStaticTransform().getInverse().transformPosition(antennaPhaseCenter1);
97
98 final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
99
100 final double downLinkDelay = solver.solve(1000, x -> {
101 final Vector3D other = ephemeris.
102 propagate(date.shiftedBy(-x)).
103 toTransform().
104 getInverse().
105 transformPosition(antennaPhaseCenter2);
106 final double d = Vector3D.distance(position, other);
107 return d - x * Constants.SPEED_OF_LIGHT;
108 }, -1.0, 1.0);
109 final AbsoluteDate transitDate = currentState.getDate().shiftedBy(-downLinkDelay);
110 final Vector3D otherAtTransit =
111 ephemeris.propagate(transitDate).
112 toTransform().
113 getInverse().
114 transformPosition(antennaPhaseCenter2);
115 final double downLinkDistance = Vector3D.distance(position, otherAtTransit);
116
117
118 final OneWayGNSSPhase gnssPhase = new OneWayGNSSPhase(ephemeris, remoteName,
119 remoteClk, date.shiftedBy(localClk),
120 (downLinkDistance + deltaD) / wavelength + n, wavelength,
121 1.0, 10.0, local,
122 cache);
123 addMeasurement(gnssPhase);
124
125 } catch (OrekitException oe) {
126 throw new OrekitException(oe);
127 }
128 }
129
130 }