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