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.geometry.euclidean.threed.FieldVector3D;
23 import org.hipparchus.geometry.euclidean.threed.Vector3D;
24 import org.hipparchus.util.MathUtils;
25 import org.orekit.frames.FieldStaticTransform;
26 import org.orekit.frames.Frame;
27 import org.orekit.frames.StaticTransform;
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
44
45 public class AngularRaDec extends GroundReceiverMeasurement<AngularRaDec> {
46
47
48 public static final String MEASUREMENT_TYPE = "AngularRaDec";
49
50
51 private final Frame referenceFrame;
52
53
54
55
56
57
58
59
60
61
62
63 public AngularRaDec(final GroundStation station, final Frame referenceFrame, final AbsoluteDate date,
64 final double[] angular, final double[] sigma, final double[] baseWeight,
65 final ObservableSatellite satellite) {
66 super(station, false, date, angular, sigma, baseWeight, satellite);
67 this.referenceFrame = referenceFrame;
68 }
69
70
71
72
73 public Frame getReferenceFrame() {
74 return referenceFrame;
75 }
76
77
78 @Override
79 protected EstimatedMeasurementBase<AngularRaDec> theoreticalEvaluationWithoutDerivatives(final int iteration,
80 final int evaluation,
81 final SpacecraftState[] states) {
82
83 final GroundReceiverCommonParametersWithoutDerivatives common = computeCommonParametersWithout(states[0]);
84 final TimeStampedPVCoordinates transitPV = common.getTransitPV();
85
86
87 final Vector3D staSatInertial = transitPV.getPosition().subtract(common.getStationDownlink().getPosition());
88
89
90 final StaticTransform inertialToReferenceDownlink = common.getState().getFrame().
91 getStaticTransformTo(referenceFrame, common.getStationDownlink().getDate());
92
93
94 final Vector3D staSatReference = inertialToReferenceDownlink.transformVector(staSatInertial);
95
96
97 final double baseRightAscension = staSatReference.getAlpha();
98 final double twoPiWrap = MathUtils.normalizeAngle(baseRightAscension, getObservedValue()[0]) - baseRightAscension;
99 final double rightAscension = baseRightAscension + twoPiWrap;
100 final double declination = staSatReference.getDelta();
101
102
103 final EstimatedMeasurementBase<AngularRaDec> estimated =
104 new EstimatedMeasurementBase<>(this, iteration, evaluation,
105 new SpacecraftState[] {
106 common.getTransitState()
107 }, new TimeStampedPVCoordinates[] {
108 transitPV,
109 common.getStationDownlink()
110 });
111
112
113 estimated.setEstimatedValue(rightAscension, declination);
114
115 return estimated;
116
117 }
118
119
120 @Override
121 protected EstimatedMeasurement<AngularRaDec> theoreticalEvaluation(final int iteration, final int evaluation,
122 final SpacecraftState[] states) {
123
124 final SpacecraftState state = states[0];
125
126
127
128
129
130
131
132
133
134 final GroundReceiverCommonParametersWithDerivatives common = computeCommonParametersWithDerivatives(state);
135 final TimeStampedFieldPVCoordinates<Gradient> transitPV = common.getTransitPV();
136
137
138 final FieldVector3D<Gradient> staSatInertial = transitPV.getPosition().subtract(common.getStationDownlink().getPosition());
139
140
141 final FieldStaticTransform<Gradient> inertialToReferenceDownlink =
142 state.getFrame().getStaticTransformTo(referenceFrame, common.getStationDownlink().getDate());
143
144
145 final FieldVector3D<Gradient> staSatReference = inertialToReferenceDownlink.transformVector(staSatInertial);
146
147
148 final Gradient baseRightAscension = staSatReference.getAlpha();
149 final double twoPiWrap = MathUtils.normalizeAngle(baseRightAscension.getReal(),
150 getObservedValue()[0]) - baseRightAscension.getReal();
151 final Gradient rightAscension = baseRightAscension.add(twoPiWrap);
152 final Gradient declination = staSatReference.getDelta();
153
154
155 final EstimatedMeasurement<AngularRaDec> estimated =
156 new EstimatedMeasurement<>(this, iteration, evaluation,
157 new SpacecraftState[] {
158 common.getTransitState()
159 }, new TimeStampedPVCoordinates[] {
160 transitPV.toTimeStampedPVCoordinates(),
161 common.getStationDownlink().toTimeStampedPVCoordinates()
162 });
163
164
165 estimated.setEstimatedValue(rightAscension.getValue(), declination.getValue());
166
167
168 final double[] raDerivatives = rightAscension.getGradient();
169 final double[] decDerivatives = declination.getGradient();
170 estimated.setStateDerivatives(0,
171 Arrays.copyOfRange(raDerivatives, 0, 6), Arrays.copyOfRange(decDerivatives, 0, 6));
172
173
174 for (final ParameterDriver driver : getParametersDrivers()) {
175 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
176 final Integer index = common.getIndices().get(span.getData());
177 if (index != null) {
178 estimated.setParameterDerivatives(driver, span.getStart(), raDerivatives[index], decDerivatives[index]);
179 }
180 }
181 }
182
183 return estimated;
184
185 }
186
187
188
189
190
191
192 public Vector3D getObservedLineOfSight(final Frame outputFrame) {
193 return referenceFrame.getStaticTransformTo(outputFrame, getDate())
194 .transformVector(new Vector3D(getObservedValue()[0], getObservedValue()[1]));
195 }
196 }