Examples with Transform org.orekit.frames.Transform used on opensource projects

Example 11 with Transform

use of org.orekit.frames.Transform in project Orekit by CS-SI.

the class RangeAnalytic method theoreticalEvaluationAnalytic.

/**
 * Analytical version of the theoreticalEvaluation function in Range class
 * The derivative structures are not used, an analytical computation is used instead.
 * @param iteration current LS estimator iteration
 * @param evaluation current LS estimator evaluation
 * @param state spacecraft state. At measurement date on first iteration then close to emission date on further iterations
 * @param interpolator Orekit step interpolator
 * @return
 * @throws OrekitException
 */
protected EstimatedMeasurement<Range> theoreticalEvaluationAnalytic(final int iteration, final int evaluation, final SpacecraftState state) throws OrekitException {
    // Station attribute from parent Range class
    final GroundStation groundStation = this.getStation();
    // Station position at signal arrival
    final AbsoluteDate downlinkDate = getDate();
    final Transform topoToInertDownlink = groundStation.getOffsetToInertial(state.getFrame(), downlinkDate);
    final TimeStampedPVCoordinates stationDownlink = topoToInertDownlink.transformPVCoordinates(new TimeStampedPVCoordinates(downlinkDate, PVCoordinates.ZERO));
    // Take propagation time into account
    // (if state has already been set up to pre-compensate propagation delay,
    // we will have offset == downlinkDelay and transitState will be
    // the same as state)
    // Downlink time of flight
    final double tauD = signalTimeOfFlight(state.getPVCoordinates(), stationDownlink.getPosition(), downlinkDate);
    final double delta = downlinkDate.durationFrom(state.getDate());
    final double dt = delta - tauD;
    // Transit state position
    final SpacecraftState transitState = state.shiftedBy(dt);
    final AbsoluteDate transitDate = transitState.getDate();
    final Vector3D transitP = transitState.getPVCoordinates().getPosition();
    // Station position at transit state date
    final Transform topoToInertAtTransitDate = groundStation.getOffsetToInertial(state.getFrame(), transitDate);
    final TimeStampedPVCoordinates stationAtTransitDate = topoToInertAtTransitDate.transformPVCoordinates(new TimeStampedPVCoordinates(transitDate, PVCoordinates.ZERO));
    // Uplink time of flight
    final double tauU = signalTimeOfFlight(stationAtTransitDate, transitP, transitDate);
    final double tau = tauD + tauU;
    // Real date and position of station at signal departure
    final AbsoluteDate uplinkDate = downlinkDate.shiftedBy(-tau);
    final TimeStampedPVCoordinates stationUplink = topoToInertDownlink.shiftedBy(-tau).transformPVCoordinates(new TimeStampedPVCoordinates(uplinkDate, PVCoordinates.ZERO));
    // Prepare the evaluation
    final EstimatedMeasurement<Range> estimated = new EstimatedMeasurement<Range>(this, iteration, evaluation, new SpacecraftState[] { transitState }, new TimeStampedPVCoordinates[] { stationUplink, transitState.getPVCoordinates(), stationDownlink });
    // Set range value
    final double cOver2 = 0.5 * Constants.SPEED_OF_LIGHT;
    estimated.setEstimatedValue(tau * cOver2);
    // Partial derivatives with respect to state
    // The formulas below take into account the fact the measurement is at fixed reception date.
    // When spacecraft position is changed, the downlink delay is changed, and in order
    // to still have the measurement arrive at exactly the same date on ground, we must
    // take the spacecraft-station relative velocity into account.
    final Vector3D v = state.getPVCoordinates().getVelocity();
    final Vector3D qv = stationDownlink.getVelocity();
    final Vector3D downInert = stationDownlink.getPosition().subtract(transitP);
    final double dDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tauD - Vector3D.dotProduct(downInert, v);
    final Vector3D upInert = transitP.subtract(stationUplink.getPosition());
    // test
    // final double   dUp       = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tauU -
    // Vector3D.dotProduct(upInert, qv);
    // test
    final double dUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tauU - Vector3D.dotProduct(upInert, stationUplink.getVelocity());
    // derivatives of the downlink time of flight
    final double dTauDdPx = -downInert.getX() / dDown;
    final double dTauDdPy = -downInert.getY() / dDown;
    final double dTauDdPz = -downInert.getZ() / dDown;
    // Derivatives of the uplink time of flight
    final double dTauUdPx = 1. / dUp * upInert.dotProduct(Vector3D.PLUS_I.add((qv.subtract(v)).scalarMultiply(dTauDdPx)));
    final double dTauUdPy = 1. / dUp * upInert.dotProduct(Vector3D.PLUS_J.add((qv.subtract(v)).scalarMultiply(dTauDdPy)));
    final double dTauUdPz = 1. / dUp * upInert.dotProduct(Vector3D.PLUS_K.add((qv.subtract(v)).scalarMultiply(dTauDdPz)));
    // derivatives of the range measurement
    final double dRdPx = (dTauDdPx + dTauUdPx) * cOver2;
    final double dRdPy = (dTauDdPy + dTauUdPy) * cOver2;
    final double dRdPz = (dTauDdPz + dTauUdPz) * cOver2;
    estimated.setStateDerivatives(0, new double[] { dRdPx, dRdPy, dRdPz, dRdPx * dt, dRdPy * dt, dRdPz * dt });
    if (groundStation.getEastOffsetDriver().isSelected() || groundStation.getNorthOffsetDriver().isSelected() || groundStation.getZenithOffsetDriver().isSelected()) {
        // Downlink tme of flight derivatives / station position in topocentric frame
        final AngularCoordinates ac = topoToInertDownlink.getAngular().revert();
        // final Rotation rotTopoToInert = ac.getRotation();
        final Vector3D omega = ac.getRotationRate();
        // Inertial frame
        final double dTauDdQIx = downInert.getX() / dDown;
        final double dTauDdQIy = downInert.getY() / dDown;
        final double dTauDdQIz = downInert.getZ() / dDown;
        // Uplink tme of flight derivatives / station position in topocentric frame
        // Inertial frame
        final double dTauUdQIx = 1 / dUp * upInert.dotProduct(Vector3D.MINUS_I.add((qv.subtract(v)).scalarMultiply(dTauDdQIx)).subtract(Vector3D.PLUS_I.crossProduct(omega).scalarMultiply(tau)));
        final double dTauUdQIy = 1 / dUp * upInert.dotProduct(Vector3D.MINUS_J.add((qv.subtract(v)).scalarMultiply(dTauDdQIy)).subtract(Vector3D.PLUS_J.crossProduct(omega).scalarMultiply(tau)));
        final double dTauUdQIz = 1 / dUp * upInert.dotProduct(Vector3D.MINUS_K.add((qv.subtract(v)).scalarMultiply(dTauDdQIz)).subtract(Vector3D.PLUS_K.crossProduct(omega).scalarMultiply(tau)));
        // Range partial derivatives
        // with respect to station position in inertial frame
        final Vector3D dRdQI = new Vector3D((dTauDdQIx + dTauUdQIx) * cOver2, (dTauDdQIy + dTauUdQIy) * cOver2, (dTauDdQIz + dTauUdQIz) * cOver2);
        // convert to topocentric frame, as the station position
        // offset parameter is expressed in this frame
        final Vector3D dRdQT = ac.getRotation().applyTo(dRdQI);
        if (groundStation.getEastOffsetDriver().isSelected()) {
            estimated.setParameterDerivatives(groundStation.getEastOffsetDriver(), dRdQT.getX());
        }
        if (groundStation.getNorthOffsetDriver().isSelected()) {
            estimated.setParameterDerivatives(groundStation.getNorthOffsetDriver(), dRdQT.getY());
        }
        if (groundStation.getZenithOffsetDriver().isSelected()) {
            estimated.setParameterDerivatives(groundStation.getZenithOffsetDriver(), dRdQT.getZ());
        }
    }
    return estimated;
}

Example 12 with Transform

use of org.orekit.frames.Transform in project Orekit by CS-SI.

the class HolmesFeatherstoneAttractionModel method accelerationWrtState.

/**
 * Compute acceleration derivatives with respect to state parameters.
 * <p>
 * From a theoretical point of view, this method computes the same values
 * as {@link #acceleration(FieldSpacecraftState, RealFieldElement[])} in the
 * specific case of {@link DerivativeStructure} with respect to state, so
 * it is less general. However, it is *much* faster in this important case.
 * <p>
 * <p>
 * The derivatives should be computed with respect to position. The input
 * parameters already take into account the free parameters (6 or 7 depending
 * on derivation with respect to mass being considered or not) and order
 * (always 1). Free parameters at indices 0, 1 and 2 correspond to derivatives
 * with respect to position. Free parameters at indices 3, 4 and 5 correspond
 * to derivatives with respect to velocity (these derivatives will remain zero
 * as acceleration due to gravity does not depend on velocity). Free parameter
 * at index 6 (if present) corresponds to to derivatives with respect to mass
 * (this derivative will remain zero as acceleration due to gravity does not
 * depend on mass).
 * </p>
 * @param date current date
 * @param frame inertial reference frame for state (both orbit and attitude)
 * @param position position of spacecraft in inertial frame
 * @param mu central attraction coefficient to use
 * @return acceleration with all derivatives specified by the input parameters
 * own derivatives
 * @exception OrekitException if derivatives cannot be computed
 * @since 6.0
 */
private FieldVector3D<DerivativeStructure> accelerationWrtState(final AbsoluteDate date, final Frame frame, final FieldVector3D<DerivativeStructure> position, final DerivativeStructure mu) throws OrekitException {
    // get the position in body frame
    final Transform fromBodyFrame = bodyFrame.getTransformTo(frame, date);
    final Transform toBodyFrame = fromBodyFrame.getInverse();
    final Vector3D positionBody = toBodyFrame.transformPosition(position.toVector3D());
    // compute gradient and Hessian
    final GradientHessian gh = gradientHessian(date, positionBody, mu.getReal());
    // gradient of the non-central part of the gravity field
    final double[] gInertial = fromBodyFrame.transformVector(new Vector3D(gh.getGradient())).toArray();
    // Hessian of the non-central part of the gravity field
    final RealMatrix hBody = new Array2DRowRealMatrix(gh.getHessian(), false);
    final RealMatrix rot = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix());
    final RealMatrix hInertial = rot.transpose().multiply(hBody).multiply(rot);
    // distribute all partial derivatives in a compact acceleration vector
    final double[] derivatives = new double[1 + position.getX().getFreeParameters()];
    final DerivativeStructure[] accDer = new DerivativeStructure[3];
    for (int i = 0; i < 3; ++i) {
        // first element is value of acceleration (i.e. gradient of field)
        derivatives[0] = gInertial[i];
        // next three elements are one row of the Jacobian of acceleration (i.e. Hessian of field)
        derivatives[1] = hInertial.getEntry(i, 0);
        derivatives[2] = hInertial.getEntry(i, 1);
        derivatives[3] = hInertial.getEntry(i, 2);
        // next element is derivative with respect to parameter mu
        if (derivatives.length > 4 && isVariable(mu, 3)) {
            derivatives[4] = gInertial[i] / mu.getReal();
        }
        accDer[i] = position.getX().getFactory().build(derivatives);
    }
    return new FieldVector3D<>(accDer);
}

Example 13 with Transform

use of org.orekit.frames.Transform in project Orekit by CS-SI.

the class Atmosphere method getVelocity.

/**
 * Get the inertial velocity of atmosphere molecules.
 * @param date current date
 * @param position current position in frame
 * @param frame the frame in which is defined the position
 * @param <T> instance of RealFieldElement
 * @return velocity (m/s) (defined in the same frame as the position)
 * @exception OrekitException if some conversion cannot be performed
 */
default <T extends RealFieldElement<T>> FieldVector3D<T> getVelocity(FieldAbsoluteDate<T> date, FieldVector3D<T> position, Frame frame) throws OrekitException {
    final Transform bodyToFrame = getFrame().getTransformTo(frame, date.toAbsoluteDate());
    final FieldVector3D<T> posInBody = bodyToFrame.getInverse().transformPosition(position);
    final FieldPVCoordinates<T> pvBody = new FieldPVCoordinates<>(posInBody, FieldVector3D.getZero(position.getX().getField()));
    final FieldPVCoordinates<T> pvFrame = bodyToFrame.transformPVCoordinates(pvBody);
    return pvFrame.getVelocity();
}

Example 14 with Transform

use of org.orekit.frames.Transform in project Orekit by CS-SI.

the class Atmosphere method getVelocity.

/**
 * Get the inertial velocity of atmosphere molecules.
 * <p>By default, atmosphere is supposed to have a null
 * velocity in the central body frame.</p>
 *
 * @param date current date
 * @param position current position in frame
 * @param frame the frame in which is defined the position
 * @return velocity (m/s) (defined in the same frame as the position)
 * @exception OrekitException if some conversion cannot be performed
 */
default Vector3D getVelocity(AbsoluteDate date, Vector3D position, Frame frame) throws OrekitException {
    final Transform bodyToFrame = getFrame().getTransformTo(frame, date);
    final Vector3D posInBody = bodyToFrame.getInverse().transformPosition(position);
    final PVCoordinates pvBody = new PVCoordinates(posInBody, Vector3D.ZERO);
    final PVCoordinates pvFrame = bodyToFrame.transformPVCoordinates(pvBody);
    return pvFrame.getVelocity();
}

Example 15 with Transform

use of org.orekit.frames.Transform in project Orekit by CS-SI.

the class DragForce method getDensityWrtStateUsingFiniteDifferences.

/**
 * Compute density and its derivatives.
 * Using finite differences for the derivatives.
 * And doing the actual computation only for the derivatives with respect to position (others are set to 0.).
 * <p>
 * From a theoretical point of view, this method computes the same values
 * as {@link Atmosphere#getDensity(FieldAbsoluteDate, FieldVector3D, Frame)} in the
 * specific case of {@link DerivativeStructure} with respect to state, so
 * it is less general. However, it is *much* faster in this important case.
 * <p>
 * <p>
 * The derivatives should be computed with respect to position. The input
 * parameters already take into account the free parameters (6, 7 or 8 depending
 * on derivation with respect to drag coefficient and lift ratio being considered or not)
 * and order (always 1). Free parameters at indices 0, 1 and 2 correspond to derivatives
 * with respect to position. Free parameters at indices 3, 4 and 5 correspond
 * to derivatives with respect to velocity (these derivatives will remain zero
 * as the atmospheric density does not depend on velocity). Free parameter
 * at indexes 6 and 7 (if present) corresponds to derivatives with respect to drag coefficient
 * and/or lift ratio (one of these or both).
 * This 2 last derivatives will remain zero as atmospheric density does not depend on them.
 * </p>
 * @param date current date
 * @param frame inertial reference frame for state (both orbit and attitude)
 * @param position position of spacecraft in inertial frame
 * @param <T> type of the elements
 * @return the density and its derivatives
 * @exception OrekitException if derivatives cannot be computed
 * @since 9.0
 */
private <T extends RealFieldElement<T>> T getDensityWrtStateUsingFiniteDifferences(final AbsoluteDate date, final Frame frame, final FieldVector3D<T> position) throws OrekitException {
    // Retrieve derivation properties for parameter T
    // It is implied here that T is a DerivativeStructure
    // With order 1 and 6, 7 or 8 free parameters
    // This is all checked before in method isStateDerivatives
    final DSFactory factory = ((DerivativeStructure) position.getX()).getFactory();
    // Build a DerivativeStructure using only derivatives with respect to position
    final DSFactory factory3 = new DSFactory(3, 1);
    final FieldVector3D<DerivativeStructure> position3 = new FieldVector3D<>(factory3.variable(0, position.getX().getReal()), factory3.variable(1, position.getY().getReal()), factory3.variable(2, position.getZ().getReal()));
    // Get atmosphere properties in atmosphere own frame
    final Frame atmFrame = atmosphere.getFrame();
    final Transform toBody = frame.getTransformTo(atmFrame, date);
    final FieldVector3D<DerivativeStructure> posBodyDS = toBody.transformPosition(position3);
    final Vector3D posBody = posBodyDS.toVector3D();
    // Estimate density model by finite differences and composition
    // Using a delta of 1m
    final double delta = 1.0;
    final double x = posBody.getX();
    final double y = posBody.getY();
    final double z = posBody.getZ();
    final double rho0 = atmosphere.getDensity(date, posBody, atmFrame);
    final double dRhodX = (atmosphere.getDensity(date, new Vector3D(x + delta, y, z), atmFrame) - rho0) / delta;
    final double dRhodY = (atmosphere.getDensity(date, new Vector3D(x, y + delta, z), atmFrame) - rho0) / delta;
    final double dRhodZ = (atmosphere.getDensity(date, new Vector3D(x, y, z + delta), atmFrame) - rho0) / delta;
    final double[] dXdQ = posBodyDS.getX().getAllDerivatives();
    final double[] dYdQ = posBodyDS.getY().getAllDerivatives();
    final double[] dZdQ = posBodyDS.getZ().getAllDerivatives();
    // Density with derivatives:
    // - The value and only the 3 first derivatives (those with respect to spacecraft position) are computed
    // - Others are set to 0.
    final int p = factory.getCompiler().getFreeParameters();
    final double[] rhoAll = new double[p + 1];
    rhoAll[0] = rho0;
    for (int i = 1; i < 4; ++i) {
        rhoAll[i] = dRhodX * dXdQ[i] + dRhodY * dYdQ[i] + dRhodZ * dZdQ[i];
    }
    @SuppressWarnings("unchecked") final T rho = (T) (factory.build(rhoAll));
    return rho;
}