MultiplexedMeasurement.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;

  19. import java.util.ArrayList;
  20. import java.util.Arrays;
  21. import java.util.IdentityHashMap;
  22. import java.util.List;
  23. import java.util.Map;
  24. import java.util.function.Function;

  26. import org.orekit.propagation.SpacecraftState;
  27. import org.orekit.time.AbsoluteDate;
  28. import org.orekit.utils.ParameterDriver;
  29. import org.orekit.utils.ParameterDriversList;
  30. import org.orekit.utils.TimeSpanMap;
  31. import org.orekit.utils.TimeStampedPVCoordinates;
  32. import org.orekit.utils.TimeSpanMap.Span;

  34. /** Class multiplexing several measurements as one.
  35.  * <p>
  36.  * Date comes from the first measurement, observed and estimated
  37.  * values result from gathering all underlying measurements values.
  38.  *
  39.  * @author Luc Maisonobe
  40.  * @since 10.1
  41.  */
  42. public class MultiplexedMeasurement extends AbstractMeasurement<MultiplexedMeasurement> {

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

  47.     /** Multiplexed measurements. */
  48.     private final List<ObservedMeasurement<?>> observedMeasurements;

  50.     /** Multiplexed measurements without derivatives.
  51.      */
  52.     private final List<EstimatedMeasurementBase<?>> estimatedMeasurementsWithoutDerivatives;

  54.     /** Multiplexed measurements. */
  55.     private final List<EstimatedMeasurement<?>> estimatedMeasurements;

  57.     /** Multiplexed parameters drivers. */
  58.     private ParameterDriversList parametersDrivers;

  60.     /** Total dimension. */
  61.     private final int dimension;

  63.     /** Total number of satellites involved. */
  64.     private final int nbSat;

  66.     /** States mapping. */
  67.     private final int[][] mapping;

  69.     /** Simple constructor.
  70.      * @param measurements measurements to multiplex
  71.      * @since 10.1
  72.      */
  73.     public MultiplexedMeasurement(final List<ObservedMeasurement<?>> measurements) {
  74.         super(measurements.get(0).getDate(),
  75.               multiplex(measurements, m -> m.getObservedValue()),
  76.               multiplex(measurements, m -> m.getTheoreticalStandardDeviation()),
  77.               multiplex(measurements, m -> m.getBaseWeight()),
  78.               multiplex(measurements));

  80.         this.observedMeasurements                    = measurements;
  81.         this.estimatedMeasurementsWithoutDerivatives = new ArrayList<>();
  82.         this.estimatedMeasurements                   = new ArrayList<>();
  83.         this.parametersDrivers                       = new ParameterDriversList();

  85.         // gather parameters drivers
  86.         int dim = 0;
  87.         for (final ObservedMeasurement<?> m : measurements) {
  88.             for (final ParameterDriver driver : m.getParametersDrivers()) {
  89.                 parametersDrivers.add(driver);
  90.             }
  91.             dim += m.getDimension();
  92.         }
  93.         parametersDrivers.sort();
  94.         for (final ParameterDriver driver : parametersDrivers.getDrivers()) {
  95.             addParameterDriver(driver);
  96.         }
  97.         this.dimension = dim;

  99.         // set up states mappings for observed satellites
  100.         final List<ObservableSatellite> deduplicated = getSatellites();
  101.         this.nbSat   = deduplicated.size();
  102.         this.mapping = new int[measurements.size()][];
  103.         for (int i = 0; i < mapping.length; ++i) {
  104.             final List<ObservableSatellite> satellites = measurements.get(i).getSatellites();
  105.             mapping[i] = new int[satellites.size()];
  106.             for (int j = 0; j < mapping[i].length; ++j) {
  107.                 final int index = satellites.get(j).getPropagatorIndex();
  108.                 for (int k = 0; k < nbSat; ++k) {
  109.                     if (deduplicated.get(k).getPropagatorIndex() == index) {
  110.                         mapping[i][j] = k;
  111.                         break;
  112.                     }
  113.                 }
  114.             }
  115.         }

  117.     }

  119.     /** Get the underlying measurements.
  120.      * @return underlying measurements
  121.      */
  122.     public List<ObservedMeasurement<?>> getMeasurements() {
  123.         return observedMeasurements;
  124.     }

  126.     /** Get the underlying estimated measurements without derivatives.
  127.      * @return underlying estimated measurements without derivatives
  128.      * @since 12.0
  129.      */
  130.     public List<EstimatedMeasurementBase<?>> getEstimatedMeasurementsWithoutDerivatives() {
  131.         return estimatedMeasurementsWithoutDerivatives;
  132.     }

  134.     /** Get the underlying estimated measurements.
  135.      * @return underlying estimated measurements
  136.      */
  137.     public List<EstimatedMeasurement<?>> getEstimatedMeasurements() {
  138.         return estimatedMeasurements;
  139.     }

  141.     /** {@inheritDoc} */
  142.     @Override
  143.     protected EstimatedMeasurementBase<MultiplexedMeasurement> theoreticalEvaluationWithoutDerivatives(final int iteration,
  144.                                                                                                        final int evaluation,
  145.                                                                                                        final SpacecraftState[] states) {

  147.         final SpacecraftState[]              evaluationStates = new SpacecraftState[nbSat];
  148.         final List<TimeStampedPVCoordinates> participants     = new ArrayList<>();
  149.         final double[]                       value            = new double[dimension];

  151.         // loop over all multiplexed measurements
  152.         estimatedMeasurementsWithoutDerivatives.clear();
  153.         int index = 0;
  154.         for (int i = 0; i < observedMeasurements.size(); ++i) {

  156.             // filter states involved in the current measurement
  157.             final SpacecraftState[] filteredStates = new SpacecraftState[mapping[i].length];
  158.             for (int j = 0; j < mapping[i].length; ++j) {
  159.                 filteredStates[j] = states[mapping[i][j]];
  160.             }

  162.             // perform evaluation
  163.             final EstimatedMeasurementBase<?> eI = observedMeasurements.get(i).estimateWithoutDerivatives(iteration, evaluation, filteredStates);
  164.             estimatedMeasurementsWithoutDerivatives.add(eI);

  166.             // extract results
  167.             final double[] valueI = eI.getEstimatedValue();
  168.             System.arraycopy(valueI, 0, value, index, valueI.length);
  169.             index += valueI.length;

  171.             // extract states
  172.             final SpacecraftState[] statesI = eI.getStates();
  173.             for (int j = 0; j < mapping[i].length; ++j) {
  174.                 evaluationStates[mapping[i][j]] = statesI[j];
  175.             }

  177.         }

  179.         // create multiplexed estimation
  180.         final EstimatedMeasurementBase<MultiplexedMeasurement> multiplexed =
  181.                         new EstimatedMeasurementBase<>(this, iteration, evaluation,
  182.                                                        evaluationStates,
  183.                                                        participants.toArray(new TimeStampedPVCoordinates[0]));

  185.         // copy multiplexed value
  186.         multiplexed.setEstimatedValue(value);

  188.         return multiplexed;

  190.     }

  192.     /** {@inheritDoc} */
  193.     @Override
  194.     protected EstimatedMeasurement<MultiplexedMeasurement> theoreticalEvaluation(final int iteration, final int evaluation,
  195.                                                                                  final SpacecraftState[] states) {

  197.         final SpacecraftState[]              evaluationStates = new SpacecraftState[nbSat];
  198.         final List<TimeStampedPVCoordinates> participants     = new ArrayList<>();
  199.         final double[]                       value            = new double[dimension];

  201.         // loop over all multiplexed measurements
  202.         estimatedMeasurements.clear();
  203.         int index = 0;
  204.         for (int i = 0; i < observedMeasurements.size(); ++i) {

  206.             // filter states involved in the current measurement
  207.             final SpacecraftState[] filteredStates = new SpacecraftState[mapping[i].length];
  208.             for (int j = 0; j < mapping[i].length; ++j) {
  209.                 filteredStates[j] = states[mapping[i][j]];
  210.             }

  212.             // perform evaluation
  213.             final EstimatedMeasurement<?> eI = observedMeasurements.get(i).estimate(iteration, evaluation, filteredStates);
  214.             estimatedMeasurements.add(eI);

  216.             // extract results
  217.             final double[] valueI = eI.getEstimatedValue();
  218.             System.arraycopy(valueI, 0, value, index, valueI.length);
  219.             index += valueI.length;

  221.             // extract states
  222.             final SpacecraftState[] statesI = eI.getStates();
  223.             for (int j = 0; j < mapping[i].length; ++j) {
  224.                 evaluationStates[mapping[i][j]] = statesI[j];
  225.             }

  227.         }

  229.         // create multiplexed estimation
  230.         final EstimatedMeasurement<MultiplexedMeasurement> multiplexed =
  231.                         new EstimatedMeasurement<>(this, iteration, evaluation,
  232.                                                    evaluationStates,
  233.                                                    participants.toArray(new TimeStampedPVCoordinates[0]));

  235.         // copy multiplexed value
  236.         multiplexed.setEstimatedValue(value);

  238.         // combine derivatives
  239.         final int                            stateSize             = estimatedMeasurements.get(0).getStateSize();
  240.         final double[]                       zeroDerivative        = new double[stateSize];
  241.         final double[][][]                   stateDerivatives      = new double[nbSat][dimension][];
  242.         for (final double[][] m : stateDerivatives) {
  243.             Arrays.fill(m, zeroDerivative);
  244.         }

  246.         final Map<ParameterDriver, TimeSpanMap<double[]>> parametersDerivatives = new IdentityHashMap<>();
  247.         index = 0;
  248.         for (int i = 0; i < observedMeasurements.size(); ++i) {

  250.             final EstimatedMeasurement<?> eI   = estimatedMeasurements.get(i);
  251.             final int                     idx  = index;
  252.             final int                     dimI = eI.getObservedMeasurement().getDimension();

  254.             // state derivatives
  255.             for (int j = 0; j < mapping[i].length; ++j) {
  256.                 System.arraycopy(eI.getStateDerivatives(j), 0,
  257.                                  stateDerivatives[mapping[i][j]], index,
  258.                                  dimI);
  259.             }

  261.             // parameters derivatives
  262.             eI.getDerivativesDrivers().forEach(driver -> {
  263.                 final ParameterDriversList.DelegatingDriver delegating = parametersDrivers.findByName(driver.getName());

  265.                 if (parametersDerivatives.get(delegating) == null) {
  266.                     final TimeSpanMap<double[]> derivativeSpanMap = new TimeSpanMap<double[]>(new double[dimension]);
  267.                     parametersDerivatives.put(delegating, derivativeSpanMap);
  268.                 }

  270.                 final TimeSpanMap<Double> driverNameSpan = delegating.getValueSpanMap();
  271.                 for (Span<Double> span = driverNameSpan.getSpan(driverNameSpan.getFirstSpan().getEnd()); span != null; span = span.next()) {

  273.                     double[] derivatives = parametersDerivatives.get(delegating).get(span.getStart());
  274.                     if (derivatives == null) {
  275.                         derivatives = new double[dimension];
  276.                     }
  277.                     if (!parametersDerivatives.get(delegating).getSpan(span.getStart()).getStart().equals(span.getStart())) {
  278.                         if ((span.getStart()).equals(AbsoluteDate.PAST_INFINITY)) {
  279.                             parametersDerivatives.get(delegating).addValidBefore(derivatives, span.getEnd(), false);
  280.                         } else {
  281.                             parametersDerivatives.get(delegating).addValidAfter(derivatives, span.getStart(), false);
  282.                         }

  284.                     }

  286.                     System.arraycopy(eI.getParameterDerivatives(driver, span.getStart()), 0, derivatives, idx, dimI);

  288.                 }

  290.             });

  292.             index += dimI;

  294.         }

  296.         // set states derivatives
  297.         for (int i = 0; i < nbSat; ++i) {
  298.             multiplexed.setStateDerivatives(i, stateDerivatives[i]);
  299.         }

  301.         // set parameters derivatives
  302.         parametersDerivatives.
  303.             entrySet().
  304.             stream().
  305.             forEach(e -> multiplexed.setParameterDerivatives(e.getKey(), e.getValue()));

  307.         return multiplexed;

  309.     }

  311.     /** Multiplex measurements data.
  312.      * @param measurements measurements to multiplex
  313.      * @param extractor data extraction function
  314.      * @return multiplexed data
  315.      */
  316.     private static double[] multiplex(final List<ObservedMeasurement<?>> measurements,
  317.                                       final Function<ObservedMeasurement<?>, double[]> extractor) {

  319.         // gather individual parts
  320.         final List<double[]> parts = new ArrayList<> (measurements.size());
  321.         int n = 0;
  322.         for (final ObservedMeasurement<?> measurement : measurements) {
  323.             final double[] p = extractor.apply(measurement);
  324.             parts.add(p);
  325.             n += p.length;
  326.         }

  328.         // create multiplexed data
  329.         final double[] multiplexed = new double[n];
  330.         int index = 0;
  331.         for (final double[] p : parts) {
  332.             System.arraycopy(p, 0, multiplexed, index, p.length);
  333.             index += p.length;
  334.         }

  336.         return multiplexed;

  338.     }

  340.     /** Multiplex satellites data.
  341.      * @param measurements measurements to multiplex
  342.      * @return multiplexed satellites data
  343.      */
  344.     private static List<ObservableSatellite> multiplex(final List<ObservedMeasurement<?>> measurements) {

  346.         final List<ObservableSatellite> satellites = new ArrayList<>();

  348.         // gather all satellites, removing duplicates
  349.         for (final ObservedMeasurement<?> measurement : measurements) {
  350.             for (final ObservableSatellite satellite : measurement.getSatellites()) {
  351.                 boolean searching = true;
  352.                 for (int i = 0; i < satellites.size() && searching; ++i) {
  353.                     // check if we already know this satellite
  354.                     searching = satellite.getPropagatorIndex() != satellites.get(i).getPropagatorIndex();
  355.                 }
  356.                 if (searching) {
  357.                     // this is a new satellite, add it to the global list
  358.                     satellites.add(satellite);
  359.                 }
  360.             }
  361.         }

  363.         return satellites;

  365.     }

  367. }
Created with JaCoCo 0.8.12.202403310830