AbstractMeasurement.java

  1. /* Copyright 2002-2017 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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.RealFieldElement;
  24. import org.hipparchus.analysis.differentiation.DSFactory;
  25. import org.hipparchus.analysis.differentiation.DerivativeStructure;
  26. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  27. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  28. import org.hipparchus.util.FastMath;
  29. import org.orekit.errors.OrekitException;
  30. import org.orekit.propagation.SpacecraftState;
  31. import org.orekit.time.AbsoluteDate;
  32. import org.orekit.time.FieldAbsoluteDate;
  33. import org.orekit.utils.Constants;
  34. import org.orekit.utils.ParameterDriver;
  35. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  36. import org.orekit.utils.TimeStampedPVCoordinates;

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

  46.     /** List of the supported parameters. */
  47.     private final List<ParameterDriver> supportedParameters;

  49.     /** Indices of the propagators related to this measurement.
  50.      * @since 9.0
  51.      */
  52.     private final List<Integer> propagatorsIndices;

  54.     /** Date of the measurement. */
  55.     private final AbsoluteDate date;

  57.     /** Observed value. */
  58.     private final double[] observed;

  60.     /** Theoretical standard deviation. */
  61.     private final double[] sigma;

  63.     /** Base weight. */
  64.     private final double[] baseWeight;

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

  69.     /** Enabling status. */
  70.     private boolean enabled;

  72.     /** Simple constructor for mono-dimensional measurements.
  73.      * <p>
  74.      * At construction, a measurement is enabled.
  75.      * </p>
  76.      * @param date date of the measurement
  77.      * @param observed observed value
  78.      * @param sigma theoretical standard deviation
  79.      * @param baseWeight base weight
  80.      * @param propagatorsIndices indices of the propagators related to this measurement
  81.      * @param supportedParameters supported parameters
  82.      */
  83.     protected AbstractMeasurement(final AbsoluteDate date, final double observed,
  84.                                   final double sigma, final double baseWeight,
  85.                                   final List<Integer> propagatorsIndices,
  86.                                   final ParameterDriver... supportedParameters) {

  88.         this.supportedParameters = new ArrayList<ParameterDriver>(supportedParameters.length);
  89.         for (final ParameterDriver parameterDriver : supportedParameters) {
  90.             this.supportedParameters.add(parameterDriver);
  91.         }

  93.         this.date       = date;
  94.         this.observed   = new double[] {
  95.             observed
  96.         };
  97.         this.sigma      = new double[] {
  98.             sigma
  99.         };
  100.         this.baseWeight = new double[] {
  101.             baseWeight
  102.         };

  104.         this.propagatorsIndices = propagatorsIndices;

  106.         this.modifiers = new ArrayList<EstimationModifier<T>>();
  107.         setEnabled(true);

  109.     }

  111.     /** Simple constructor, for multi-dimensional measurements.
  112.      * <p>
  113.      * At construction, a measurement is enabled.
  114.      * </p>
  115.      * @param date date of the measurement
  116.      * @param observed observed value
  117.      * @param sigma theoretical standard deviation
  118.      * @param baseWeight base weight
  119.      * @param propagatorsIndices indices of the propagators related to this measurement
  120.      * @param supportedParameters supported parameters
  121.      */
  122.     protected AbstractMeasurement(final AbsoluteDate date, final double[] observed,
  123.                                   final double[] sigma, final double[] baseWeight,
  124.                                   final List<Integer> propagatorsIndices,
  125.                                   final ParameterDriver... supportedParameters) {
  126.         this.supportedParameters = new ArrayList<ParameterDriver>(supportedParameters.length);
  127.         for (final ParameterDriver parameterDriver : supportedParameters) {
  128.             this.supportedParameters.add(parameterDriver);
  129.         }

  131.         this.date       = date;
  132.         this.observed   = observed.clone();
  133.         this.sigma      = sigma.clone();
  134.         this.baseWeight = baseWeight.clone();

  136.         this.propagatorsIndices = propagatorsIndices;

  138.         this.modifiers = new ArrayList<EstimationModifier<T>>();
  139.         setEnabled(true);

  141.     }

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

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

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

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

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

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

  179.     /** {@inheritDoc} */
  180.     @Override
  181.     public List<Integer> getPropagatorsIndices() {
  182.         return propagatorsIndices;
  183.     }

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

  199.     /** {@inheritDoc} */
  200.     @Override
  201.     public EstimatedMeasurement<T> estimate(final int iteration, final int evaluation, final SpacecraftState[] states)
  202.         throws OrekitException {

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

  207.         // apply the modifiers
  208.         for (final EstimationModifier<T> modifier : modifiers) {
  209.             modifier.modify(estimation);
  210.         }

  212.         return estimation;

  214.     }

  216.     /** {@inheritDoc} */
  217.     @Override
  218.     public AbsoluteDate getDate() {
  219.         return date;
  220.     }

  222.     /** {@inheritDoc} */
  223.     @Override
  224.     public double[] getObservedValue() {
  225.         return observed.clone();
  226.     }

  228.     /** {@inheritDoc} */
  229.     @Override
  230.     public void addModifier(final EstimationModifier<T> modifier)
  231.         throws OrekitException {

  233.         // combine the measurement parameters and the modifier parameters
  234.         supportedParameters.addAll(modifier.getParametersDrivers());

  236.         modifiers.add(modifier);

  238.     }

  240.     /** {@inheritDoc} */
  241.     @Override
  242.     public List<EstimationModifier<T>> getModifiers() {
  243.         return Collections.unmodifiableList(modifiers);
  244.     }

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

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

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

  274.         return delay;

  276.     }

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

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

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

  307.         return delay;

  309.     }

  311.     /** Get Cartesian coordinates as derivatives.
  312.      * <p>
  313.      * The position will correspond to variables {@code firstDerivative},
  314.      * {@code firstDerivative + 1} and {@code firstDerivative + 2}.
  315.      * The velocity will correspond to variables {@code firstDerivative + 3},
  316.      * {@code firstDerivative + 4} and {@code firstDerivative + 5}.
  317.      * The acceleration will corredpon to constants.
  318.      * </p>
  319.      * @param state state of the satellite considered
  320.      * @param firstDerivative index of the first derivative
  321.      * @param factory factory for building the derivatives
  322.      * @return Cartesian coordinates as derivatives
  323.      */
  324.     public static TimeStampedFieldPVCoordinates<DerivativeStructure> getCoordinates(final SpacecraftState state,
  325.                                                                                     final int firstDerivative,
  326.                                                                                     final DSFactory factory) {

  328.         // Position of the satellite expressed as a derivative structure
  329.         // The components of the position are the 3 first derivative parameters
  330.         final Vector3D p = state.getPVCoordinates().getPosition();
  331.         final FieldVector3D<DerivativeStructure> pDS =
  332.                         new FieldVector3D<>(factory.variable(firstDerivative + 0, p.getX()),
  333.                                             factory.variable(firstDerivative + 1, p.getY()),
  334.                                             factory.variable(firstDerivative + 2, p.getZ()));

  336.         // Velocity of the satellite expressed as a derivative structure
  337.         // The components of the velocity are the 3 second derivative parameters
  338.         final Vector3D v = state.getPVCoordinates().getVelocity();
  339.         final FieldVector3D<DerivativeStructure> vDS =
  340.                         new FieldVector3D<>(factory.variable(firstDerivative + 3, v.getX()),
  341.                                             factory.variable(firstDerivative + 4, v.getY()),
  342.                                             factory.variable(firstDerivative + 5, v.getZ()));

  344.         // Acceleration of the satellite
  345.         // The components of the acceleration are not derivative parameters
  346.         final Vector3D a = state.getPVCoordinates().getAcceleration();
  347.         final FieldVector3D<DerivativeStructure> aDS =
  348.                         new FieldVector3D<>(factory.constant(a.getX()),
  349.                                             factory.constant(a.getY()),
  350.                                             factory.constant(a.getZ()));

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

  354.     }

  356. }
Created with JaCoCo 0.7.9.201702052155