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 java.util.Arrays;
20
21 import org.hipparchus.analysis.UnivariateFunction;
22 import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
23 import org.hipparchus.analysis.solvers.UnivariateSolver;
24 import org.hipparchus.geometry.euclidean.threed.Vector3D;
25 import org.hipparchus.util.FastMath;
26 import org.orekit.errors.OrekitException;
27 import org.orekit.estimation.Context;
28 import org.orekit.estimation.measurements.GroundStation;
29 import org.orekit.estimation.measurements.MeasurementCreator;
30 import org.orekit.estimation.measurements.ObservableSatellite;
31 import org.orekit.estimation.measurements.modifiers.PhaseAmbiguityModifier;
32 import org.orekit.frames.Frame;
33 import org.orekit.frames.Transform;
34 import org.orekit.gnss.Frequency;
35 import org.orekit.propagation.SpacecraftState;
36 import org.orekit.time.AbsoluteDate;
37 import org.orekit.utils.Constants;
38 import org.orekit.utils.ParameterDriver;
39
40 public class PhaseMeasurementCreator extends MeasurementCreator {
41
42 private final Context context;
43 private final double wavelength;
44 private final PhaseAmbiguityModifier ambiguity;
45 private final Vector3D antennaPhaseCenter;
46 private final ObservableSatellite satellite;
47
48 public PhaseMeasurementCreator(final Context context, final Frequency frequency,
49 final int ambiguity, final double satClockOffset) {
50 this(context, frequency, ambiguity, satClockOffset, Vector3D.ZERO);
51 }
52
53 public PhaseMeasurementCreator(final Context context, final Frequency frequency,
54 final int ambiguity, final double satClockOffset,
55 final Vector3D antennaPhaseCenter) {
56 this.context = context;
57 this.wavelength = frequency.getWavelength();
58 this.ambiguity = new PhaseAmbiguityModifier(0, ambiguity);
59 this.antennaPhaseCenter = antennaPhaseCenter;
60 this.satellite = new ObservableSatellite(0);
61 this.satellite.getClockOffsetDriver().setValue(satClockOffset);
62 }
63
64 public ObservableSatellite getSatellite() {
65 return satellite;
66 }
67
68 public void init(SpacecraftState s0, AbsoluteDate t, double step) {
69 for (final GroundStation station : context.stations) {
70 for (ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
71 station.getEastOffsetDriver(),
72 station.getNorthOffsetDriver(),
73 station.getZenithOffsetDriver(),
74 station.getPrimeMeridianOffsetDriver(),
75 station.getPrimeMeridianDriftDriver(),
76 station.getPolarOffsetXDriver(),
77 station.getPolarDriftXDriver(),
78 station.getPolarOffsetYDriver(),
79 station.getPolarDriftYDriver())) {
80 if (driver.getReferenceDate() == null) {
81 driver.setReferenceDate(s0.getDate());
82 }
83 }
84
85 }
86 if (satellite.getClockOffsetDriver().getReferenceDate() == null) {
87 satellite.getClockOffsetDriver().setReferenceDate(s0.getDate());
88 }
89 }
90
91 public void handleStep(final SpacecraftState currentState, final boolean isLast) {
92 try {
93 final double n = ambiguity.getParametersDrivers().get(0).getValue();
94 for (final GroundStation station : context.stations) {
95 final double groundClk = station.getClockOffsetDriver().getValue();
96 final double satClk = satellite.getClockOffsetDriver().getValue();
97 final double deltaD = Constants.SPEED_OF_LIGHT * (groundClk - satClk);
98 final AbsoluteDate date = currentState.getDate();
99 final Frame inertial = currentState.getFrame();
100 final Vector3D position = currentState.toTransform().getInverse().transformPosition(antennaPhaseCenter);
101
102 if (station.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0)) {
103 final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
104
105 final double downLinkDelay = solver.solve(1000, new UnivariateFunction() {
106 public double value(final double x) {
107 final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(groundClk + x));
108 final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
109 return d - x * Constants.SPEED_OF_LIGHT;
110 }
111 }, -1.0, 1.0);
112 final AbsoluteDate receptionDate = currentState.getDate().shiftedBy(downLinkDelay);
113 final Vector3D stationAtReception =
114 station.getOffsetToInertial(inertial, receptionDate.shiftedBy(groundClk)).transformPosition(Vector3D.ZERO);
115 final double downLinkDistance = Vector3D.distance(position, stationAtReception);
116
117 final Phase phase = new Phase(station, receptionDate.shiftedBy(groundClk),
118 (downLinkDistance + deltaD) / wavelength + n,
119 wavelength, 1.0, 10, satellite);
120 phase.addModifier(ambiguity);
121 addMeasurement(phase);
122 }
123
124 }
125 } catch (OrekitException oe) {
126 throw new OrekitException(oe);
127 }
128 }
129
130 }