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.differentiation.Gradient;
20 import org.hipparchus.analysis.differentiation.GradientField;
21 import org.orekit.estimation.measurements.AbstractMeasurement;
22 import org.orekit.estimation.measurements.ObservableSatellite;
23 import org.orekit.estimation.measurements.ObservedMeasurement;
24 import org.orekit.estimation.measurements.QuadraticClockModel;
25 import org.orekit.estimation.measurements.QuadraticFieldClockModel;
26 import org.orekit.propagation.SpacecraftState;
27 import org.orekit.time.AbsoluteDate;
28 import org.orekit.time.ClockOffset;
29 import org.orekit.time.FieldAbsoluteDate;
30 import org.orekit.time.FieldClockOffset;
31 import org.orekit.utils.FieldPVCoordinatesProvider;
32 import org.orekit.utils.PVCoordinatesProvider;
33 import org.orekit.utils.ParameterDriver;
34 import org.orekit.utils.TimeSpanMap.Span;
35 import org.orekit.utils.TimeStampedFieldPVCoordinates;
36 import org.orekit.utils.TimeStampedPVCoordinates;
37
38 import java.util.HashMap;
39 import java.util.List;
40 import java.util.Map;
41
42
43
44
45
46
47 public abstract class AbstractOnBoardMeasurement<T extends ObservedMeasurement<T>> extends AbstractMeasurement<T> {
48
49
50
51
52
53
54
55
56 public AbstractOnBoardMeasurement(final AbsoluteDate date, final double observed,
57 final double sigma, final double baseWeight,
58 final List<ObservableSatellite> satellites) {
59
60 super(date, observed, sigma, baseWeight, satellites);
61
62
63 satellites.forEach(s -> {
64 addParameterDriver(s.getClockOffsetDriver());
65 addParameterDriver(s.getClockDriftDriver());
66 addParameterDriver(s.getClockAccelerationDriver());
67 });
68
69 }
70
71
72
73
74 protected abstract QuadraticClockModel getRemoteClock();
75
76
77
78
79
80 protected abstract PVCoordinatesProvider getRemotePV(SpacecraftState[] states);
81
82
83
84
85
86
87 protected abstract FieldPVCoordinatesProvider<Gradient> getRemotePV(SpacecraftState[] states,
88 int freeParameters);
89
90
91
92
93
94
95
96 protected QuadraticFieldClockModel<Gradient> getRemoteClock(final int freeParameters,
97 final Map<String, Integer> indices) {
98 return getRemoteClock().toGradientModel(freeParameters, indices, getDate());
99 }
100
101
102
103
104
105
106
107
108 protected OnBoardCommonParametersWithoutDerivatives computeCommonParametersWithout(final SpacecraftState[] states,
109 final boolean clockOffsetAlreadyApplied) {
110
111
112 final TimeStampedPVCoordinates pvaLocal = states[0].getPVCoordinates();
113 final ClockOffset localClock = getSatellites().
114 get(0).
115 getQuadraticClockModel().
116 getOffset(getDate());
117 final double localClockOffset = localClock.getOffset();
118 final double localClockRate = localClock.getRate();
119 final PVCoordinatesProvider remotePV = getRemotePV(states);
120
121
122 final AbsoluteDate arrivalDate = clockOffsetAlreadyApplied ? getDate() : getDate().shiftedBy(-localClockOffset);
123
124
125 final double deltaT = arrivalDate.durationFrom(states[0]);
126 final TimeStampedPVCoordinates pvaDownlink = pvaLocal.shiftedBy(deltaT);
127 final double tauD = signalTimeOfFlight(remotePV, arrivalDate, pvaDownlink.getPosition(),
128 arrivalDate, states[0].getFrame());
129
130
131 final AbsoluteDate emissionDate = arrivalDate.shiftedBy(-tauD);
132 final ClockOffset remoteClock = getRemoteClock().getOffset(emissionDate);
133 final double remoteClockOffset = remoteClock.getOffset();
134 final double remoteClockRate = remoteClock.getRate();
135 return new OnBoardCommonParametersWithoutDerivatives(states[0],
136 localClockOffset, localClockRate,
137 remoteClockOffset, remoteClockRate,
138 tauD, pvaDownlink,
139 remotePV.getPVCoordinates(emissionDate, states[0].getFrame()));
140
141 }
142
143
144
145
146
147
148
149
150 protected OnBoardCommonParametersWithDerivatives computeCommonParametersWith(final SpacecraftState[] states,
151 final boolean clockOffsetAlreadyApplied) {
152
153
154
155
156
157
158 int nbEstimatedParams = 6 * states.length;
159 final Map<String, Integer> parameterIndices = new HashMap<>();
160 for (ParameterDriver measurementDriver : getParametersDrivers()) {
161 if (measurementDriver.isSelected()) {
162 for (Span<String> span = measurementDriver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
163 parameterIndices.put(span.getData(), nbEstimatedParams++);
164 }
165 }
166 }
167 final FieldAbsoluteDate<Gradient> gDate = new FieldAbsoluteDate<>(GradientField.getField(nbEstimatedParams),
168 getDate());
169
170
171 final TimeStampedFieldPVCoordinates<Gradient> pvaLocal = getCoordinates(states[0], 0, nbEstimatedParams);
172 final QuadraticFieldClockModel<Gradient> localClock = getSatellites().get(0).getQuadraticClockModel().
173 toGradientModel(nbEstimatedParams, parameterIndices, getDate());
174 final FieldClockOffset<Gradient> localClockOffset = localClock.getOffset(gDate);
175 final FieldPVCoordinatesProvider<Gradient> remotePV = getRemotePV(states, nbEstimatedParams);
176
177
178 final FieldAbsoluteDate<Gradient> arrivalDate = clockOffsetAlreadyApplied ?
179 gDate : gDate.shiftedBy(localClockOffset.getOffset().negate());
180
181
182 final Gradient deltaT = arrivalDate.durationFrom(states[0].getDate());
183 final TimeStampedFieldPVCoordinates<Gradient> pvaDownlink = pvaLocal.shiftedBy(deltaT);
184 final Gradient tauD = signalTimeOfFlight(remotePV, arrivalDate,
185 pvaDownlink.getPosition(), arrivalDate,
186 states[0].getFrame());
187
188
189 final FieldAbsoluteDate<Gradient> emissionDate = arrivalDate.shiftedBy(tauD.negate());
190 final QuadraticFieldClockModel<Gradient> remoteClock = getRemoteClock(nbEstimatedParams, parameterIndices);
191 final FieldClockOffset<Gradient> remoteClockOffset = remoteClock.getOffset(emissionDate);
192 return new OnBoardCommonParametersWithDerivatives(states[0], parameterIndices,
193 localClockOffset.getOffset(), localClockOffset.getRate(),
194 remoteClockOffset.getOffset(), remoteClockOffset.getRate(),
195 tauD, pvaDownlink,
196 remotePV.getPVCoordinates(emissionDate, states[0].getFrame()));
197
198 }
199
200 }