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
21 import org.hipparchus.analysis.differentiation.Gradient;
22 import org.hipparchus.analysis.differentiation.GradientField;
23 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
24 import org.hipparchus.geometry.euclidean.threed.Vector3D;
25 import org.hipparchus.util.FastMath;
26 import org.hipparchus.util.MathUtils;
27 import org.orekit.frames.Frame;
28 import org.orekit.propagation.SpacecraftState;
29 import org.orekit.time.AbsoluteDate;
30 import org.orekit.utils.ParameterDriver;
31 import org.orekit.utils.TimeSpanMap.Span;
32 import org.orekit.utils.TimeStampedFieldPVCoordinates;
33 import org.orekit.utils.TimeStampedPVCoordinates;
34
35
36
37
38
39
40
41
42
43 public class AngularAzEl extends GroundReceiverMeasurement<AngularAzEl> {
44
45
46 public static final String MEASUREMENT_TYPE = "AngularAzEl";
47
48
49
50
51
52
53
54
55
56
57 public AngularAzEl(final GroundStation station, final AbsoluteDate date,
58 final double[] angular, final double[] sigma, final double[] baseWeight,
59 final ObservableSatellite satellite) {
60 super(station, false, date, angular, sigma, baseWeight, satellite);
61 }
62
63
64 @Override
65 protected EstimatedMeasurementBase<AngularAzEl> theoreticalEvaluationWithoutDerivatives(final int iteration,
66 final int evaluation,
67 final SpacecraftState[] states) {
68
69 final GroundReceiverCommonParametersWithoutDerivatives common = computeCommonParametersWithout(states[0]);
70 final TimeStampedPVCoordinates transitPV = common.getTransitPV();
71
72
73 final Vector3D east = common.getOffsetToInertialDownlink().transformVector(Vector3D.PLUS_I);
74 final Vector3D north = common.getOffsetToInertialDownlink().transformVector(Vector3D.PLUS_J);
75 final Vector3D zenith = common.getOffsetToInertialDownlink().transformVector(Vector3D.PLUS_K);
76
77
78 final Vector3D staSat = transitPV.getPosition().subtract(common.getStationDownlink().getPosition());
79
80
81 final double baseAzimuth = FastMath.atan2(staSat.dotProduct(east), staSat.dotProduct(north));
82 final double twoPiWrap = MathUtils.normalizeAngle(baseAzimuth, getObservedValue()[0]) - baseAzimuth;
83 final double azimuth = baseAzimuth + twoPiWrap;
84 final double elevation = FastMath.asin(staSat.dotProduct(zenith) / staSat.getNorm());
85
86
87 final EstimatedMeasurementBase<AngularAzEl> estimated =
88 new EstimatedMeasurementBase<>(this, iteration, evaluation,
89 new SpacecraftState[] {
90 common.getTransitState()
91 }, new TimeStampedPVCoordinates[] {
92 transitPV,
93 common.getStationDownlink()
94 });
95
96
97 estimated.setEstimatedValue(azimuth, elevation);
98
99 return estimated;
100
101 }
102
103
104 @Override
105 protected EstimatedMeasurement<AngularAzEl> theoreticalEvaluation(final int iteration, final int evaluation,
106 final SpacecraftState[] states) {
107
108 final SpacecraftState state = states[0];
109
110
111
112
113
114
115
116
117
118 final GroundReceiverCommonParametersWithDerivatives common = computeCommonParametersWithDerivatives(state);
119 final TimeStampedFieldPVCoordinates<Gradient> transitPV = common.getTransitPV();
120
121
122 final GradientField field = common.getTauD().getField();
123 final FieldVector3D<Gradient> east = common.getOffsetToInertialDownlink().transformVector(FieldVector3D.getPlusI(field));
124 final FieldVector3D<Gradient> north = common.getOffsetToInertialDownlink().transformVector(FieldVector3D.getPlusJ(field));
125 final FieldVector3D<Gradient> zenith = common.getOffsetToInertialDownlink().transformVector(FieldVector3D.getPlusK(field));
126
127
128 final FieldVector3D<Gradient> staSat = transitPV.getPosition().subtract(common.getStationDownlink().getPosition());
129
130
131 final Gradient baseAzimuth = staSat.dotProduct(east).atan2(staSat.dotProduct(north));
132 final double twoPiWrap = MathUtils.normalizeAngle(baseAzimuth.getReal(), getObservedValue()[0]) -
133 baseAzimuth.getReal();
134 final Gradient azimuth = baseAzimuth.add(twoPiWrap);
135 final Gradient elevation = staSat.dotProduct(zenith).divide(staSat.getNorm()).asin();
136
137
138 final EstimatedMeasurement<AngularAzEl> estimated =
139 new EstimatedMeasurement<>(this, iteration, evaluation,
140 new SpacecraftState[] {
141 common.getTransitState()
142 }, new TimeStampedPVCoordinates[] {
143 transitPV.toTimeStampedPVCoordinates(),
144 common.getStationDownlink().toTimeStampedPVCoordinates()
145 });
146
147
148 estimated.setEstimatedValue(azimuth.getValue(), elevation.getValue());
149
150
151 final double[] azDerivatives = azimuth.getGradient();
152 final double[] elDerivatives = elevation.getGradient();
153 estimated.setStateDerivatives(0,
154 Arrays.copyOfRange(azDerivatives, 0, 6), Arrays.copyOfRange(elDerivatives, 0, 6));
155
156
157 for (final ParameterDriver driver : getParametersDrivers()) {
158
159 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
160 final Integer index = common.getIndices().get(span.getData());
161 if (index != null) {
162 estimated.setParameterDerivatives(driver, span.getStart(), azDerivatives[index], elDerivatives[index]);
163 }
164 }
165 }
166
167 return estimated;
168
169 }
170
171
172
173
174
175 public Vector3D getObservedLineOfSight(final Frame outputFrame) {
176 return getStation().getBaseFrame().getStaticTransformTo(outputFrame, getDate())
177 .transformVector(new Vector3D(MathUtils.SEMI_PI - getObservedValue()[0], getObservedValue()[1]));
178 }
179
180 }