1   /* Copyright 2022-2025 Thales Alenia Space
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.gnss;
18  
19  import org.hipparchus.analysis.differentiation.Gradient;
20  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
21  import org.hipparchus.geometry.euclidean.threed.Vector3D;
22  import org.orekit.estimation.measurements.EstimatedMeasurement;
23  import org.orekit.estimation.measurements.EstimatedMeasurementBase;
24  import org.orekit.estimation.measurements.ObservableSatellite;
25  import org.orekit.propagation.SpacecraftState;
26  import org.orekit.time.AbsoluteDate;
27  import org.orekit.utils.Constants;
28  import org.orekit.utils.FieldPVCoordinates;
29  import org.orekit.utils.PVCoordinates;
30  import org.orekit.utils.ParameterDriver;
31  import org.orekit.utils.TimeSpanMap.Span;
32  import org.orekit.utils.TimeStampedPVCoordinates;
33  
34  import java.util.Arrays;
35  
36  /** One way range-rate measurement between two satellites.
37   * @author Luc Maisonobe
38   * @since 12.1
39   */
40  public class InterSatellitesOneWayRangeRate
41      extends AbstractInterSatellitesMeasurement<InterSatellitesOneWayRangeRate> {
42  
43      /** Type of the measurement. */
44      public static final String MEASUREMENT_TYPE = "InterSatellitesOneWayRangeRate";
45  
46      /** Constructor.
47       * @param local satellite which receives the signal and performs the measurement
48       * @param remote remote satellite which simply emits the signal
49       * @param date date of the measurement
50       * @param rangeRate observed value (m/s)
51       * @param sigma theoretical standard deviation
52       * @param baseWeight base weight
53       */
54      public InterSatellitesOneWayRangeRate(final ObservableSatellite local,
55                                            final ObservableSatellite remote,
56                                            final AbsoluteDate date, final double rangeRate,
57                                            final double sigma, final double baseWeight) {
58          // Call to super constructor
59          super(date, rangeRate, sigma, baseWeight, local, remote);
60      }
61  
62      /** {@inheritDoc} */
63      @Override
64      protected EstimatedMeasurementBase<InterSatellitesOneWayRangeRate> theoreticalEvaluationWithoutDerivatives(final int iteration,
65                                                                                                                 final int evaluation,
66                                                                                                                 final SpacecraftState[] states) {
67  
68          final OnBoardCommonParametersWithoutDerivatives common = computeCommonParametersWithout(states, false);
69  
70          // prepare the evaluation
71          final EstimatedMeasurementBase<InterSatellitesOneWayRangeRate> estimatedPhase =
72                          new EstimatedMeasurementBase<>(this, iteration, evaluation,
73                                                         new SpacecraftState[] {
74                                                             common.getState(),
75                                                             states[1]
76                                                         }, new TimeStampedPVCoordinates[] {
77                                                             common.getRemotePV(),
78                                                             common.getTransitPV()
79                                                         });
80  
81          // Range rate value
82          final PVCoordinates delta = new PVCoordinates(common.getRemotePV(), common.getTransitPV());
83          final double rangeRate = Vector3D.dotProduct(delta.getVelocity(), delta.getPosition().normalize()) +
84                                   Constants.SPEED_OF_LIGHT * (common.getLocalRate() - common.getRemoteRate());
85  
86          estimatedPhase.setEstimatedValue(rangeRate);
87  
88          // Return the estimated measurement
89          return estimatedPhase;
90  
91      }
92  
93      /** {@inheritDoc} */
94      @Override
95      protected EstimatedMeasurement<InterSatellitesOneWayRangeRate> theoreticalEvaluation(final int iteration,
96                                                                                           final int evaluation,
97                                                                                           final SpacecraftState[] states) {
98  
99          final OnBoardCommonParametersWithDerivatives common = computeCommonParametersWith(states, false);
100 
101         // prepare the evaluation
102         final EstimatedMeasurement<InterSatellitesOneWayRangeRate> estimatedPhase =
103                         new EstimatedMeasurement<>(this, iteration, evaluation,
104                                                    new SpacecraftState[] {
105                                                        common.getState(),
106                                                        states[1]
107                                                    }, new TimeStampedPVCoordinates[] {
108                                                        common.getRemotePV().toTimeStampedPVCoordinates(),
109                                                        common.getTransitPV().toTimeStampedPVCoordinates()
110                                                    });
111 
112         // Range rate value
113         final FieldPVCoordinates<Gradient> delta = new FieldPVCoordinates<>(common.getRemotePV(), common.getTransitPV());
114         final Gradient rangeRate = FieldVector3D.dotProduct(delta.getVelocity(), delta.getPosition().normalize()).
115                                    add(common.getLocalRate().subtract(common.getRemoteRate()).multiply(Constants.SPEED_OF_LIGHT));
116 
117         estimatedPhase.setEstimatedValue(rangeRate.getValue());
118 
119         // Range first order derivatives with respect to states
120         final double[] derivatives = rangeRate.getGradient();
121         estimatedPhase.setStateDerivatives(0, Arrays.copyOfRange(derivatives, 0,  6));
122         estimatedPhase.setStateDerivatives(1, Arrays.copyOfRange(derivatives, 6, 12));
123 
124         // Set first order derivatives with respect to parameters
125         for (final ParameterDriver driver : getParametersDrivers()) {
126             for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
127                 final Integer index = common.getIndices().get(span.getData());
128                 if (index != null) {
129                     estimatedPhase.setParameterDerivatives(driver, span.getStart(), derivatives[index]);
130                 }
131             }
132         }
133 
134         // Return the estimated measurement
135         return estimatedPhase;
136 
137     }
138 
139 }