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.UnivariateFunction;
20 import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
21 import org.hipparchus.analysis.solvers.UnivariateSolver;
22 import org.hipparchus.geometry.euclidean.threed.Vector3D;
23 import org.orekit.errors.OrekitException;
24 import org.orekit.estimation.measurements.MeasurementCreator;
25 import org.orekit.estimation.measurements.ObservableSatellite;
26 import org.orekit.estimation.measurements.modifiers.InterSatellitesPhaseAmbiguityModifier;
27 import org.orekit.gnss.Frequency;
28 import org.orekit.propagation.BoundedPropagator;
29 import org.orekit.propagation.SpacecraftState;
30 import org.orekit.time.AbsoluteDate;
31 import org.orekit.utils.Constants;
32
33 public class InterSatellitesPhaseMeasurementCreator extends MeasurementCreator {
34
35 private final BoundedPropagator ephemeris;
36 private final double wavelength;
37 private final InterSatellitesPhaseAmbiguityModifier ambiguity;
38 private final Vector3D antennaPhaseCenter1;
39 private final Vector3D antennaPhaseCenter2;
40 private final ObservableSatellite local;
41 private final ObservableSatellite remote;
42
43 public InterSatellitesPhaseMeasurementCreator(final BoundedPropagator ephemeris,
44 final Frequency frequency,
45 final int ambiguity,
46 final double localClockOffset,
47 final double remoteClockOffset) {
48 this(ephemeris, frequency, ambiguity, localClockOffset, remoteClockOffset, Vector3D.ZERO, Vector3D.ZERO);
49 }
50
51 public InterSatellitesPhaseMeasurementCreator(final BoundedPropagator ephemeris,
52 final Frequency frequency,
53 final int ambiguity,
54 final double localClockOffset,
55 final double remoteClockOffset,
56 final Vector3D antennaPhaseCenter1,
57 final Vector3D antennaPhaseCenter2) {
58 this.ephemeris = ephemeris;
59 this.wavelength = frequency.getWavelength();
60 this.ambiguity = new InterSatellitesPhaseAmbiguityModifier(0, ambiguity);
61 this.antennaPhaseCenter1 = antennaPhaseCenter1;
62 this.antennaPhaseCenter2 = antennaPhaseCenter2;
63 this.local = new ObservableSatellite(0);
64 this.local.getClockOffsetDriver().setValue(localClockOffset);
65 this.remote = new ObservableSatellite(1);
66 this.remote.getClockOffsetDriver().setValue(remoteClockOffset);
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 double n = ambiguity.getParametersDrivers().get(0).getValue();
89 final double remoteClk = remote.getClockOffsetDriver().getValue();
90 final double localClk = local.getClockOffsetDriver().getValue();
91 final double deltaD = Constants.SPEED_OF_LIGHT * (localClk - remoteClk);
92 final AbsoluteDate date = currentState.getDate();
93 final Vector3D position = currentState.toTransform().getInverse().transformPosition(antennaPhaseCenter1);
94
95 final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
96
97 final double downLinkDelay = solver.solve(1000, new UnivariateFunction() {
98 public double value(final double x) {
99 final Vector3D other = ephemeris.
100 propagate(date.shiftedBy(-x)).
101 toTransform().
102 getInverse().
103 transformPosition(antennaPhaseCenter2);
104 final double d = Vector3D.distance(position, other);
105 return d - x * Constants.SPEED_OF_LIGHT;
106 }
107 }, -1.0, 1.0);
108 final AbsoluteDate transitDate = currentState.getDate().shiftedBy(-downLinkDelay);
109 final Vector3D otherAtTransit =
110 ephemeris.propagate(transitDate).
111 toTransform().
112 getInverse().
113 transformPosition(antennaPhaseCenter2);
114 final double downLinkDistance = Vector3D.distance(position, otherAtTransit);
115
116
117 final InterSatellitesPhase phase = new InterSatellitesPhase(local, remote, date.shiftedBy(localClk), (downLinkDistance + deltaD) / wavelength + n, wavelength, 1.0, 10);
118 phase.addModifier(ambiguity);
119 addMeasurement(phase);
120
121 } catch (OrekitException oe) {
122 throw new OrekitException(oe);
123 }
124 }
125
126 }