1 /* Copyright 2002-2021 CS GROUP
2 * Licensed to CS GROUP (CS) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * CS licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
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.analysis.differentiation.Gradient;
25 import org.hipparchus.analysis.differentiation.GradientField;
26 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
27 import org.hipparchus.util.MathUtils;
28 import org.orekit.frames.FieldTransform;
29 import org.orekit.frames.Frame;
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 /** Class modeling an Right Ascension - Declination measurement from a ground point (station, telescope).
38 * The angles are given in an inertial reference frame.
39 * The motion of the spacecraft during the signal flight time is taken into
40 * account. The date of the measurement corresponds to the reception on
41 * ground of the reflected signal.
42 *
43 * @author Thierry Ceolin
44 * @author Maxime Journot
45 * @since 9.0
46 */
47 public class AngularRaDec extends AbstractMeasurement<AngularRaDec> {
48
49 /** Ground station from which measurement is performed. */
50 private final GroundStation station;
51
52 /** Reference frame in which the right ascension - declination angles are given. */
53 private final Frame referenceFrame;
54
55 /** Simple constructor.
56 * @param station ground station from which measurement is performed
57 * @param referenceFrame Reference frame in which the right ascension - declination angles are given
58 * @param date date of the measurement
59 * @param angular observed value
60 * @param sigma theoretical standard deviation
61 * @param baseWeight base weight
62 * @param satellite satellite related to this measurement
63 * @since 9.3
64 */
65 public AngularRaDec(final GroundStation station, final Frame referenceFrame, final AbsoluteDate date,
66 final double[] angular, final double[] sigma, final double[] baseWeight,
67 final ObservableSatellite satellite) {
68 super(date, angular, sigma, baseWeight, Collections.singletonList(satellite));
69 addParameterDriver(station.getClockOffsetDriver());
70 addParameterDriver(station.getEastOffsetDriver());
71 addParameterDriver(station.getNorthOffsetDriver());
72 addParameterDriver(station.getZenithOffsetDriver());
73 addParameterDriver(station.getPrimeMeridianOffsetDriver());
74 addParameterDriver(station.getPrimeMeridianDriftDriver());
75 addParameterDriver(station.getPolarOffsetXDriver());
76 addParameterDriver(station.getPolarDriftXDriver());
77 addParameterDriver(station.getPolarOffsetYDriver());
78 addParameterDriver(station.getPolarDriftYDriver());
79 this.station = station;
80 this.referenceFrame = referenceFrame;
81 }
82
83 /** Get the ground station from which measurement is performed.
84 * @return ground station from which measurement is performed
85 */
86 public GroundStation getStation() {
87 return station;
88 }
89
90 /** Get the reference frame in which the right ascension - declination angles are given.
91 * @return reference frame in which the right ascension - declination angles are given
92 */
93 public Frame getReferenceFrame() {
94 return referenceFrame;
95 }
96
97 /** {@inheritDoc} */
98 @Override
99 protected EstimatedMeasurement<AngularRaDec> theoreticalEvaluation(final int iteration, final int evaluation,
100 final SpacecraftState[] states) {
101
102 final SpacecraftState state = states[0];
103
104 // Right Ascension/elevation (in reference frame )derivatives are computed with respect to spacecraft state in inertial frame
105 // and station parameters
106 // ----------------------
107 //
108 // Parameters:
109 // - 0..2 - Position of the spacecraft in inertial frame
110 // - 3..5 - Velocity of the spacecraft in inertial frame
111 // - 6..n - station parameters (clock offset, station offsets, pole, prime meridian...)
112
113 // Get the number of parameters used for derivation
114 // Place the selected drivers into a map
115 int nbParams = 6;
116 final Map<String, Integer> indices = new HashMap<>();
117 for (ParameterDriver driver : getParametersDrivers()) {
118 if (driver.isSelected()) {
119 indices.put(driver.getName(), nbParams++);
120 }
121 }
122 final FieldVector3D<Gradient> zero = FieldVector3D.getZero(GradientField.getField(nbParams));
123
124 // Coordinates of the spacecraft expressed as a gradient
125 final TimeStampedFieldPVCoordinates<Gradient> pvaDS = getCoordinates(state, 0, nbParams);
126
127 // Transform between station and inertial frame, expressed as a gradient
128 // The components of station's position in offset frame are the 3 last derivative parameters
129 final FieldTransform<Gradient> offsetToInertialDownlink =
130 station.getOffsetToInertial(state.getFrame(), getDate(), nbParams, indices);
131 final FieldAbsoluteDate<Gradient> downlinkDateDS =
132 offsetToInertialDownlink.getFieldDate();
133
134 // Station position/velocity in inertial frame at end of the downlink leg
135 final TimeStampedFieldPVCoordinates<Gradient> stationDownlink =
136 offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS,
137 zero, zero, zero));
138
139 // Compute propagation times
140 // (if state has already been set up to pre-compensate propagation delay,
141 // we will have delta == tauD and transitState will be the same as state)
142
143 // Downlink delay
144 final Gradient tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS);
145
146 // Transit state
147 final Gradient delta = downlinkDateDS.durationFrom(state.getDate());
148 final Gradient deltaMTauD = tauD.negate().add(delta);
149 final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue());
150
151 // Transit state (re)computed with gradients
152 final TimeStampedFieldPVCoordinates<Gradient> transitStateDS = pvaDS.shiftedBy(deltaMTauD);
153
154 // Station-satellite vector expressed in inertial frame
155 final FieldVector3D<Gradient> staSatInertial = transitStateDS.getPosition().subtract(stationDownlink.getPosition());
156
157 // Field transform from inertial to reference frame at station's reception date
158 final FieldTransform<Gradient> inertialToReferenceDownlink =
159 state.getFrame().getTransformTo(referenceFrame, downlinkDateDS);
160
161 // Station-satellite vector in reference frame
162 final FieldVector3D<Gradient> staSatReference = inertialToReferenceDownlink.transformPosition(staSatInertial);
163
164 // Compute right ascension and declination
165 final Gradient baseRightAscension = staSatReference.getAlpha();
166 final double twoPiWrap = MathUtils.normalizeAngle(baseRightAscension.getReal(),
167 getObservedValue()[0]) - baseRightAscension.getReal();
168 final Gradient rightAscension = baseRightAscension.add(twoPiWrap);
169 final Gradient declination = staSatReference.getDelta();
170
171 // Prepare the estimation
172 final EstimatedMeasurement<AngularRaDec> estimated =
173 new EstimatedMeasurement<>(this, iteration, evaluation,
174 new SpacecraftState[] {
175 transitState
176 }, new TimeStampedPVCoordinates[] {
177 transitStateDS.toTimeStampedPVCoordinates(),
178 stationDownlink.toTimeStampedPVCoordinates()
179 });
180
181 // azimuth - elevation values
182 estimated.setEstimatedValue(rightAscension.getValue(), declination.getValue());
183
184 // Partial derivatives of right ascension/declination in reference frame with respect to state
185 // (beware element at index 0 is the value, not a derivative)
186 final double[] raDerivatives = rightAscension.getGradient();
187 final double[] decDerivatives = declination.getGradient();
188 estimated.setStateDerivatives(0,
189 Arrays.copyOfRange(raDerivatives, 0, 6), Arrays.copyOfRange(decDerivatives, 0, 6));
190
191 // Partial derivatives with respect to parameters
192 // (beware element at index 0 is the value, not a derivative)
193 for (final ParameterDriver driver : getParametersDrivers()) {
194 final Integer index = indices.get(driver.getName());
195 if (index != null) {
196 estimated.setParameterDerivatives(driver, raDerivatives[index], decDerivatives[index]);
197 }
198 }
199
200 return estimated;
201 }
202 }