Examples with ParameterDriver org.orekit.utils.ParameterDriver used on opensource projects

Example 56 with ParameterDriver

use of org.orekit.utils.ParameterDriver in project Orekit by CS-SI.

the class Model method fetchEvaluatedMeasurement.

/**
 * Fetch a measurement that was evaluated during propagation.
 * @param index index of the measurement first component
 * @param evaluation measurement evaluation
 * @exception OrekitException if Jacobians cannot be computed
 */
void fetchEvaluatedMeasurement(final int index, final EstimatedMeasurement<?> evaluation) throws OrekitException {
    // States and observed measurement
    final SpacecraftState[] evaluationStates = evaluation.getStates();
    final ObservedMeasurement<?> observedMeasurement = evaluation.getObservedMeasurement();
    evaluations.put(observedMeasurement, evaluation);
    if (evaluation.getStatus() == EstimatedMeasurement.Status.REJECTED) {
        return;
    }
    // compute weighted residuals
    final double[] evaluated = evaluation.getEstimatedValue();
    final double[] observed = observedMeasurement.getObservedValue();
    final double[] sigma = observedMeasurement.getTheoreticalStandardDeviation();
    final double[] weight = evaluation.getObservedMeasurement().getBaseWeight();
    for (int i = 0; i < evaluated.length; ++i) {
        value.setEntry(index + i, weight[i] * (evaluated[i] - observed[i]) / sigma[i]);
    }
    for (int k = 0; k < evaluationStates.length; ++k) {
        final int p = observedMeasurement.getPropagatorsIndices().get(k);
        // partial derivatives of the current Cartesian coordinates with respect to current orbital state
        final double[][] aCY = new double[6][6];
        final Orbit currentOrbit = evaluationStates[k].getOrbit();
        currentOrbit.getJacobianWrtParameters(builders[p].getPositionAngle(), aCY);
        final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);
        // Jacobian of the measurement with respect to current orbital state
        final RealMatrix dMdC = new Array2DRowRealMatrix(evaluation.getStateDerivatives(k), false);
        final RealMatrix dMdY = dMdC.multiply(dCdY);
        // Jacobian of the measurement with respect to initial orbital state
        final double[][] aYY0 = new double[6][6];
        mappers[p].getStateJacobian(evaluationStates[k], aYY0);
        final RealMatrix dYdY0 = new Array2DRowRealMatrix(aYY0, false);
        final RealMatrix dMdY0 = dMdY.multiply(dYdY0);
        for (int i = 0; i < dMdY0.getRowDimension(); ++i) {
            int jOrb = orbitsStartColumns[p];
            for (int j = 0; j < dMdY0.getColumnDimension(); ++j) {
                final ParameterDriver driver = builders[p].getOrbitalParametersDrivers().getDrivers().get(j);
                if (driver.isSelected()) {
                    jacobian.setEntry(index + i, jOrb++, weight[i] * dMdY0.getEntry(i, j) / sigma[i] * driver.getScale());
                }
            }
        }
        // Jacobian of the measurement with respect to propagation parameters
        final ParameterDriversList selectedPropagationDrivers = getSelectedPropagationDriversForBuilder(p);
        final int nbParams = selectedPropagationDrivers.getNbParams();
        if (nbParams > 0) {
            final double[][] aYPp = new double[6][nbParams];
            mappers[p].getParametersJacobian(evaluationStates[k], aYPp);
            final RealMatrix dYdPp = new Array2DRowRealMatrix(aYPp, false);
            final RealMatrix dMdPp = dMdY.multiply(dYdPp);
            for (int i = 0; i < dMdPp.getRowDimension(); ++i) {
                for (int j = 0; j < nbParams; ++j) {
                    final ParameterDriver delegating = selectedPropagationDrivers.getDrivers().get(j);
                    jacobian.addToEntry(index + i, propagationParameterColumns.get(delegating.getName()), weight[i] * dMdPp.getEntry(i, j) / sigma[i] * delegating.getScale());
                }
            }
        }
    }
    // Jacobian of the measurement with respect to measurements parameters
    for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
        if (driver.isSelected()) {
            final double[] aMPm = evaluation.getParameterDerivatives(driver);
            for (int i = 0; i < aMPm.length; ++i) {
                jacobian.setEntry(index + i, measurementParameterColumns.get(driver.getName()), weight[i] * aMPm[i] / sigma[i] * driver.getScale());
            }
        }
    }
}

Example 57 with ParameterDriver

use of org.orekit.utils.ParameterDriver in project Orekit by CS-SI.

the class Model method configureMeasurements.

/**
 * Configure the multi-satellites handler to handle measurements.
 * @param point evaluation point
 * @return multi-satellites handler to handle measurements
 * @exception OrekitException if measurements parameters cannot be set with the current point
 */
private MultiSatStepHandler configureMeasurements(final RealVector point) throws OrekitException {
    // Set up the measurement parameters
    int index = orbitsEndColumns[builders.length - 1] + propagationParameterColumns.size();
    for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
        parameter.setNormalizedValue(point.getEntry(index++));
    }
    // Set up measurements handler
    final List<PreCompensation> precompensated = new ArrayList<>();
    for (final ObservedMeasurement<?> measurement : measurements) {
        if (measurement.isEnabled()) {
            precompensated.add(new PreCompensation(measurement, evaluations.get(measurement)));
        }
    }
    precompensated.sort(new ChronologicalComparator());
    // Assign first and last date
    firstDate = precompensated.get(0).getDate();
    lastDate = precompensated.get(precompensated.size() - 1).getDate();
    // Reverse the list in case of backward propagation
    if (!forwardPropagation) {
        Collections.reverse(precompensated);
    }
    return new MeasurementHandler(this, precompensated);
}

Example 58 with ParameterDriver

use of org.orekit.utils.ParameterDriver in project Orekit by CS-SI.

the class AngularRaDec method theoreticalEvaluation.

/**
 * {@inheritDoc}
 */
@Override
protected EstimatedMeasurement<AngularRaDec> theoreticalEvaluation(final int iteration, final int evaluation, final SpacecraftState[] states) throws OrekitException {
    final SpacecraftState state = states[getPropagatorsIndices().get(0)];
    // Right Ascension/elevation (in reference frame )derivatives are computed with respect to spacecraft state in inertial frame
    // and station parameters
    // ----------------------
    // 
    // Parameters:
    // - 0..2 - Position of the spacecraft in inertial frame
    // - 3..5 - Velocity of the spacecraft in inertial frame
    // - 6..n - station parameters (station offsets, pole, prime meridian...)
    // Get the number of parameters used for derivation
    // Place the selected drivers into a map
    int nbParams = 6;
    final Map<String, Integer> indices = new HashMap<>();
    for (ParameterDriver driver : getParametersDrivers()) {
        if (driver.isSelected()) {
            indices.put(driver.getName(), nbParams++);
        }
    }
    final DSFactory factory = new DSFactory(nbParams, 1);
    final Field<DerivativeStructure> field = factory.getDerivativeField();
    final FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
    // Coordinates of the spacecraft expressed as a derivative structure
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pvaDS = getCoordinates(state, 0, factory);
    // Transform between station and inertial frame, expressed as a derivative structure
    // The components of station's position in offset frame are the 3 last derivative parameters
    final AbsoluteDate downlinkDate = getDate();
    final FieldAbsoluteDate<DerivativeStructure> downlinkDateDS = new FieldAbsoluteDate<>(field, downlinkDate);
    final FieldTransform<DerivativeStructure> offsetToInertialDownlink = station.getOffsetToInertial(state.getFrame(), downlinkDateDS, factory, indices);
    // Station position/velocity in inertial frame at end of the downlink leg
    final TimeStampedFieldPVCoordinates<DerivativeStructure> stationDownlink = offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS, zero, zero, zero));
    // Compute propagation times
    // (if state has already been set up to pre-compensate propagation delay,
    // we will have delta == tauD and transitState will be the same as state)
    // Downlink delay
    final DerivativeStructure tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS);
    // Transit state
    final double delta = downlinkDate.durationFrom(state.getDate());
    final DerivativeStructure deltaMTauD = tauD.negate().add(delta);
    final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue());
    // Transit state (re)computed with derivative structures
    final TimeStampedFieldPVCoordinates<DerivativeStructure> transitStateDS = pvaDS.shiftedBy(deltaMTauD);
    // Station-satellite vector expressed in inertial frame
    final FieldVector3D<DerivativeStructure> staSatInertial = transitStateDS.getPosition().subtract(stationDownlink.getPosition());
    // Field transform from inertial to reference frame at station's reception date
    final FieldTransform<DerivativeStructure> inertialToReferenceDownlink = state.getFrame().getTransformTo(referenceFrame, downlinkDateDS);
    // Station-satellite vector in reference frame
    final FieldVector3D<DerivativeStructure> staSatReference = inertialToReferenceDownlink.transformPosition(staSatInertial);
    // Compute right ascension and declination
    final DerivativeStructure baseRightAscension = staSatReference.getAlpha();
    final double twoPiWrap = MathUtils.normalizeAngle(baseRightAscension.getReal(), getObservedValue()[0]) - baseRightAscension.getReal();
    final DerivativeStructure rightAscension = baseRightAscension.add(twoPiWrap);
    final DerivativeStructure declination = staSatReference.getDelta();
    // Prepare the estimation
    final EstimatedMeasurement<AngularRaDec> estimated = new EstimatedMeasurement<>(this, iteration, evaluation, new SpacecraftState[] { transitState }, new TimeStampedPVCoordinates[] { transitStateDS.toTimeStampedPVCoordinates(), stationDownlink.toTimeStampedPVCoordinates() });
    // azimuth - elevation values
    estimated.setEstimatedValue(rightAscension.getValue(), declination.getValue());
    // Partial derivatives of right ascension/declination in reference frame with respect to state
    // (beware element at index 0 is the value, not a derivative)
    final double[] raDerivatives = rightAscension.getAllDerivatives();
    final double[] decDerivatives = declination.getAllDerivatives();
    estimated.setStateDerivatives(0, Arrays.copyOfRange(raDerivatives, 1, 7), Arrays.copyOfRange(decDerivatives, 1, 7));
    // (beware element at index 0 is the value, not a derivative)
    for (final ParameterDriver driver : getParametersDrivers()) {
        final Integer index = indices.get(driver.getName());
        if (index != null) {
            estimated.setParameterDerivatives(driver, raDerivatives[index + 1], decDerivatives[index + 1]);
        }
    }
    return estimated;
}

Example 59 with ParameterDriver

use of org.orekit.utils.ParameterDriver in project Orekit by CS-SI.

the class RangeRate method theoreticalEvaluation.

/**
 * {@inheritDoc}
 */
@Override
protected EstimatedMeasurement<RangeRate> theoreticalEvaluation(final int iteration, final int evaluation, final SpacecraftState[] states) throws OrekitException {
    final SpacecraftState state = states[getPropagatorsIndices().get(0)];
    // Range-rate derivatives are computed with respect to spacecraft state in inertial frame
    // and station position in station's offset frame
    // -------
    // 
    // Parameters:
    // - 0..2 - Position of the spacecraft in inertial frame
    // - 3..5 - Velocity of the spacecraft in inertial frame
    // - 6..n - station parameters (station offsets, pole, prime meridian...)
    int nbParams = 6;
    final Map<String, Integer> indices = new HashMap<>();
    for (ParameterDriver driver : getParametersDrivers()) {
        if (driver.isSelected()) {
            indices.put(driver.getName(), nbParams++);
        }
    }
    final DSFactory factory = new DSFactory(nbParams, 1);
    final Field<DerivativeStructure> field = factory.getDerivativeField();
    final FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
    // Coordinates of the spacecraft expressed as a derivative structure
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pvaDS = getCoordinates(state, 0, factory);
    // transform between station and inertial frame, expressed as a derivative structure
    // The components of station's position in offset frame are the 3 last derivative parameters
    final AbsoluteDate downlinkDate = getDate();
    final FieldAbsoluteDate<DerivativeStructure> downlinkDateDS = new FieldAbsoluteDate<>(field, downlinkDate);
    final FieldTransform<DerivativeStructure> offsetToInertialDownlink = station.getOffsetToInertial(state.getFrame(), downlinkDateDS, factory, indices);
    // Station position in inertial frame at end of the downlink leg
    final TimeStampedFieldPVCoordinates<DerivativeStructure> stationDownlink = offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS, zero, zero, zero));
    // Compute propagation times
    // (if state has already been set up to pre-compensate propagation delay,
    // we will have delta == tauD and transitState will be the same as state)
    // Downlink delay
    final DerivativeStructure tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS);
    // Transit state
    final double delta = downlinkDate.durationFrom(state.getDate());
    final DerivativeStructure deltaMTauD = tauD.negate().add(delta);
    final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue());
    // Transit state (re)computed with derivative structures
    final TimeStampedFieldPVCoordinates<DerivativeStructure> transitPV = pvaDS.shiftedBy(deltaMTauD);
    // one-way (downlink) range-rate
    final EstimatedMeasurement<RangeRate> evalOneWay1 = oneWayTheoreticalEvaluation(iteration, evaluation, true, stationDownlink, transitPV, transitState, indices);
    final EstimatedMeasurement<RangeRate> estimated;
    if (twoway) {
        // one-way (uplink) light time correction
        final AbsoluteDate approxUplinkDate = downlinkDate.shiftedBy(-2 * tauD.getValue());
        final FieldAbsoluteDate<DerivativeStructure> approxUplinkDateDS = new FieldAbsoluteDate<>(field, approxUplinkDate);
        final FieldTransform<DerivativeStructure> offsetToInertialApproxUplink = station.getOffsetToInertial(state.getFrame(), approxUplinkDateDS, factory, indices);
        final TimeStampedFieldPVCoordinates<DerivativeStructure> stationApproxUplink = offsetToInertialApproxUplink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(approxUplinkDateDS, zero, zero, zero));
        final DerivativeStructure tauU = signalTimeOfFlight(stationApproxUplink, transitPV.getPosition(), transitPV.getDate());
        final TimeStampedFieldPVCoordinates<DerivativeStructure> stationUplink = stationApproxUplink.shiftedBy(transitPV.getDate().durationFrom(approxUplinkDateDS).subtract(tauU));
        final EstimatedMeasurement<RangeRate> evalOneWay2 = oneWayTheoreticalEvaluation(iteration, evaluation, false, stationUplink, transitPV, transitState, indices);
        // combine uplink and downlink values
        estimated = new EstimatedMeasurement<>(this, iteration, evaluation, evalOneWay1.getStates(), new TimeStampedPVCoordinates[] { evalOneWay2.getParticipants()[0], evalOneWay1.getParticipants()[0], evalOneWay1.getParticipants()[1] });
        estimated.setEstimatedValue(0.5 * (evalOneWay1.getEstimatedValue()[0] + evalOneWay2.getEstimatedValue()[0]));
        // combine uplink and downlink partial derivatives with respect to state
        final double[][] sd1 = evalOneWay1.getStateDerivatives(0);
        final double[][] sd2 = evalOneWay2.getStateDerivatives(0);
        final double[][] sd = new double[sd1.length][sd1[0].length];
        for (int i = 0; i < sd.length; ++i) {
            for (int j = 0; j < sd[0].length; ++j) {
                sd[i][j] = 0.5 * (sd1[i][j] + sd2[i][j]);
            }
        }
        estimated.setStateDerivatives(0, sd);
        // combine uplink and downlink partial derivatives with respect to parameters
        evalOneWay1.getDerivativesDrivers().forEach(driver -> {
            final double[] pd1 = evalOneWay1.getParameterDerivatives(driver);
            final double[] pd2 = evalOneWay2.getParameterDerivatives(driver);
            final double[] pd = new double[pd1.length];
            for (int i = 0; i < pd.length; ++i) {
                pd[i] = 0.5 * (pd1[i] + pd2[i]);
            }
            estimated.setParameterDerivatives(driver, pd);
        });
    } else {
        estimated = evalOneWay1;
    }
    return estimated;
}

Example 60 with ParameterDriver

use of org.orekit.utils.ParameterDriver in project Orekit by CS-SI.

the class RangeRate method oneWayTheoreticalEvaluation.

/**
 * Evaluate measurement in one-way.
 * @param iteration iteration number
 * @param evaluation evaluations counter
 * @param downlink indicator for downlink leg
 * @param stationPV station coordinates when signal is at station
 * @param transitPV spacecraft coordinates at onboard signal transit
 * @param transitState orbital state at onboard signal transit
 * @param indices indices of the estimated parameters in derivatives computations
 * @return theoretical value
 * @exception OrekitException if value cannot be computed
 * @see #evaluate(SpacecraftStatet)
 */
private EstimatedMeasurement<RangeRate> oneWayTheoreticalEvaluation(final int iteration, final int evaluation, final boolean downlink, final TimeStampedFieldPVCoordinates<DerivativeStructure> stationPV, final TimeStampedFieldPVCoordinates<DerivativeStructure> transitPV, final SpacecraftState transitState, final Map<String, Integer> indices) throws OrekitException {
    // prepare the evaluation
    final EstimatedMeasurement<RangeRate> estimated = new EstimatedMeasurement<RangeRate>(this, iteration, evaluation, new SpacecraftState[] { transitState }, new TimeStampedPVCoordinates[] { (downlink ? transitPV : stationPV).toTimeStampedPVCoordinates(), (downlink ? stationPV : transitPV).toTimeStampedPVCoordinates() });
    // range rate value
    final FieldVector3D<DerivativeStructure> stationPosition = stationPV.getPosition();
    final FieldVector3D<DerivativeStructure> relativePosition = stationPosition.subtract(transitPV.getPosition());
    final FieldVector3D<DerivativeStructure> stationVelocity = stationPV.getVelocity();
    final FieldVector3D<DerivativeStructure> relativeVelocity = stationVelocity.subtract(transitPV.getVelocity());
    // radial direction
    final FieldVector3D<DerivativeStructure> lineOfSight = relativePosition.normalize();
    // range rate
    final DerivativeStructure rangeRate = FieldVector3D.dotProduct(relativeVelocity, lineOfSight);
    estimated.setEstimatedValue(rangeRate.getValue());
    // compute partial derivatives of (rr) with respect to spacecraft state Cartesian coordinates
    final double[] derivatives = rangeRate.getAllDerivatives();
    estimated.setStateDerivatives(0, Arrays.copyOfRange(derivatives, 1, 7));
    // (beware element at index 0 is the value, not a derivative)
    for (final ParameterDriver driver : getParametersDrivers()) {
        final Integer index = indices.get(driver.getName());
        if (index != null) {
            estimated.setParameterDerivatives(driver, derivatives[index + 1]);
        }
    }
    return estimated;
}