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