AngularAzEl.java

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

  21. import org.hipparchus.analysis.differentiation.Gradient;
  22. import org.hipparchus.analysis.differentiation.GradientField;
  23. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  24. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  25. import org.hipparchus.util.FastMath;
  26. import org.hipparchus.util.MathUtils;
  27. import org.orekit.frames.Frame;
  28. import org.orekit.propagation.SpacecraftState;
  29. import org.orekit.time.AbsoluteDate;
  30. import org.orekit.utils.ParameterDriver;
  31. import org.orekit.utils.TimeSpanMap.Span;
  32. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  33. import org.orekit.utils.TimeStampedPVCoordinates;

  35. /** Class modeling an Azimuth-Elevation measurement from a ground station.
  36.  * The motion of the spacecraft during the signal flight time is taken into
  37.  * account. The date of the measurement corresponds to the reception on
  38.  * ground of the reflected signal.
  39.  *
  40.  * @author Thierry Ceolin
  41.  * @since 8.0
  42.  */
  43. public class AngularAzEl extends GroundReceiverMeasurement<AngularAzEl> {

  45.     /** Type of the measurement. */
  46.     public static final String MEASUREMENT_TYPE = "AngularAzEl";

  48.     /** Simple constructor.
  49.      * @param station ground station from which measurement is performed
  50.      * @param date date of the measurement
  51.      * @param angular observed value
  52.      * @param sigma theoretical standard deviation
  53.      * @param baseWeight base weight
  54.      * @param satellite satellite related to this measurement
  55.      * @since 9.3
  56.      */
  57.     public AngularAzEl(final GroundStation station, final AbsoluteDate date,
  58.                        final double[] angular, final double[] sigma, final double[] baseWeight,
  59.                        final ObservableSatellite satellite) {
  60.         super(station, false, date, angular, sigma, baseWeight, satellite);
  61.     }

  63.     /** {@inheritDoc} */
  64.     @Override
  65.     protected EstimatedMeasurementBase<AngularAzEl> theoreticalEvaluationWithoutDerivatives(final int iteration,
  66.                                                                                             final int evaluation,
  67.                                                                                             final SpacecraftState[] states) {

  69.         final GroundReceiverCommonParametersWithoutDerivatives common = computeCommonParametersWithout(states[0]);
  70.         final TimeStampedPVCoordinates transitPV = common.getTransitPV();

  72.         // Station topocentric frame (east-north-zenith) in inertial frame expressed as Gradient
  73.         final Vector3D east   = common.getOffsetToInertialDownlink().transformVector(Vector3D.PLUS_I);
  74.         final Vector3D north  = common.getOffsetToInertialDownlink().transformVector(Vector3D.PLUS_J);
  75.         final Vector3D zenith = common.getOffsetToInertialDownlink().transformVector(Vector3D.PLUS_K);

  77.         // Station-satellite vector expressed in inertial frame
  78.         final Vector3D staSat = transitPV.getPosition().subtract(common.getStationDownlink().getPosition());

  80.         // Compute azimuth/elevation
  81.         final double baseAzimuth = FastMath.atan2(staSat.dotProduct(east), staSat.dotProduct(north));
  82.         final double twoPiWrap   = MathUtils.normalizeAngle(baseAzimuth, getObservedValue()[0]) - baseAzimuth;
  83.         final double azimuth     = baseAzimuth + twoPiWrap;
  84.         final double elevation   = FastMath.asin(staSat.dotProduct(zenith) / staSat.getNorm());

  86.         // Prepare the estimation
  87.         final EstimatedMeasurementBase<AngularAzEl> estimated =
  88.                         new EstimatedMeasurementBase<>(this, iteration, evaluation,
  89.                                                        new SpacecraftState[] {
  90.                                                            common.getTransitState()
  91.                                                        }, new TimeStampedPVCoordinates[] {
  92.                                                            transitPV,
  93.                                                            common.getStationDownlink()
  94.                                                        });

  96.         // azimuth - elevation values
  97.         estimated.setEstimatedValue(azimuth, elevation);

  99.         return estimated;

  101.     }

  103.     /** {@inheritDoc} */
  104.     @Override
  105.     protected EstimatedMeasurement<AngularAzEl> theoreticalEvaluation(final int iteration, final int evaluation,
  106.                                                                       final SpacecraftState[] states) {

  108.         final SpacecraftState state = states[0];

  110.         // Azimuth/elevation derivatives are computed with respect to spacecraft state in inertial frame
  111.         // and station parameters
  112.         // ----------------------
  113.         //
  114.         // Parameters:
  115.         //  - 0..2 - Position of the spacecraft in inertial frame
  116.         //  - 3..5 - Velocity of the spacecraft in inertial frame
  117.         //  - 6..n - station parameters (clock offset, station offsets, pole, prime meridian...)
  118.         final GroundReceiverCommonParametersWithDerivatives common = computeCommonParametersWithDerivatives(state);
  119.         final TimeStampedFieldPVCoordinates<Gradient> transitPV = common.getTransitPV();

  121.         // Station topocentric frame (east-north-zenith) in inertial frame expressed as Gradient
  122.         final GradientField field = common.getTauD().getField();
  123.         final FieldVector3D<Gradient> east   = common.getOffsetToInertialDownlink().transformVector(FieldVector3D.getPlusI(field));
  124.         final FieldVector3D<Gradient> north  = common.getOffsetToInertialDownlink().transformVector(FieldVector3D.getPlusJ(field));
  125.         final FieldVector3D<Gradient> zenith = common.getOffsetToInertialDownlink().transformVector(FieldVector3D.getPlusK(field));

  127.         // Station-satellite vector expressed in inertial frame
  128.         final FieldVector3D<Gradient> staSat = transitPV.getPosition().subtract(common.getStationDownlink().getPosition());

  130.         // Compute azimuth/elevation
  131.         final Gradient baseAzimuth = staSat.dotProduct(east).atan2(staSat.dotProduct(north));
  132.         final double   twoPiWrap   = MathUtils.normalizeAngle(baseAzimuth.getReal(), getObservedValue()[0]) -
  133.                                                 baseAzimuth.getReal();
  134.         final Gradient azimuth     = baseAzimuth.add(twoPiWrap);
  135.         final Gradient elevation   = staSat.dotProduct(zenith).divide(staSat.getNorm()).asin();

  137.         // Prepare the estimation
  138.         final EstimatedMeasurement<AngularAzEl> estimated =
  139.                         new EstimatedMeasurement<>(this, iteration, evaluation,
  140.                                                    new SpacecraftState[] {
  141.                                                        common.getTransitState()
  142.                                                    }, new TimeStampedPVCoordinates[] {
  143.                                                        transitPV.toTimeStampedPVCoordinates(),
  144.                                                        common.getStationDownlink().toTimeStampedPVCoordinates()
  145.                                                    });

  147.         // azimuth - elevation values
  148.         estimated.setEstimatedValue(azimuth.getValue(), elevation.getValue());

  150.         // Partial derivatives of azimuth/elevation with respect to state
  151.         // (beware element at index 0 is the value, not a derivative)
  152.         final double[] azDerivatives = azimuth.getGradient();
  153.         final double[] elDerivatives = elevation.getGradient();
  154.         estimated.setStateDerivatives(0,
  155.                                       Arrays.copyOfRange(azDerivatives, 0, 6), Arrays.copyOfRange(elDerivatives, 0, 6));

  157.         // Set partial derivatives with respect to parameters
  158.         // (beware element at index 0 is the value, not a derivative)
  159.         for (final ParameterDriver driver : getParametersDrivers()) {

  161.             for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
  162.                 final Integer index = common.getIndices().get(span.getData());
  163.                 if (index != null) {
  164.                     estimated.setParameterDerivatives(driver, span.getStart(), azDerivatives[index], elDerivatives[index]);
  165.                 }
  166.             }
  167.         }

  169.         return estimated;

  171.     }

  173.     /** Calculate the Line Of Sight of the given measurement.
  174.      * @param outputFrame output frame of the line of sight vector
  175.      * @return Vector3D the line of Sight of the measurement
  176.      */
  177.     public Vector3D getObservedLineOfSight(final Frame outputFrame) {
  178.         return getStation().getBaseFrame().getStaticTransformTo(outputFrame, getDate())
  179.             .transformVector(new Vector3D(MathUtils.SEMI_PI - getObservedValue()[0], getObservedValue()[1]));
  180.     }

  182. }
Created with JaCoCo 0.8.11.202310140853