Examples with UnivariateSolver org.hipparchus.analysis.solvers.UnivariateSolver used on opensource projects

Example 1 with UnivariateSolver

use of org.hipparchus.analysis.solvers.UnivariateSolver in project Orekit by CS-SI.

the class TurnAroundRangeMeasurementCreator method handleStep.

/**
 * Function handling the steps of the propagator
 * A turn-around measurement needs 2 stations, a master and a slave
 * The measurement is a signal:
 * - Emitted from the master ground station
 * - Reflected on the spacecraft
 * - Reflected on the slave ground station
 * - Reflected on the spacecraft again
 * - Received on the master ground station
 * Its value is the elapsed time between emission and reception
 * divided by 2c were c is the speed of light.
 *
 * The path of the signal is divided into 2 legs:
 *  - The 1st leg goes from emission by the master station to reception by the slave station
 *  - The 2nd leg goes from emission by the slave station to reception by the master station
 *
 * The spacecraft state date should, after a few iterations of the estimation process, be
 * set to the date of arrival/departure of the signal to/from the slave station.
 * It is guaranteed by implementation of the estimated measurement.
 * This is done to avoid big shifts in time to compute the transit states.
 * See TurnAroundRange.java for more
 * Thus the spacecraft date is the date when the 1st leg of the path ends and the 2nd leg begins
 */
public void handleStep(final SpacecraftState currentState, final boolean isLast) throws OrekitException {
    try {
        for (Map.Entry<GroundStation, GroundStation> entry : context.TARstations.entrySet()) {
            final GroundStation masterStation = entry.getKey();
            final GroundStation slaveStation = entry.getValue();
            final AbsoluteDate date = currentState.getDate();
            final Frame inertial = currentState.getFrame();
            final Vector3D position = currentState.toTransform().getInverse().transformPosition(antennaPhaseCenter);
            // Create a TAR measurement only if elevation for both stations is higher than elevationMin°
            if ((masterStation.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0)) && (slaveStation.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0))) {
                // The solver used
                final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
                // Spacecraft date t = date of arrival/departure of the signal to/from from the slave station
                // Slave station position in inertial frame at t
                final Vector3D slaveStationPosition = slaveStation.getOffsetToInertial(inertial, date).transformPosition(Vector3D.ZERO);
                // Downlink time of flight to slave station
                // The date of arrival/departure of the signal to/from the slave station is known and
                // equal to spacecraft date t.
                // Therefore we can use the function "downlinkTimeOfFlight" from GroundStation class
                // final double slaveTauD = slaveStation.downlinkTimeOfFlight(currentState, date);
                final double slaveTauD = solver.solve(1000, new UnivariateFunction() {

                    public double value(final double x) throws OrekitExceptionWrapper {
                        final SpacecraftState transitState = currentState.shiftedBy(-x);
                        final double d = Vector3D.distance(transitState.toTransform().getInverse().transformPosition(antennaPhaseCenter), slaveStationPosition);
                        return d - x * Constants.SPEED_OF_LIGHT;
                    }
                }, -1.0, 1.0);
                // Uplink time of flight from slave station
                // A solver is used to know where the satellite is when it receives the signal
                // back from the slave station
                final double slaveTauU = solver.solve(1000, new UnivariateFunction() {

                    public double value(final double x) throws OrekitExceptionWrapper {
                        final SpacecraftState transitState = currentState.shiftedBy(+x);
                        final double d = Vector3D.distance(transitState.toTransform().getInverse().transformPosition(antennaPhaseCenter), slaveStationPosition);
                        return d - x * Constants.SPEED_OF_LIGHT;
                    }
                }, -1.0, 1.0);
                // Find the position of the master station at signal departure and arrival
                // ----
                // Transit state position & date for the 1st leg of the signal path
                final SpacecraftState S1 = currentState.shiftedBy(-slaveTauD);
                final Vector3D P1 = S1.toTransform().getInverse().transformPosition(antennaPhaseCenter);
                final AbsoluteDate T1 = date.shiftedBy(-slaveTauD);
                // Transit state position & date for the 2nd leg of the signal path
                final Vector3D P2 = currentState.shiftedBy(+slaveTauU).toTransform().getInverse().transformPosition(antennaPhaseCenter);
                final AbsoluteDate T2 = date.shiftedBy(+slaveTauU);
                // Master station downlink delay - from P2 to master station
                // We use a solver to know where the master station is when it receives
                // the signal back from the satellite on the 2nd leg of the path
                final double masterTauD = solver.solve(1000, new UnivariateFunction() {

                    public double value(final double x) throws OrekitExceptionWrapper {
                        try {
                            final Transform t = masterStation.getOffsetToInertial(inertial, T2.shiftedBy(+x));
                            final double d = Vector3D.distance(P2, t.transformPosition(Vector3D.ZERO));
                            return d - x * Constants.SPEED_OF_LIGHT;
                        } catch (OrekitException oe) {
                            throw new OrekitExceptionWrapper(oe);
                        }
                    }
                }, -1.0, 1.0);
                final AbsoluteDate masterReceptionDate = T2.shiftedBy(+masterTauD);
                final TimeStampedPVCoordinates masterStationAtReception = masterStation.getOffsetToInertial(inertial, masterReceptionDate).transformPVCoordinates(new TimeStampedPVCoordinates(masterReceptionDate, PVCoordinates.ZERO));
                // Master station uplink delay - from master station to P1
                // Here the state date is known. Thus we can use the function "signalTimeOfFlight"
                // of the AbstractMeasurement class
                final double masterTauU = AbstractMeasurement.signalTimeOfFlight(masterStationAtReception, P1, T1);
                final AbsoluteDate masterEmissionDate = T1.shiftedBy(-masterTauU);
                final Vector3D masterStationAtEmission = masterStation.getOffsetToInertial(inertial, masterEmissionDate).transformPosition(Vector3D.ZERO);
                // Uplink/downlink distance from/to slave station
                final double slaveDownLinkDistance = Vector3D.distance(P1, slaveStationPosition);
                final double slaveUpLinkDistance = Vector3D.distance(P2, slaveStationPosition);
                // Uplink/downlink distance from/to master station
                final double masterUpLinkDistance = Vector3D.distance(P1, masterStationAtEmission);
                final double masterDownLinkDistance = Vector3D.distance(P2, masterStationAtReception.getPosition());
                addMeasurement(new TurnAroundRange(masterStation, slaveStation, masterReceptionDate, 0.5 * (masterUpLinkDistance + slaveDownLinkDistance + slaveUpLinkDistance + masterDownLinkDistance), 1.0, 10));
            }
        }
    } catch (OrekitExceptionWrapper oew) {
        throw new OrekitException(oew.getException());
    } catch (OrekitException oe) {
        throw new OrekitException(oe);
    }
}

Example 2 with UnivariateSolver

use of org.hipparchus.analysis.solvers.UnivariateSolver in project Orekit by CS-SI.

the class Geoid method getIntersectionPoint.

/**
 * {@inheritDoc}
 *
 * <p> The intersection point is computed using a line search along the
 * specified line. This is accurate when the geoid is slowly varying.
 */
@Override
public GeodeticPoint getIntersectionPoint(final Line lineInFrame, final Vector3D closeInFrame, final Frame frame, final AbsoluteDate date) throws OrekitException {
    /*
         * It is assumed that the geoid is slowly varying over it's entire
         * surface. Therefore there will one local intersection.
         */
    // transform to body frame
    final Frame bodyFrame = this.getBodyFrame();
    final Transform frameToBody = frame.getTransformTo(bodyFrame, date);
    final Vector3D close = frameToBody.transformPosition(closeInFrame);
    final Line lineInBodyFrame = frameToBody.transformLine(lineInFrame);
    // set the line's direction so the solved for value is always positive
    final Line line;
    if (lineInBodyFrame.getAbscissa(close) < 0) {
        line = lineInBodyFrame.revert();
    } else {
        line = lineInBodyFrame;
    }
    final ReferenceEllipsoid ellipsoid = this.getEllipsoid();
    // calculate end points
    // distance from line to center of earth, squared
    final double d2 = line.pointAt(0.0).getNormSq();
    // the minimum abscissa, squared
    final double n = ellipsoid.getPolarRadius() + MIN_UNDULATION;
    final double minAbscissa2 = n * n - d2;
    // smaller end point of the interval = 0.0 or intersection with
    // min_undulation sphere
    final double lowPoint = FastMath.sqrt(FastMath.max(minAbscissa2, 0.0));
    // the maximum abscissa, squared
    final double x = ellipsoid.getEquatorialRadius() + MAX_UNDULATION;
    final double maxAbscissa2 = x * x - d2;
    // larger end point of the interval
    final double highPoint = FastMath.sqrt(maxAbscissa2);
    // line search function
    final UnivariateFunction heightFunction = new UnivariateFunction() {

        @Override
        public double value(final double x) {
            try {
                final GeodeticPoint geodetic = transform(line.pointAt(x), bodyFrame, date);
                return geodetic.getAltitude();
            } catch (OrekitException e) {
                // due to frame transform -> re-throw
                throw new RuntimeException(e);
            }
        }
    };
    // compute answer
    if (maxAbscissa2 < 0) {
        // ray does not pierce bounding sphere -> no possible intersection
        return null;
    }
    // solve line search problem to find the intersection
    final UnivariateSolver solver = new BracketingNthOrderBrentSolver();
    try {
        final double abscissa = solver.solve(MAX_EVALUATIONS, heightFunction, lowPoint, highPoint);
        // return intersection point
        return this.transform(line.pointAt(abscissa), bodyFrame, date);
    } catch (MathRuntimeException e) {
        // no intersection
        return null;
    }
}

Example 3 with UnivariateSolver

use of org.hipparchus.analysis.solvers.UnivariateSolver in project Orekit by CS-SI.

the class TopocentricFrame method computeLimitVisibilityPoint.

/**
 * Compute the limit visibility point for a satellite in a given direction.
 * <p>
 * This method can be used to compute visibility circles around ground stations
 * for example, using a simple loop on azimuth, with either a fixed elevation
 * or an elevation that depends on azimuth to take ground masks into account.
 * </p>
 * @param radius satellite distance to Earth center
 * @param azimuth pointing azimuth from station
 * @param elevation pointing elevation from station
 * @return limit visibility point for the satellite
 * @throws OrekitException if point cannot be found
 */
public GeodeticPoint computeLimitVisibilityPoint(final double radius, final double azimuth, final double elevation) throws OrekitException {
    try {
        // convergence threshold on point position: 1mm
        final double deltaP = 0.001;
        final UnivariateSolver solver = new BracketingNthOrderBrentSolver(deltaP / Constants.WGS84_EARTH_EQUATORIAL_RADIUS, deltaP, deltaP, 5);
        // find the distance such that a point in the specified direction and at the solved-for
        // distance is exactly at the specified radius
        final double distance = solver.solve(1000, new UnivariateFunction() {

            /**
             * {@inheritDoc}
             */
            public double value(final double x) {
                try {
                    final GeodeticPoint gp = pointAtDistance(azimuth, elevation, x);
                    return parentShape.transform(gp).getNorm() - radius;
                } catch (OrekitException oe) {
                    throw new OrekitExceptionWrapper(oe);
                }
            }
        }, 0, 2 * radius);
        // return the limit point
        return pointAtDistance(azimuth, elevation, distance);
    } catch (MathRuntimeException mrte) {
        throw new OrekitException(mrte);
    } catch (OrekitExceptionWrapper lwe) {
        throw lwe.getException();
    }
}

Example 4 with UnivariateSolver

use of org.hipparchus.analysis.solvers.UnivariateSolver in project Orekit by CS-SI.

the class AngularAzElMeasurementCreator method handleStep.

public void handleStep(final SpacecraftState currentState, final boolean isLast) throws OrekitException {
    for (final GroundStation station : context.stations) {
        final AbsoluteDate date = currentState.getDate();
        final Frame inertial = currentState.getFrame();
        final Vector3D position = currentState.getPVCoordinates().getPosition();
        if (station.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0)) {
            final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
            final double downLinkDelay = solver.solve(1000, new UnivariateFunction() {

                public double value(final double x) throws OrekitExceptionWrapper {
                    try {
                        final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(x));
                        final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
                        return d - x * Constants.SPEED_OF_LIGHT;
                    } catch (OrekitException oe) {
                        throw new OrekitExceptionWrapper(oe);
                    }
                }
            }, -1.0, 1.0);
            // Satellite position at signal departure
            final Vector3D satelliteAtDeparture = currentState.shiftedBy(-downLinkDelay).getPVCoordinates().getPosition();
            // Initialize measurement
            final double[] angular = new double[2];
            final double[] sigma = { 1.0, 1.0 };
            final double[] baseweight = { 10.0, 10.0 };
            // Compute measurement
            // Elevation
            angular[1] = station.getBaseFrame().getElevation(satelliteAtDeparture, currentState.getFrame(), currentState.getDate());
            // Azimuth
            angular[0] = station.getBaseFrame().getAzimuth(satelliteAtDeparture, currentState.getFrame(), currentState.getDate());
            addMeasurement(new AngularAzEl(station, date, angular, sigma, baseweight));
        }
    }
}

Example 5 with UnivariateSolver

use of org.hipparchus.analysis.solvers.UnivariateSolver in project Orekit by CS-SI.

the class AngularRaDecMeasurementCreator method handleStep.

public void handleStep(final SpacecraftState currentState, final boolean isLast) throws OrekitException {
    for (final GroundStation station : context.stations) {
        final AbsoluteDate date = currentState.getDate();
        final Frame inertial = currentState.getFrame();
        final Vector3D position = currentState.getPVCoordinates().getPosition();
        if (station.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0)) {
            final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
            final double downLinkDelay = solver.solve(1000, new UnivariateFunction() {

                public double value(final double x) throws OrekitExceptionWrapper {
                    try {
                        final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(x));
                        final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
                        return d - x * Constants.SPEED_OF_LIGHT;
                    } catch (OrekitException oe) {
                        throw new OrekitExceptionWrapper(oe);
                    }
                }
            }, -1.0, 1.0);
            // Satellite position at signal departure
            final Vector3D satelliteAtDeparture = currentState.shiftedBy(-downLinkDelay).getPVCoordinates().getPosition();
            // Initialize measurement
            final double[] angular = new double[2];
            final double[] sigma = { 1.0, 1.0 };
            final double[] baseweight = { 10.0, 10.0 };
            // Inertial frame used
            final Frame inertialFrame = context.initialOrbit.getFrame();
            // Station position at signal arrival
            // Set the reference date of offset drivers arbitrarily to avoid exception
            station.getPrimeMeridianOffsetDriver().setReferenceDate(date);
            station.getPolarOffsetXDriver().setReferenceDate(date);
            station.getPolarOffsetYDriver().setReferenceDate(date);
            final Transform offsetToInertialArrival = station.getOffsetToInertial(inertialFrame, date);
            final Vector3D stationPArrival = offsetToInertialArrival.transformVector(Vector3D.ZERO);
            // Vector station position at signal arrival - satellite at signal departure
            // In inertial reference frame
            final Vector3D staSat = satelliteAtDeparture.subtract(stationPArrival);
            // Compute measurement
            // Right ascension
            final double baseRightAscension = staSat.getAlpha();
            angular[0] = MathUtils.normalizeAngle(baseRightAscension, 0.0);
            // Declination
            angular[1] = staSat.getDelta();
            addMeasurement(new AngularRaDec(station, inertialFrame, date, angular, sigma, baseweight));
        }
    }
}