AngularRaDec.java

  1. /* Copyright 2002-2018 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.Arrays;
  20. import java.util.HashMap;
  21. import java.util.Map;

  23. import org.hipparchus.Field;
  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.util.MathUtils;
  28. import org.orekit.errors.OrekitException;
  29. import org.orekit.frames.FieldTransform;
  30. import org.orekit.frames.Frame;
  31. import org.orekit.propagation.SpacecraftState;
  32. import org.orekit.time.AbsoluteDate;
  33. import org.orekit.time.FieldAbsoluteDate;
  34. import org.orekit.utils.ParameterDriver;
  35. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  36. import org.orekit.utils.TimeStampedPVCoordinates;

  38. /** Class modeling an Right Ascension - Declination measurement from a ground point (station, telescope).
  39.  * The angles are given in an inertial reference frame.
  40.  * The motion of the spacecraft during the signal flight time is taken into
  41.  * account. The date of the measurement corresponds to the reception on
  42.  * ground of the reflected signal.
  43.  *
  44.  * @author Thierry Ceolin
  45.  * @author Maxime Journot
  46.  * @since 9.0
  47.  */
  48. public class AngularRaDec extends AbstractMeasurement<AngularRaDec> {

  50.     /** Ground station from which measurement is performed. */
  51.     private final GroundStation station;

  53.     /** Reference frame in which the right ascension - declination angles are given. */
  54.     private final Frame referenceFrame;

  56.     /** Simple constructor.
  57.      * <p>
  58.      * This constructor uses 0 as the index of the propagator related
  59.      * to this measurement, thus being well suited for mono-satellite
  60.      * orbit determination.
  61.      * </p>
  62.      * @param station ground station from which measurement is performed
  63.      * @param referenceFrame Reference frame in which the right ascension - declination angles are given
  64.      * @param date date of the measurement
  65.      * @param angular observed value
  66.      * @param sigma theoretical standard deviation
  67.      * @param baseWeight base weight
  68.      * @exception OrekitException if a {@link org.orekit.utils.ParameterDriver}
  69.      * name conflict occurs
  70.      */
  71.     public AngularRaDec(final GroundStation station, final Frame referenceFrame, final AbsoluteDate date,
  72.                        final double[] angular, final double[] sigma, final double[] baseWeight)
  73.         throws OrekitException {
  74.         this(station, referenceFrame, date, angular, sigma, baseWeight, 0);
  75.     }

  77.     /** Simple constructor.
  78.      * @param station ground station from which measurement is performed
  79.      * @param referenceFrame Reference frame in which the right ascension - declination angles are given
  80.      * @param date date of the measurement
  81.      * @param angular observed value
  82.      * @param sigma theoretical standard deviation
  83.      * @param baseWeight base weight
  84.      * @param propagatorIndex index of the propagator related to this measurement
  85.      * @exception OrekitException if a {@link org.orekit.utils.ParameterDriver}
  86.      * name conflict occurs
  87.      */
  88.     public AngularRaDec(final GroundStation station, final Frame referenceFrame, final AbsoluteDate date,
  89.                         final double[] angular, final double[] sigma, final double[] baseWeight,
  90.                         final int propagatorIndex)
  91.         throws OrekitException {
  92.         super(date, angular, sigma, baseWeight, Arrays.asList(propagatorIndex),
  93.               station.getEastOffsetDriver(),
  94.               station.getNorthOffsetDriver(),
  95.               station.getZenithOffsetDriver(),
  96.               station.getPrimeMeridianOffsetDriver(),
  97.               station.getPrimeMeridianDriftDriver(),
  98.               station.getPolarOffsetXDriver(),
  99.               station.getPolarDriftXDriver(),
  100.               station.getPolarOffsetYDriver(),
  101.               station.getPolarDriftYDriver());
  102.         this.station        = station;
  103.         this.referenceFrame = referenceFrame;
  104.     }

  106.     /** Get the ground station from which measurement is performed.
  107.      * @return ground station from which measurement is performed
  108.      */
  109.     public GroundStation getStation() {
  110.         return station;
  111.     }

  113.     /** Get the reference frame in which the right ascension - declination angles are given.
  114.      * @return reference frame in which the right ascension - declination angles are given
  115.      */
  116.     public Frame getReferenceFrame() {
  117.         return referenceFrame;
  118.     }

  120.     /** {@inheritDoc} */
  121.     @Override
  122.     protected EstimatedMeasurement<AngularRaDec> theoreticalEvaluation(final int iteration, final int evaluation,
  123.                                                                        final SpacecraftState[] states)
  124.         throws OrekitException {

  126.         final SpacecraftState state = states[getPropagatorsIndices().get(0)];

  128.         // Right Ascension/elevation (in reference frame )derivatives are computed with respect to spacecraft state in inertial frame
  129.         // and station parameters
  130.         // ----------------------
  131.         //
  132.         // Parameters:
  133.         //  - 0..2 - Position of the spacecraft in inertial frame
  134.         //  - 3..5 - Velocity of the spacecraft in inertial frame
  135.         //  - 6..n - station parameters (station offsets, pole, prime meridian...)

  137.         // Get the number of parameters used for derivation
  138.         // Place the selected drivers into a map
  139.         int nbParams = 6;
  140.         final Map<String, Integer> indices = new HashMap<>();
  141.         for (ParameterDriver driver : getParametersDrivers()) {
  142.             if (driver.isSelected()) {
  143.                 indices.put(driver.getName(), nbParams++);
  144.             }
  145.         }
  146.         final DSFactory                          factory = new DSFactory(nbParams, 1);
  147.         final Field<DerivativeStructure>         field   = factory.getDerivativeField();
  148.         final FieldVector3D<DerivativeStructure> zero    = FieldVector3D.getZero(field);

  150.         // Coordinates of the spacecraft expressed as a derivative structure
  151.         final TimeStampedFieldPVCoordinates<DerivativeStructure> pvaDS = getCoordinates(state, 0, factory);

  153.         // Transform between station and inertial frame, expressed as a derivative structure
  154.         // The components of station's position in offset frame are the 3 last derivative parameters
  155.         final AbsoluteDate downlinkDate = getDate();
  156.         final FieldAbsoluteDate<DerivativeStructure> downlinkDateDS =
  157.                         new FieldAbsoluteDate<>(field, downlinkDate);
  158.         final FieldTransform<DerivativeStructure> offsetToInertialDownlink =
  159.                         station.getOffsetToInertial(state.getFrame(), downlinkDateDS, factory, indices);

  161.         // Station position/velocity in inertial frame at end of the downlink leg
  162.         final TimeStampedFieldPVCoordinates<DerivativeStructure> stationDownlink =
  163.                         offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS,
  164.                                                                                                             zero, zero, zero));

  166.         // Compute propagation times
  167.         // (if state has already been set up to pre-compensate propagation delay,
  168.         //  we will have delta == tauD and transitState will be the same as state)

  170.         // Downlink delay
  171.         final DerivativeStructure tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS);

  173.         // Transit state
  174.         final double                delta        = downlinkDate.durationFrom(state.getDate());
  175.         final DerivativeStructure   deltaMTauD   = tauD.negate().add(delta);
  176.         final SpacecraftState       transitState = state.shiftedBy(deltaMTauD.getValue());

  178.         // Transit state (re)computed with derivative structures
  179.         final TimeStampedFieldPVCoordinates<DerivativeStructure> transitStateDS = pvaDS.shiftedBy(deltaMTauD);

  181.         // Station-satellite vector expressed in inertial frame
  182.         final FieldVector3D<DerivativeStructure> staSatInertial = transitStateDS.getPosition().subtract(stationDownlink.getPosition());

  184.         // Field transform from inertial to reference frame at station's reception date
  185.         final FieldTransform<DerivativeStructure> inertialToReferenceDownlink =
  186.                         state.getFrame().getTransformTo(referenceFrame, downlinkDateDS);

  188.         // Station-satellite vector in reference frame
  189.         final FieldVector3D<DerivativeStructure> staSatReference = inertialToReferenceDownlink.transformPosition(staSatInertial);

  191.         // Compute right ascension and declination
  192.         final DerivativeStructure baseRightAscension = staSatReference.getAlpha();
  193.         final double              twoPiWrap          = MathUtils.normalizeAngle(baseRightAscension.getReal(),
  194.                                                                                 getObservedValue()[0]) - baseRightAscension.getReal();
  195.         final DerivativeStructure rightAscension     = baseRightAscension.add(twoPiWrap);
  196.         final DerivativeStructure declination        = staSatReference.getDelta();

  198.         // Prepare the estimation
  199.         final EstimatedMeasurement<AngularRaDec> estimated =
  200.                         new EstimatedMeasurement<>(this, iteration, evaluation,
  201.                                                    new SpacecraftState[] {
  202.                                                        transitState
  203.                                                    }, new TimeStampedPVCoordinates[] {
  204.                                                        transitStateDS.toTimeStampedPVCoordinates(),
  205.                                                        stationDownlink.toTimeStampedPVCoordinates()
  206.                                                    });

  208.         // azimuth - elevation values
  209.         estimated.setEstimatedValue(rightAscension.getValue(), declination.getValue());

  211.         // Partial derivatives of right ascension/declination in reference frame with respect to state
  212.         // (beware element at index 0 is the value, not a derivative)
  213.         final double[] raDerivatives  = rightAscension.getAllDerivatives();
  214.         final double[] decDerivatives = declination.getAllDerivatives();
  215.         estimated.setStateDerivatives(0,
  216.                                       Arrays.copyOfRange(raDerivatives, 1, 7), Arrays.copyOfRange(decDerivatives, 1, 7));

  218.         // Partial derivatives with respect to parameters
  219.         // (beware element at index 0 is the value, not a derivative)
  220.         for (final ParameterDriver driver : getParametersDrivers()) {
  221.             final Integer index = indices.get(driver.getName());
  222.             if (index != null) {
  223.                 estimated.setParameterDerivatives(driver, raDerivatives[index + 1], decDerivatives[index + 1]);
  224.             }
  225.         }

  227.         return estimated;
  228.     }
  229. }
Created with JaCoCo 0.7.9.201702052155