1 /* Copyright 2002-2020 CS GROUP
2 * Licensed to CS GROUP (CS) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * CS licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.orekit.estimation.measurements;
18
19 import java.util.Arrays;
20 import java.util.Map;
21
22 import org.hipparchus.analysis.UnivariateFunction;
23 import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
24 import org.hipparchus.analysis.solvers.UnivariateSolver;
25 import org.hipparchus.geometry.euclidean.threed.Vector3D;
26 import org.hipparchus.util.FastMath;
27 import org.orekit.errors.OrekitException;
28 import org.orekit.estimation.Context;
29 import org.orekit.frames.Frame;
30 import org.orekit.frames.Transform;
31 import org.orekit.propagation.SpacecraftState;
32 import org.orekit.time.AbsoluteDate;
33 import org.orekit.utils.Constants;
34 import org.orekit.utils.PVCoordinates;
35 import org.orekit.utils.ParameterDriver;
36 import org.orekit.utils.TimeStampedPVCoordinates;
37 /**
38 * Class creating a list of turn-around range measurement
39 * @author Maxime Journot
40 *
41 */
42 public class TurnAroundRangeMeasurementCreator extends MeasurementCreator {
43
44 private final Context context;
45 private final Vector3D antennaPhaseCenter;
46 private final ObservableSatellite satellite;
47
48 public TurnAroundRangeMeasurementCreator(final Context context) {
49 this(context, Vector3D.ZERO);
50 }
51
52 public TurnAroundRangeMeasurementCreator(final Context context, final Vector3D antennaPhaseCenter) {
53 this.context = context;
54 this.antennaPhaseCenter = antennaPhaseCenter;
55 this.satellite = new ObservableSatellite(0);
56 }
57
58 public void init(SpacecraftState s0, AbsoluteDate t, double step) {
59 for (Map.Entry<GroundStation, GroundStation> entry : context.TARstations.entrySet()) {
60 for (final GroundStation station : Arrays.asList(entry.getKey(), entry.getValue())) {
61 for (ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
62 station.getEastOffsetDriver(),
63 station.getNorthOffsetDriver(),
64 station.getZenithOffsetDriver(),
65 station.getPrimeMeridianOffsetDriver(),
66 station.getPrimeMeridianDriftDriver(),
67 station.getPolarOffsetXDriver(),
68 station.getPolarDriftXDriver(),
69 station.getPolarOffsetYDriver(),
70 station.getPolarDriftYDriver())) {
71 if (driver.getReferenceDate() == null) {
72 driver.setReferenceDate(s0.getDate());
73 }
74 }
75
76 }
77 }
78 }
79
80 /**
81 * Function handling the steps of the propagator
82 * A turn-around measurement needs 2 stations, a master and a slave
83 * The measurement is a signal:
84 * - Emitted from the master ground station
85 * - Reflected on the spacecraft
86 * - Reflected on the slave ground station
87 * - Reflected on the spacecraft again
88 * - Received on the master ground station
89 * Its value is the elapsed time between emission and reception
90 * divided by 2c were c is the speed of light.
91 *
92 * The path of the signal is divided into 2 legs:
93 * - The 1st leg goes from emission by the master station to reception by the slave station
94 * - The 2nd leg goes from emission by the slave station to reception by the master station
95 *
96 * The spacecraft state date should, after a few iterations of the estimation process, be
97 * set to the date of arrival/departure of the signal to/from the slave station.
98 * It is guaranteed by implementation of the estimated measurement.
99 * This is done to avoid big shifts in time to compute the transit states.
100 * See TurnAroundRange.java for more
101 * Thus the spacecraft date is the date when the 1st leg of the path ends and the 2nd leg begins
102 */
103 public void handleStep(final SpacecraftState currentState, final boolean isLast) {
104 try {
105 for (Map.Entry<GroundStation, GroundStation> entry : context.TARstations.entrySet()) {
106
107 final GroundStation masterStation = entry.getKey();
108 final GroundStation slaveStation = entry.getValue();
109 final AbsoluteDate date = currentState.getDate();
110 final Frame inertial = currentState.getFrame();
111 final Vector3D position = currentState.toTransform().getInverse().transformPosition(antennaPhaseCenter);
112
113 // Create a TAR measurement only if elevation for both stations is higher than elevationMin°
114 if ((masterStation.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0))&&
115 (slaveStation.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0))) {
116
117 // The solver used
118 final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
119
120 // Spacecraft date t = date of arrival/departure of the signal to/from from the slave station
121 // Slave station position in inertial frame at t
122 final Vector3D slaveStationPosition =
123 slaveStation.getOffsetToInertial(inertial, date).transformPosition(Vector3D.ZERO);
124
125 // Downlink time of flight to slave station
126 // The date of arrival/departure of the signal to/from the slave station is known and
127 // equal to spacecraft date t.
128 // Therefore we can use the function "downlinkTimeOfFlight" from GroundStation class
129 // final double slaveTauD = slaveStation.downlinkTimeOfFlight(currentState, date);
130 final double slaveTauD = solver.solve(1000, new UnivariateFunction() {
131 public double value(final double x) {
132 final SpacecraftState transitState = currentState.shiftedBy(-x);
133 final double d = Vector3D.distance(transitState.toTransform().getInverse().transformPosition(antennaPhaseCenter),
134 slaveStationPosition);
135 return d - x * Constants.SPEED_OF_LIGHT;
136 }
137 }, -1.0, 1.0);
138
139 // Uplink time of flight from slave station
140 // A solver is used to know where the satellite is when it receives the signal
141 // back from the slave station
142 final double slaveTauU = solver.solve(1000, new UnivariateFunction() {
143 public double value(final double x) {
144 final SpacecraftState transitState = currentState.shiftedBy(+x);
145 final double d = Vector3D.distance(transitState.toTransform().getInverse().transformPosition(antennaPhaseCenter),
146 slaveStationPosition);
147 return d - x * Constants.SPEED_OF_LIGHT;
148 }
149 }, -1.0, 1.0);
150
151
152 // Find the position of the master station at signal departure and arrival
153 // ----
154
155 // Transit state position & date for the 1st leg of the signal path
156 final SpacecraftState S1 = currentState.shiftedBy(-slaveTauD);
157 final Vector3D P1 = S1.toTransform().getInverse().transformPosition(antennaPhaseCenter);
158 final AbsoluteDate T1 = date.shiftedBy(-slaveTauD);
159
160 // Transit state position & date for the 2nd leg of the signal path
161 final Vector3D P2 = currentState.shiftedBy(+slaveTauU).toTransform().getInverse().transformPosition(antennaPhaseCenter);
162 final AbsoluteDate T2 = date.shiftedBy(+slaveTauU);
163
164
165 // Master station downlink delay - from P2 to master station
166 // We use a solver to know where the master station is when it receives
167 // the signal back from the satellite on the 2nd leg of the path
168 final double masterTauD = solver.solve(1000, new UnivariateFunction() {
169 public double value(final double x) {
170 final Transform t = masterStation.getOffsetToInertial(inertial, T2.shiftedBy(+x));
171 final double d = Vector3D.distance(P2, t.transformPosition(Vector3D.ZERO));
172 return d - x * Constants.SPEED_OF_LIGHT;
173 }
174 }, -1.0, 1.0);
175
176 final AbsoluteDate masterReceptionDate = T2.shiftedBy(+masterTauD);
177 final TimeStampedPVCoordinates masterStationAtReception =
178 masterStation.getOffsetToInertial(inertial, masterReceptionDate).
179 transformPVCoordinates(new TimeStampedPVCoordinates(masterReceptionDate, PVCoordinates.ZERO));
180
181
182 // Master station uplink delay - from master station to P1
183 // Here the state date is known. Thus we can use the function "signalTimeOfFlight"
184 // of the AbstractMeasurement class
185 final double masterTauU = AbstractMeasurement.signalTimeOfFlight(masterStationAtReception, P1, T1);
186
187 final AbsoluteDate masterEmissionDate = T1.shiftedBy(-masterTauU);
188
189 final Vector3D masterStationAtEmission =
190 masterStation.getOffsetToInertial(inertial, masterEmissionDate).transformPosition(Vector3D.ZERO);
191
192
193 // Uplink/downlink distance from/to slave station
194 final double slaveDownLinkDistance = Vector3D.distance(P1, slaveStationPosition);
195 final double slaveUpLinkDistance = Vector3D.distance(P2, slaveStationPosition);
196
197 // Uplink/downlink distance from/to master station
198 final double masterUpLinkDistance = Vector3D.distance(P1, masterStationAtEmission);
199 final double masterDownLinkDistance = Vector3D.distance(P2, masterStationAtReception.getPosition());
200
201 addMeasurement(new TurnAroundRange(masterStation, slaveStation, masterReceptionDate,
202 0.5 * (masterUpLinkDistance + slaveDownLinkDistance +
203 slaveUpLinkDistance + masterDownLinkDistance), 1.0, 10, satellite));
204 }
205
206 }
207 } catch (OrekitException oe) {
208 throw new OrekitException(oe);
209 }
210 }
211
212 }