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 java.util.Arrays;
20 import java.util.Collections;
21 import java.util.HashMap;
22 import java.util.Map;
23
24 import org.hipparchus.Field;
25 import org.hipparchus.analysis.differentiation.Gradient;
26 import org.hipparchus.analysis.differentiation.GradientField;
27 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
28 import org.hipparchus.util.MathUtils;
29 import org.orekit.frames.FieldTransform;
30 import org.orekit.propagation.SpacecraftState;
31 import org.orekit.time.AbsoluteDate;
32 import org.orekit.time.FieldAbsoluteDate;
33 import org.orekit.utils.ParameterDriver;
34 import org.orekit.utils.TimeStampedFieldPVCoordinates;
35 import org.orekit.utils.TimeStampedPVCoordinates;
36
37
38
39
40
41
42
43
44
45 public class AngularAzEl extends AbstractMeasurement<AngularAzEl> {
46
47
48 private final GroundStation station;
49
50
51
52
53
54
55
56
57
58
59 public AngularAzEl(final GroundStation station, final AbsoluteDate date,
60 final double[] angular, final double[] sigma, final double[] baseWeight,
61 final ObservableSatellite satellite) {
62 super(date, angular, sigma, baseWeight, Collections.singletonList(satellite));
63 addParameterDriver(station.getClockOffsetDriver());
64 addParameterDriver(station.getEastOffsetDriver());
65 addParameterDriver(station.getNorthOffsetDriver());
66 addParameterDriver(station.getZenithOffsetDriver());
67 addParameterDriver(station.getPrimeMeridianOffsetDriver());
68 addParameterDriver(station.getPrimeMeridianDriftDriver());
69 addParameterDriver(station.getPolarOffsetXDriver());
70 addParameterDriver(station.getPolarDriftXDriver());
71 addParameterDriver(station.getPolarOffsetYDriver());
72 addParameterDriver(station.getPolarDriftYDriver());
73 this.station = station;
74 }
75
76
77
78
79 public GroundStation getStation() {
80 return station;
81 }
82
83
84 @Override
85 protected EstimatedMeasurement<AngularAzEl> theoreticalEvaluation(final int iteration, final int evaluation,
86 final SpacecraftState[] states) {
87
88 final SpacecraftState state = states[0];
89
90
91
92
93
94
95
96
97
98
99
100
101 int nbParams = 6;
102 final Map<String, Integer> indices = new HashMap<>();
103 for (ParameterDriver driver : getParametersDrivers()) {
104 if (driver.isSelected()) {
105 indices.put(driver.getName(), nbParams++);
106 }
107 }
108 final Field<Gradient> field = GradientField.getField(nbParams);
109 final FieldVector3D<Gradient> zero = FieldVector3D.getZero(field);
110
111
112 final TimeStampedFieldPVCoordinates<Gradient> pvaDS = getCoordinates(state, 0, nbParams);
113
114
115
116 final FieldTransform<Gradient> offsetToInertialDownlink =
117 station.getOffsetToInertial(state.getFrame(), getDate(), nbParams, indices);
118 final FieldAbsoluteDate<Gradient> downlinkDateDS =
119 offsetToInertialDownlink.getFieldDate();
120
121
122 final TimeStampedFieldPVCoordinates<Gradient> stationDownlink =
123 offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS,
124 zero, zero, zero));
125
126 final FieldVector3D<Gradient> east = offsetToInertialDownlink.transformVector(FieldVector3D.getPlusI(field));
127 final FieldVector3D<Gradient> north = offsetToInertialDownlink.transformVector(FieldVector3D.getPlusJ(field));
128 final FieldVector3D<Gradient> zenith = offsetToInertialDownlink.transformVector(FieldVector3D.getPlusK(field));
129
130
131
132
133
134
135 final Gradient tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS);
136
137
138 final Gradient delta = downlinkDateDS.durationFrom(state.getDate());
139 final Gradient deltaMTauD = tauD.negate().add(delta);
140 final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue());
141
142
143 final TimeStampedFieldPVCoordinates<Gradient> transitStateDS = pvaDS.shiftedBy(deltaMTauD);
144
145
146 final FieldVector3D<Gradient> staSat = transitStateDS.getPosition().subtract(stationDownlink.getPosition());
147
148
149 final Gradient baseAzimuth = staSat.dotProduct(east).atan2(staSat.dotProduct(north));
150 final double twoPiWrap = MathUtils.normalizeAngle(baseAzimuth.getReal(), getObservedValue()[0]) -
151 baseAzimuth.getReal();
152 final Gradient azimuth = baseAzimuth.add(twoPiWrap);
153 final Gradient elevation = staSat.dotProduct(zenith).divide(staSat.getNorm()).asin();
154
155
156 final EstimatedMeasurement<AngularAzEl> estimated =
157 new EstimatedMeasurement<>(this, iteration, evaluation,
158 new SpacecraftState[] {
159 transitState
160 }, new TimeStampedPVCoordinates[] {
161 transitStateDS.toTimeStampedPVCoordinates(),
162 stationDownlink.toTimeStampedPVCoordinates()
163 });
164
165
166 estimated.setEstimatedValue(azimuth.getValue(), elevation.getValue());
167
168
169
170 final double[] azDerivatives = azimuth.getGradient();
171 final double[] elDerivatives = elevation.getGradient();
172 estimated.setStateDerivatives(0,
173 Arrays.copyOfRange(azDerivatives, 0, 6), Arrays.copyOfRange(elDerivatives, 0, 6));
174
175
176
177 for (final ParameterDriver driver : getParametersDrivers()) {
178 final Integer index = indices.get(driver.getName());
179 if (index != null) {
180 estimated.setParameterDerivatives(driver, azDerivatives[index], elDerivatives[index]);
181 }
182 }
183
184 return estimated;
185 }
186 }