MultiplexedMeasurement.java

/* Copyright 2002-2025 CS GROUP
 * Licensed to CS GROUP (CS) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * CS licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.orekit.estimation.measurements;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.IdentityHashMap;
import java.util.List;
import java.util.Map;
import java.util.function.Function;

import org.orekit.propagation.SpacecraftState;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.ParameterDriversList;
import org.orekit.utils.TimeSpanMap;
import org.orekit.utils.TimeStampedPVCoordinates;
import org.orekit.utils.TimeSpanMap.Span;

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

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

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

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

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

    /** Multiplexed parameters drivers. */
    private final ParameterDriversList parametersDrivers;

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

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

    /** States mapping. */
    private final int[][] multiplexedToUnderlying;

    /** States mapping. */
    private final int[][] underlyingToMultiplexed;

    /** Simple constructor.
     * @param measurements measurements to multiplex
     * @since 10.1
     */
    public MultiplexedMeasurement(final List<ObservedMeasurement<?>> measurements) {
        super(measurements.get(0).getDate(),
              multiplex(measurements, ComparableMeasurement::getObservedValue),
              multiplex(measurements, ObservedMeasurement::getTheoreticalStandardDeviation),
              multiplex(measurements, ObservedMeasurement::getBaseWeight),
              multiplex(measurements));

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

        // gather parameters drivers
        int dim = 0;
        for (final ObservedMeasurement<?> m : measurements) {
            for (final ParameterDriver driver : m.getParametersDrivers()) {
                parametersDrivers.add(driver);
            }
            dim += m.getDimension();
        }
        parametersDrivers.sort();
        for (final ParameterDriver driver : parametersDrivers.getDrivers()) {
            addParameterDriver(driver);
        }
        this.dimension = dim;

        // set up states mappings for observed satellites
        final List<ObservableSatellite> deduplicated = getSatellites();
        this.nbSat   = deduplicated.size();
        this.multiplexedToUnderlying = new int[measurements.size()][];
        this.underlyingToMultiplexed = new int[measurements.size()][deduplicated.size()];
        for (int i = 0; i < multiplexedToUnderlying.length; ++i) {
            final List<ObservableSatellite> satellites = measurements.get(i).getSatellites();
            multiplexedToUnderlying[i] = new int[satellites.size()];
            for (int j = 0; j < multiplexedToUnderlying[i].length; ++j) {
                final int index = satellites.get(j).getPropagatorIndex();
                for (int k = 0; k < nbSat; ++k) {
                    if (deduplicated.get(k).getPropagatorIndex() == index) {
                        multiplexedToUnderlying[i][j] = k;
                        underlyingToMultiplexed[i][k] = j;
                        break;
                    }
                }
            }
        }

    }

    /** Get the underlying measurements.
     * @return underlying measurements
     */
    public List<ObservedMeasurement<?>> getMeasurements() {
        return observedMeasurements;
    }

    /** Get the underlying estimated measurements without derivatives.
     * @return underlying estimated measurements without derivatives
     * @since 12.0
     */
    public List<EstimatedMeasurementBase<?>> getEstimatedMeasurementsWithoutDerivatives() {
        return estimatedMeasurementsWithoutDerivatives;
    }

    /** Get the underlying estimated measurements.
     * @return underlying estimated measurements
     */
    public List<EstimatedMeasurement<?>> getEstimatedMeasurements() {
        return estimatedMeasurements;
    }

    /** Get the spacecraft state index in the underlying measurement.
     * @param measurementIndex index of the underlying measurement
     * @param multiplexedStateIndex index of the spacecraft state in the multiplexed array
     * @return spacecraft state index in the underlying measurement
     * @since 13.0
     */
    public int getUnderlyingStateIndex(final int measurementIndex, final int multiplexedStateIndex) {
        return multiplexedToUnderlying[measurementIndex][multiplexedStateIndex];
    }

    /** Get the spacecraft state index in the multiplexed measurement.
     * @param measurementIndex index of the underlying measurement
     * @param underlyingStateIndex index of the spacecraft state in the underlying array
     * @return spacecraft state index in the multiplexed measurement
     * @since 13.0
     */
    public int getMultiplexedStateIndex(final int measurementIndex, final int underlyingStateIndex) {
        return underlyingToMultiplexed[measurementIndex][underlyingStateIndex];
    }

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

        final SpacecraftState[]              evaluationStates = new SpacecraftState[nbSat];
        final double[]                       value            = new double[dimension];

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

            // filter states involved in the current measurement
            final SpacecraftState[] filteredStates = new SpacecraftState[multiplexedToUnderlying[i].length];
            for (int j = 0; j < multiplexedToUnderlying[i].length; ++j) {
                filteredStates[j] = states[getUnderlyingStateIndex(i, j)];
            }

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

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

            // extract states
            final SpacecraftState[] statesI = eI.getStates();
            for (int j = 0; j < multiplexedToUnderlying[i].length; ++j) {
                evaluationStates[multiplexedToUnderlying[i][j]] = statesI[j];
            }

        }

        // create multiplexed estimation
        final EstimatedMeasurementBase<MultiplexedMeasurement> multiplexed =
                        new EstimatedMeasurementBase<>(this, iteration, evaluation,
                                                       evaluationStates,
                                                       new TimeStampedPVCoordinates[0]);

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

        return multiplexed;

    }

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

        final SpacecraftState[]              evaluationStates = new SpacecraftState[nbSat];
        final double[]                       value            = new double[dimension];

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

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

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

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

            // extract states
            final SpacecraftState[] statesI = eI.getStates();
            for (int j = 0; j < multiplexedToUnderlying[i].length; ++j) {
                evaluationStates[multiplexedToUnderlying[i][j]] = statesI[j];
            }

        }

        // create multiplexed estimation
        final EstimatedMeasurement<MultiplexedMeasurement> multiplexed =
                        new EstimatedMeasurement<>(this, iteration, evaluation,
                                                   evaluationStates,
                                                   new TimeStampedPVCoordinates[0]);

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

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

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

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

            // state derivatives
            for (int j = 0; j < multiplexedToUnderlying[i].length; ++j) {
                System.arraycopy(eI.getStateDerivatives(j), 0,
                                 stateDerivatives[multiplexedToUnderlying[i][j]], index,
                                 dimI);
            }

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

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

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

                    double[] derivatives = parametersDerivatives.get(delegating).get(span.getStart());
                    if (derivatives == null) {
                        derivatives = new double[dimension];
                    }
                    if (!parametersDerivatives.get(delegating).getSpan(span.getStart()).getStart().equals(span.getStart())) {
                        if ((span.getStart()).equals(AbsoluteDate.PAST_INFINITY)) {
                            parametersDerivatives.get(delegating).addValidBefore(derivatives, span.getEnd(), false);
                        } else {
                            parametersDerivatives.get(delegating).addValidAfter(derivatives, span.getStart(), false);
                        }

                    }

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

                }

            });

            index += dimI;

        }

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

        // set parameters derivatives
        parametersDerivatives.
            entrySet().
            forEach(e -> multiplexed.setParameterDerivatives(e.getKey(), e.getValue()));

        return multiplexed;

    }

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

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

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

        return multiplexed;

    }

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

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

        // gather all satellites, removing duplicates
        for (final ObservedMeasurement<?> measurement : measurements) {
            for (final ObservableSatellite satellite : measurement.getSatellites()) {
                boolean searching = true;
                for (int i = 0; i < satellites.size() && searching; ++i) {
                    // check if we already know this satellite
                    searching = satellite.getPropagatorIndex() != satellites.get(i).getPropagatorIndex();
                }
                if (searching) {
                    // this is a new satellite, add it to the global list
                    satellites.add(satellite);
                }
            }
        }

        return satellites;

    }

}