AbstractMeasurement.java

  1. /* Copyright 2002-2022 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;

  19. import java.util.ArrayList;
  20. import java.util.Collections;
  21. import java.util.List;

  23. import org.hipparchus.CalculusFieldElement;
  24. import org.hipparchus.analysis.differentiation.Gradient;
  25. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  26. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  27. import org.hipparchus.util.FastMath;
  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.TimeStampedFieldPVCoordinates;
  34. import org.orekit.utils.TimeStampedPVCoordinates;

  36. /** Abstract class handling measurements boilerplate.
  37.  * @param <T> the type of the measurement
  38.  * @author Luc Maisonobe
  39.  * @since 8.0
  40.  */
  41. public abstract class AbstractMeasurement<T extends ObservedMeasurement<T>>
  42.     implements ObservedMeasurement<T> {

  44.     /** List of the supported parameters. */
  45.     private final List<ParameterDriver> supportedParameters;

  47.     /** Satellites related to this measurement.
  48.      * @since 9.3
  49.      */
  50.     private final List<ObservableSatellite> satellites;

  52.     /** Date of the measurement. */
  53.     private final AbsoluteDate date;

  55.     /** Observed value. */
  56.     private final double[] observed;

  58.     /** Theoretical standard deviation. */
  59.     private final double[] sigma;

  61.     /** Base weight. */
  62.     private final double[] baseWeight;

  64.     /** Modifiers that apply to the measurement.*/
  65.     private final List<EstimationModifier<T>> modifiers;

  67.     /** Enabling status. */
  68.     private boolean enabled;

  70.     /** Simple constructor for mono-dimensional measurements.
  71.      * <p>
  72.      * At construction, a measurement is enabled.
  73.      * </p>
  74.      * @param date date of the measurement
  75.      * @param observed observed value
  76.      * @param sigma theoretical standard deviation
  77.      * @param baseWeight base weight
  78.      * @param satellites satellites related to this measurement
  79.      * @since 9.3
  80.      */
  81.     protected AbstractMeasurement(final AbsoluteDate date, final double observed,
  82.                                   final double sigma, final double baseWeight,
  83.                                   final List<ObservableSatellite> satellites) {

  85.         this.supportedParameters = new ArrayList<ParameterDriver>();

  87.         this.date       = date;
  88.         this.observed   = new double[] {
  89.             observed
  90.         };
  91.         this.sigma      = new double[] {
  92.             sigma
  93.         };
  94.         this.baseWeight = new double[] {
  95.             baseWeight
  96.         };

  98.         this.satellites = satellites;

  100.         this.modifiers = new ArrayList<EstimationModifier<T>>();
  101.         setEnabled(true);

  103.     }

  105.     /** Simple constructor, for multi-dimensional measurements.
  106.      * <p>
  107.      * At construction, a measurement is enabled.
  108.      * </p>
  109.      * @param date date of the measurement
  110.      * @param observed observed value
  111.      * @param sigma theoretical standard deviation
  112.      * @param baseWeight base weight
  113.      * @param satellites satellites related to this measurement
  114.      * @since 9.3
  115.      */
  116.     protected AbstractMeasurement(final AbsoluteDate date, final double[] observed,
  117.                                   final double[] sigma, final double[] baseWeight,
  118.                                   final List<ObservableSatellite> satellites) {
  119.         this.supportedParameters = new ArrayList<ParameterDriver>();

  121.         this.date       = date;
  122.         this.observed   = observed.clone();
  123.         this.sigma      = sigma.clone();
  124.         this.baseWeight = baseWeight.clone();

  126.         this.satellites = satellites;

  128.         this.modifiers = new ArrayList<EstimationModifier<T>>();
  129.         setEnabled(true);

  131.     }

  133.     /** Add a parameter driver.
  134.      * @param driver parameter driver to add
  135.      * @since 9.3
  136.      */
  137.     protected void addParameterDriver(final ParameterDriver driver) {
  138.         supportedParameters.add(driver);
  139.     }

  141.     /** {@inheritDoc} */
  142.     @Override
  143.     public List<ParameterDriver> getParametersDrivers() {
  144.         return Collections.unmodifiableList(supportedParameters);
  145.     }

  147.     /** {@inheritDoc} */
  148.     @Override
  149.     public void setEnabled(final boolean enabled) {
  150.         this.enabled = enabled;
  151.     }

  153.     /** {@inheritDoc} */
  154.     @Override
  155.     public boolean isEnabled() {
  156.         return enabled;
  157.     }

  159.     /** {@inheritDoc} */
  160.     @Override
  161.     public int getDimension() {
  162.         return observed.length;
  163.     }

  165.     /** {@inheritDoc} */
  166.     @Override
  167.     public double[] getTheoreticalStandardDeviation() {
  168.         return sigma.clone();
  169.     }

  171.     /** {@inheritDoc} */
  172.     @Override
  173.     public double[] getBaseWeight() {
  174.         return baseWeight.clone();
  175.     }

  177.     /** {@inheritDoc} */
  178.     @Override
  179.     public List<ObservableSatellite> getSatellites() {
  180.         return satellites;
  181.     }

  183.     /** Estimate the theoretical value.
  184.      * <p>
  185.      * The theoretical value does not have <em>any</em> modifiers applied.
  186.      * </p>
  187.      * @param iteration iteration number
  188.      * @param evaluation evaluation number
  189.      * @param states orbital states at measurement date
  190.      * @return theoretical value
  191.      * @see #estimate(int, int, SpacecraftState[])
  192.      */
  193.     protected abstract EstimatedMeasurement<T> theoreticalEvaluation(int iteration, int evaluation, SpacecraftState[] states);

  195.     /** {@inheritDoc} */
  196.     @Override
  197.     public EstimatedMeasurement<T> estimate(final int iteration, final int evaluation, final SpacecraftState[] states) {

  199.         // compute the theoretical value
  200.         final EstimatedMeasurement<T> estimation = theoreticalEvaluation(iteration, evaluation, states);

  202.         // apply the modifiers
  203.         for (final EstimationModifier<T> modifier : modifiers) {
  204.             modifier.modify(estimation);
  205.         }

  207.         return estimation;

  209.     }

  211.     /** {@inheritDoc} */
  212.     @Override
  213.     public AbsoluteDate getDate() {
  214.         return date;
  215.     }

  217.     /** {@inheritDoc} */
  218.     @Override
  219.     public double[] getObservedValue() {
  220.         return observed.clone();
  221.     }

  223.     /** {@inheritDoc} */
  224.     @Override
  225.     public void addModifier(final EstimationModifier<T> modifier) {

  227.         // combine the measurement parameters and the modifier parameters
  228.         supportedParameters.addAll(modifier.getParametersDrivers());

  230.         modifiers.add(modifier);

  232.     }

  234.     /** {@inheritDoc} */
  235.     @Override
  236.     public List<EstimationModifier<T>> getModifiers() {
  237.         return Collections.unmodifiableList(modifiers);
  238.     }

  240.     /** Compute propagation delay on a link leg (typically downlink or uplink).
  241.      * @param adjustableEmitterPV position/velocity of emitter that may be adjusted
  242.      * @param receiverPosition fixed position of receiver at {@code signalArrivalDate},
  243.      * in the same frame as {@code adjustableEmitterPV}
  244.      * @param signalArrivalDate date at which the signal arrives to receiver
  245.      * @return <em>positive</em> delay between signal emission and signal reception dates
  246.      */
  247.     public static double signalTimeOfFlight(final TimeStampedPVCoordinates adjustableEmitterPV,
  248.                                             final Vector3D receiverPosition,
  249.                                             final AbsoluteDate signalArrivalDate) {

  251.         // initialize emission date search loop assuming the state is already correct
  252.         // this will be true for all but the first orbit determination iteration,
  253.         // and even for the first iteration the loop will converge very fast
  254.         final double offset = signalArrivalDate.durationFrom(adjustableEmitterPV.getDate());
  255.         double delay = offset;

  257.         // search signal transit date, computing the signal travel in inertial frame
  258.         final double cReciprocal = 1.0 / Constants.SPEED_OF_LIGHT;
  259.         double delta;
  260.         int count = 0;
  261.         do {
  262.             final double previous   = delay;
  263.             final Vector3D transitP = adjustableEmitterPV.shiftedBy(offset - delay).getPosition();
  264.             delay                   = receiverPosition.distance(transitP) * cReciprocal;
  265.             delta                   = FastMath.abs(delay - previous);
  266.         } while (count++ < 10 && delta >= 2 * FastMath.ulp(delay));

  268.         return delay;

  270.     }

  272.     /** Compute propagation delay on a link leg (typically downlink or uplink).
  273.      * @param adjustableEmitterPV position/velocity of emitter that may be adjusted
  274.      * @param receiverPosition fixed position of receiver at {@code signalArrivalDate},
  275.      * in the same frame as {@code adjustableEmitterPV}
  276.      * @param signalArrivalDate date at which the signal arrives to receiver
  277.      * @return <em>positive</em> delay between signal emission and signal reception dates
  278.      * @param <T> the type of the components
  279.      */
  280.     public static <T extends CalculusFieldElement<T>> T signalTimeOfFlight(final TimeStampedFieldPVCoordinates<T> adjustableEmitterPV,
  281.                                                                        final FieldVector3D<T> receiverPosition,
  282.                                                                        final FieldAbsoluteDate<T> signalArrivalDate) {

  284.         // Initialize emission date search loop assuming the emitter PV is almost correct
  285.         // this will be true for all but the first orbit determination iteration,
  286.         // and even for the first iteration the loop will converge extremely fast
  287.         final T offset = signalArrivalDate.durationFrom(adjustableEmitterPV.getDate());
  288.         T delay = offset;

  290.         // search signal transit date, computing the signal travel in the frame shared by emitter and receiver
  291.         final double cReciprocal = 1.0 / Constants.SPEED_OF_LIGHT;
  292.         double delta;
  293.         int count = 0;
  294.         do {
  295.             final double previous           = delay.getReal();
  296.             final FieldVector3D<T> transitP = adjustableEmitterPV.shiftedBy(delay.negate().add(offset)).getPosition();
  297.             delay                           = receiverPosition.distance(transitP).multiply(cReciprocal);
  298.             delta                           = FastMath.abs(delay.getReal() - previous);
  299.         } while (count++ < 10 && delta >= 2 * FastMath.ulp(delay.getReal()));

  301.         return delay;

  303.     }

  305.     /** Get Cartesian coordinates as derivatives.
  306.      * <p>
  307.      * The position will correspond to variables {@code firstDerivative},
  308.      * {@code firstDerivative + 1} and {@code firstDerivative + 2}.
  309.      * The velocity will correspond to variables {@code firstDerivative + 3},
  310.      * {@code firstDerivative + 4} and {@code firstDerivative + 5}.
  311.      * The acceleration will correspond to constants.
  312.      * </p>
  313.      * @param state state of the satellite considered
  314.      * @param firstDerivative index of the first derivative
  315.      * @param freeParameters total number of free parameters in the gradient
  316.      * @return Cartesian coordinates as derivatives
  317.      * @since 10.2
  318.      */
  319.     public static TimeStampedFieldPVCoordinates<Gradient> getCoordinates(final SpacecraftState state,
  320.                                                                          final int firstDerivative,
  321.                                                                          final int freeParameters) {

  323.         // Position of the satellite expressed as a gradient
  324.         // The components of the position are the 3 first derivative parameters
  325.         final Vector3D p = state.getPVCoordinates().getPosition();
  326.         final FieldVector3D<Gradient> pDS =
  327.                         new FieldVector3D<>(Gradient.variable(freeParameters, firstDerivative + 0, p.getX()),
  328.                                             Gradient.variable(freeParameters, firstDerivative + 1, p.getY()),
  329.                                             Gradient.variable(freeParameters, firstDerivative + 2, p.getZ()));

  331.         // Velocity of the satellite expressed as a gradient
  332.         // The components of the velocity are the 3 second derivative parameters
  333.         final Vector3D v = state.getPVCoordinates().getVelocity();
  334.         final FieldVector3D<Gradient> vDS =
  335.                         new FieldVector3D<>(Gradient.variable(freeParameters, firstDerivative + 3, v.getX()),
  336.                                             Gradient.variable(freeParameters, firstDerivative + 4, v.getY()),
  337.                                             Gradient.variable(freeParameters, firstDerivative + 5, v.getZ()));

  339.         // Acceleration of the satellite
  340.         // The components of the acceleration are not derivative parameters
  341.         final Vector3D a = state.getPVCoordinates().getAcceleration();
  342.         final FieldVector3D<Gradient> aDS =
  343.                         new FieldVector3D<>(Gradient.constant(freeParameters, a.getX()),
  344.                                             Gradient.constant(freeParameters, a.getY()),
  345.                                             Gradient.constant(freeParameters, a.getZ()));

  347.         return new TimeStampedFieldPVCoordinates<>(state.getDate(), pDS, vDS, aDS);

  349.     }

  351. }
Created with JaCoCo 0.8.8.202204050719