InterSatellitesPhase.java

  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;

  19. import java.util.Arrays;
  20. import java.util.HashMap;
  21. import java.util.Map;

  23. import org.hipparchus.analysis.differentiation.Gradient;
  24. import org.orekit.estimation.measurements.AbstractMeasurement;
  25. import org.orekit.estimation.measurements.EstimatedMeasurement;
  26. import org.orekit.estimation.measurements.EstimatedMeasurementBase;
  27. import org.orekit.estimation.measurements.ObservableSatellite;
  28. import org.orekit.propagation.SpacecraftState;
  29. import org.orekit.time.AbsoluteDate;
  30. import org.orekit.time.FieldAbsoluteDate;
  31. import org.orekit.utils.Constants;
  32. import org.orekit.utils.ParameterDriver;
  33. import org.orekit.utils.TimeSpanMap.Span;
  34. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  35. import org.orekit.utils.TimeStampedPVCoordinates;

  37. /** Phase measurement between two satellites.
  38.  * <p>
  39.  * The measurement is considered to be a signal emitted from
  40.  * a remote satellite and received by a local satellite.
  41.  * Its value is the number of cycles between emission and reception.
  42.  * The motion of both spacecrafts during the signal flight time
  43.  * are taken into account. The date of the measurement corresponds to the
  44.  * reception on ground of the emitted signal.
  45.  * </p>
  46.  * @author Bryan Cazabonne
  47.  * @since 10.3
  48.  */
  49. public class InterSatellitesPhase extends AbstractMeasurement<InterSatellitesPhase> {

  51.     /** Type of the measurement. */
  52.     public static final String MEASUREMENT_TYPE = "InterSatellitesPhase";

  54.     /** Name for ambiguity driver. */
  55.     public static final String AMBIGUITY_NAME = "ambiguity";

  57.     /** Driver for ambiguity. */
  58.     private final ParameterDriver ambiguityDriver;

  60.     /** Wavelength of the phase observed value [m]. */
  61.     private final double wavelength;

  63.     /** Constructor.
  64.      * @param local satellite which receives the signal and performs the measurement
  65.      * @param remote remote satellite which simply emits the signal
  66.      * @param date date of the measurement
  67.      * @param phase observed value (cycles)
  68.      * @param wavelength phase observed value wavelength (m)
  69.      * @param sigma theoretical standard deviation
  70.      * @param baseWeight base weight
  71.      */
  72.     public InterSatellitesPhase(final ObservableSatellite local,
  73.                                 final ObservableSatellite remote,
  74.                                 final AbsoluteDate date, final double phase,
  75.                                 final double wavelength, final double sigma,
  76.                                 final double baseWeight) {
  77.         // Call to super constructor
  78.         super(date, phase, sigma, baseWeight, Arrays.asList(local, remote));

  80.         // Initialize phase ambiguity driver
  81.         ambiguityDriver = new ParameterDriver(AMBIGUITY_NAME, 0.0, 1.0,
  82.                                               Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);

  84.         // Add parameter drivers
  85.         addParameterDriver(ambiguityDriver);
  86.         addParameterDriver(local.getClockOffsetDriver());
  87.         addParameterDriver(remote.getClockOffsetDriver());

  89.         // Initialize fields
  90.         this.wavelength = wavelength;
  91.     }

  93.     /** Get the wavelength.
  94.      * @return wavelength (m)
  95.      */
  96.     public double getWavelength() {
  97.         return wavelength;
  98.     }

  100.     /** Get the driver for phase ambiguity.
  101.      * @return the driver for phase ambiguity
  102.      */
  103.     public ParameterDriver getAmbiguityDriver() {
  104.         return ambiguityDriver;
  105.     }

  107.     /** {@inheritDoc} */
  108.     @Override
  109.     protected EstimatedMeasurementBase<InterSatellitesPhase> theoreticalEvaluationWithoutDerivatives(final int iteration,
  110.                                                                                                      final int evaluation,
  111.                                                                                                      final SpacecraftState[] states) {

  113.         // Coordinates of both satellites
  114.         final SpacecraftState local = states[0];
  115.         final TimeStampedPVCoordinates pvaL = local.getPVCoordinates();
  116.         final SpacecraftState remote = states[1];
  117.         final TimeStampedPVCoordinates pvaR = remote.getPVCoordinates();

  119.         // Compute propagation times
  120.         // Downlink delay
  121.         final double dtl = getSatellites().get(0).getClockOffsetDriver().getValue(AbsoluteDate.ARBITRARY_EPOCH);
  122.         final AbsoluteDate arrivalDate = getDate().shiftedBy(-dtl);

  124.         final TimeStampedPVCoordinates s1Downlink = pvaL.shiftedBy(arrivalDate.durationFrom(pvaL.getDate()));
  125.         final double tauD = signalTimeOfFlight(pvaR, s1Downlink.getPosition(), arrivalDate);

  127.         // Transit state
  128.         final double delta      = getDate().durationFrom(remote.getDate());
  129.         final double deltaMTauD = delta - tauD;

  131.         // prepare the evaluation
  132.         final EstimatedMeasurementBase<InterSatellitesPhase> estimatedPhase =
  133.                         new EstimatedMeasurementBase<>(this, iteration, evaluation,
  134.                                                        new SpacecraftState[] {
  135.                                                            local.shiftedBy(deltaMTauD),
  136.                                                            remote.shiftedBy(deltaMTauD)
  137.                                                        }, new TimeStampedPVCoordinates[] {
  138.                                                            remote.shiftedBy(delta - tauD).getPVCoordinates(),
  139.                                                            local.shiftedBy(delta).getPVCoordinates()
  140.                                                        });

  142.         // Clock offsets
  143.         final double dtr = getSatellites().get(1).getClockOffsetDriver().getValue(AbsoluteDate.ARBITRARY_EPOCH);

  145.         // Phase value
  146.         final double cOverLambda = Constants.SPEED_OF_LIGHT / wavelength;
  147.         final double ambiguity   = ambiguityDriver.getValue(AbsoluteDate.ARBITRARY_EPOCH);
  148.         final double phase       = (tauD + dtl - dtr) * cOverLambda + ambiguity;

  150.         estimatedPhase.setEstimatedValue(phase);

  152.         // Return the estimated measurement
  153.         return estimatedPhase;

  155.     }

  157.     /** {@inheritDoc} */
  158.     @Override
  159.     protected EstimatedMeasurement<InterSatellitesPhase> theoreticalEvaluation(final int iteration,
  160.                                                                                final int evaluation,
  161.                                                                                final SpacecraftState[] states) {

  163.         // Phase derivatives are computed with respect to spacecrafts states in inertial frame
  164.         // ----------------------
  165.         //
  166.         // Parameters:
  167.         //  - 0..2  - Position of the receiver satellite in inertial frame
  168.         //  - 3..5  - Velocity of the receiver satellite in inertial frame
  169.         //  - 6..8  - Position of the remote satellite in inertial frame
  170.         //  - 9..11 - Velocity of the remote satellite in inertial frame
  171.         //  - 12..  - Measurement parameters: ambiguity, local clock offset, remote clock offset...
  172.         int nbParams = 12;
  173.         final Map<String, Integer> indices = new HashMap<>();
  174.         for (ParameterDriver phaseMeasurementDriver : getParametersDrivers()) {
  175.             if (phaseMeasurementDriver.isSelected()) {
  176.                 for (Span<String> span = phaseMeasurementDriver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  178.                     indices.put(span.getData(), nbParams++);
  179.                 }
  180.             }
  181.         }

  183.         // Coordinates of both satellites
  184.         final SpacecraftState local = states[0];
  185.         final TimeStampedFieldPVCoordinates<Gradient> pvaL = getCoordinates(local, 0, nbParams);
  186.         final SpacecraftState remote = states[1];
  187.         final TimeStampedFieldPVCoordinates<Gradient> pvaR = getCoordinates(remote, 6, nbParams);

  189.         // Compute propagation times
  190.         // Downlink delay
  191.         final Gradient dtl = getSatellites().get(0).getClockOffsetDriver().getValue(nbParams, indices, AbsoluteDate.ARBITRARY_EPOCH);
  192.         final FieldAbsoluteDate<Gradient> arrivalDate = new FieldAbsoluteDate<>(getDate(), dtl.negate());

  194.         final TimeStampedFieldPVCoordinates<Gradient> s1Downlink =
  195.                         pvaL.shiftedBy(arrivalDate.durationFrom(pvaL.getDate()));
  196.         final Gradient tauD = signalTimeOfFlight(pvaR, s1Downlink.getPosition(), arrivalDate);

  198.         // Transit state
  199.         final double   delta      = getDate().durationFrom(remote.getDate());
  200.         final Gradient deltaMTauD = tauD.negate().add(delta);

  202.         // prepare the evaluation
  203.         final EstimatedMeasurement<InterSatellitesPhase> estimatedPhase =
  204.                         new EstimatedMeasurement<>(this, iteration, evaluation,
  205.                                                    new SpacecraftState[] {
  206.                                                        local.shiftedBy(deltaMTauD.getValue()),
  207.                                                        remote.shiftedBy(deltaMTauD.getValue())
  208.                                                    }, new TimeStampedPVCoordinates[] {
  209.                                                        remote.shiftedBy(delta - tauD.getValue()).getPVCoordinates(),
  210.                                                        local.shiftedBy(delta).getPVCoordinates()
  211.                                                    });

  213.         // Clock offsets
  214.         final Gradient dtr = getSatellites().get(1).getClockOffsetDriver().getValue(nbParams, indices, AbsoluteDate.ARBITRARY_EPOCH);

  216.         // Phase value
  217.         final double   cOverLambda = Constants.SPEED_OF_LIGHT / wavelength;
  218.         final Gradient ambiguity   = ambiguityDriver.getValue(nbParams, indices, AbsoluteDate.ARBITRARY_EPOCH);
  219.         final Gradient phase       = tauD.add(dtl).subtract(dtr).multiply(cOverLambda).add(ambiguity);

  221.         estimatedPhase.setEstimatedValue(phase.getValue());

  223.         // Range partial derivatives with respect to states
  224.         final double[] derivatives = phase.getGradient();
  225.         estimatedPhase.setStateDerivatives(0, Arrays.copyOfRange(derivatives, 0,  6));
  226.         estimatedPhase.setStateDerivatives(1, Arrays.copyOfRange(derivatives, 6, 12));

  228.         // Set partial derivatives with respect to parameters
  229.         for (final ParameterDriver driver : getParametersDrivers()) {
  230.             for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

  232.                 final Integer index = indices.get(span.getData());
  233.                 if (index != null) {
  234.                     estimatedPhase.setParameterDerivatives(driver, span.getStart(), derivatives[index]);
  235.                 }
  236.             }
  237.         }

  239.         // Return the estimated measurement
  240.         return estimatedPhase;

  242.     }

  244. }
Created with JaCoCo 0.8.11.202310140853