1   /* Copyright 2002-2024 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.gnss;
18  
19  import java.util.Collections;
20  import java.util.List;
21  
22  import org.hipparchus.geometry.euclidean.threed.Rotation;
23  import org.hipparchus.geometry.euclidean.threed.Vector3D;
24  import org.hipparchus.util.FastMath;
25  import org.hipparchus.util.MathUtils;
26  import org.orekit.estimation.measurements.EstimatedMeasurementBase;
27  import org.orekit.estimation.measurements.EstimationModifier;
28  import org.orekit.estimation.measurements.ObservedMeasurement;
29  import org.orekit.utils.ParameterDriver;
30  import org.orekit.utils.TimeStampedPVCoordinates;
31  
32  /** Base class for wind-up effect computation.
33   * @see <a href="https://gssc.esa.int/navipedia/index.php/Carrier_Phase_Wind-up_Effect">Carrier Phase Wind-up Effect</a>
34   * @param <T> the type of the measurement
35   * @author Luc Maisonobe
36   * @since 12.0
37   */
38  public abstract class AbstractWindUp<T extends ObservedMeasurement<T>> implements EstimationModifier<T> {
39  
40      /** Emitter dipole. */
41      private final Dipole emitter;
42  
43      /** Receiver dipole. */
44      private final Dipole receiver;
45  
46      /** Cached angular value of wind-up. */
47      private double angularWindUp;
48  
49      /** Simple constructor.
50       * @param emitter emitter dipole
51       * @param receiver receiver dipole
52       */
53      protected AbstractWindUp(final Dipole emitter, final Dipole receiver) {
54          this.emitter  = emitter;
55          this.receiver = receiver;
56          angularWindUp = 0.0;
57      }
58  
59      /** {@inheritDoc}
60       * <p>
61       * Wind-up effect has no parameters, the returned list is always empty.
62       * </p>
63       */
64      @Override
65      public List<ParameterDriver> getParametersDrivers() {
66          return Collections.emptyList();
67      }
68  
69      /** Compute rotation from emitter to inertial frame.
70       * @param estimated estimated measurement to modify
71       * @return rotation from emitter to inertial frame
72       */
73      protected abstract Rotation emitterToInert(EstimatedMeasurementBase<T> estimated);
74  
75      /** Compute rotation from receiver to inertial frame.
76       * @param estimated estimated measurement to modify
77       * @return rotation from receiver to inertial frame
78       */
79      protected abstract Rotation receiverToInert(EstimatedMeasurementBase<T> estimated);
80  
81      /** {@inheritDoc} */
82      @Override
83      public void modifyWithoutDerivatives(final EstimatedMeasurementBase<T> estimated) {
84  
85          // signal line of sight
86          final TimeStampedPVCoordinates[] participants = estimated.getParticipants();
87          final Vector3D los = participants[1].getPosition().subtract(participants[0].getPosition()).normalize();
88  
89          // get receiver dipole
90          final Rotation receiverToInert = receiverToInert(estimated);
91          final Vector3D iReceiver       = receiverToInert.applyTo(receiver.getPrimary());
92          final Vector3D jReceiver       = receiverToInert.applyTo(receiver.getSecondary());
93          final Vector3D dReceiver       = new Vector3D(1.0, iReceiver, -Vector3D.dotProduct(iReceiver, los), los).
94                                           add(Vector3D.crossProduct(los, jReceiver));
95  
96          // get emitter dipole
97          final Rotation emitterToInert = emitterToInert(estimated);
98          final Vector3D iEmitter       = emitterToInert.applyTo(emitter.getPrimary());
99          final Vector3D jEmitter       = emitterToInert.applyTo(emitter.getSecondary());
100         final Vector3D dEmitter       = new Vector3D(1.0, iEmitter, -Vector3D.dotProduct(iEmitter, los), los).
101                                         subtract(Vector3D.crossProduct(los, jEmitter));
102 
103         // raw correction
104         final double correction = FastMath.copySign(Vector3D.angle(dEmitter, dReceiver),
105                                                     Vector3D.dotProduct(los, Vector3D.crossProduct(dEmitter, dReceiver)));
106 
107         // ensure continuity across measurements
108         // we assume the various measurements are close enough in time
109         // (less the one satellite half-turn) so the angles remain close
110         angularWindUp = MathUtils.normalizeAngle(correction, angularWindUp);
111 
112         // update estimate
113         estimated.modifyEstimatedValue(this, estimated.getEstimatedValue()[0] + angularWindUp / MathUtils.TWO_PI);
114 
115     }
116 
117 }