use of org.orekit.estimation.measurements.AngularAzEl in project Orekit by CS-SI.
the class OrbitDetermination method readMeasurements.
/**
* Read a measurements file.
* @param file measurements file
* @param stations name to stations data map
* @param pvData PV measurements data
* @param satRangeBias range bias due to transponder delay
* @param weights base weights for measurements
* @param rangeOutliersManager manager for range measurements outliers (null if none configured)
* @param rangeRateOutliersManager manager for range-rate measurements outliers (null if none configured)
* @param azElOutliersManager manager for azimuth-elevation measurements outliers (null if none configured)
* @param pvOutliersManager manager for PV measurements outliers (null if none configured)
* @return measurements list
*/
private List<ObservedMeasurement<?>> readMeasurements(final File file, final Map<String, StationData> stations, final PVData pvData, final Bias<Range> satRangeBias, final Weights weights, final OutlierFilter<Range> rangeOutliersManager, final OutlierFilter<RangeRate> rangeRateOutliersManager, final OutlierFilter<AngularAzEl> azElOutliersManager, final OutlierFilter<PV> pvOutliersManager) throws UnsupportedEncodingException, IOException, OrekitException {
final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
BufferedReader br = null;
try {
br = new BufferedReader(new InputStreamReader(new FileInputStream(file), "UTF-8"));
int lineNumber = 0;
for (String line = br.readLine(); line != null; line = br.readLine()) {
++lineNumber;
line = line.trim();
if (line.length() > 0 && !line.startsWith("#")) {
String[] fields = line.split("\\s+");
if (fields.length < 2) {
throw new OrekitException(OrekitMessages.UNABLE_TO_PARSE_LINE_IN_FILE, lineNumber, file.getName(), line);
}
switch(fields[1]) {
case "RANGE":
final Range range = new RangeParser().parseFields(fields, stations, pvData, satRangeBias, weights, line, lineNumber, file.getName());
if (rangeOutliersManager != null) {
range.addModifier(rangeOutliersManager);
}
addIfNonZeroWeight(range, measurements);
break;
case "RANGE_RATE":
final RangeRate rangeRate = new RangeRateParser().parseFields(fields, stations, pvData, satRangeBias, weights, line, lineNumber, file.getName());
if (rangeRateOutliersManager != null) {
rangeRate.addModifier(rangeRateOutliersManager);
}
addIfNonZeroWeight(rangeRate, measurements);
break;
case "AZ_EL":
final AngularAzEl angular = new AzElParser().parseFields(fields, stations, pvData, satRangeBias, weights, line, lineNumber, file.getName());
if (azElOutliersManager != null) {
angular.addModifier(azElOutliersManager);
}
addIfNonZeroWeight(angular, measurements);
break;
case "PV":
final PV pv = new PVParser().parseFields(fields, stations, pvData, satRangeBias, weights, line, lineNumber, file.getName());
if (pvOutliersManager != null) {
pv.addModifier(pvOutliersManager);
}
addIfNonZeroWeight(pv, measurements);
break;
default:
throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE, "unknown measurement type " + fields[1] + " at line " + lineNumber + " in file " + file.getName());
}
}
}
} finally {
if (br != null) {
br.close();
}
}
if (measurements.isEmpty()) {
throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE, "not measurements read from file " + file.getAbsolutePath());
}
return measurements;
}
use of org.orekit.estimation.measurements.AngularAzEl in project Orekit by CS-SI.
the class OrbitDetermination method createStationsData.
/**
* Set up stations.
* @param parser input file parser
* @param body central body
* @return name to station data map
* @exception OrekitException if some frame transforms cannot be computed
* @throws NoSuchElementException if input parameters are missing
*/
private Map<String, StationData> createStationsData(final KeyValueFileParser<ParameterKey> parser, final OneAxisEllipsoid body) throws OrekitException, NoSuchElementException {
final Map<String, StationData> stations = new HashMap<String, StationData>();
final String[] stationNames = parser.getStringArray(ParameterKey.GROUND_STATION_NAME);
final double[] stationLatitudes = parser.getAngleArray(ParameterKey.GROUND_STATION_LATITUDE);
final double[] stationLongitudes = parser.getAngleArray(ParameterKey.GROUND_STATION_LONGITUDE);
final double[] stationAltitudes = parser.getDoubleArray(ParameterKey.GROUND_STATION_ALTITUDE);
final boolean[] stationPositionEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_POSITION_ESTIMATED);
final double[] stationRangeSigma = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_SIGMA);
final double[] stationRangeBias = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS);
final double[] stationRangeBiasMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS_MIN);
final double[] stationRangeBiasMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_BIAS_MAX);
final boolean[] stationRangeBiasEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_BIAS_ESTIMATED);
final double[] stationRangeRateSigma = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_SIGMA);
final double[] stationRangeRateBias = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS);
final double[] stationRangeRateBiasMin = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_MIN);
final double[] stationRangeRateBiasMax = parser.getDoubleArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_MAX);
final boolean[] stationRangeRateBiasEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_RATE_BIAS_ESTIMATED);
final double[] stationAzimuthSigma = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_SIGMA);
final double[] stationAzimuthBias = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS);
final double[] stationAzimuthBiasMin = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS_MIN);
final double[] stationAzimuthBiasMax = parser.getAngleArray(ParameterKey.GROUND_STATION_AZIMUTH_BIAS_MAX);
final double[] stationElevationSigma = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_SIGMA);
final double[] stationElevationBias = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS);
final double[] stationElevationBiasMin = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS_MIN);
final double[] stationElevationBiasMax = parser.getAngleArray(ParameterKey.GROUND_STATION_ELEVATION_BIAS_MAX);
final boolean[] stationAzElBiasesEstimated = parser.getBooleanArray(ParameterKey.GROUND_STATION_AZ_EL_BIASES_ESTIMATED);
final boolean[] stationElevationRefraction = parser.getBooleanArray(ParameterKey.GROUND_STATION_ELEVATION_REFRACTION_CORRECTION);
final boolean[] stationRangeTropospheric = parser.getBooleanArray(ParameterKey.GROUND_STATION_RANGE_TROPOSPHERIC_CORRECTION);
for (int i = 0; i < stationNames.length; ++i) {
// the station itself
final GeodeticPoint position = new GeodeticPoint(stationLatitudes[i], stationLongitudes[i], stationAltitudes[i]);
final TopocentricFrame topo = new TopocentricFrame(body, position, stationNames[i]);
final GroundStation station = new GroundStation(topo);
station.getEastOffsetDriver().setSelected(stationPositionEstimated[i]);
station.getNorthOffsetDriver().setSelected(stationPositionEstimated[i]);
station.getZenithOffsetDriver().setSelected(stationPositionEstimated[i]);
// range
final double rangeSigma = stationRangeSigma[i];
final Bias<Range> rangeBias;
if (FastMath.abs(stationRangeBias[i]) >= Precision.SAFE_MIN || stationRangeBiasEstimated[i]) {
rangeBias = new Bias<Range>(new String[] { stationNames[i] + "/range bias" }, new double[] { stationRangeBias[i] }, new double[] { rangeSigma }, new double[] { stationRangeBiasMin[i] }, new double[] { stationRangeBiasMax[i] });
rangeBias.getParametersDrivers().get(0).setSelected(stationRangeBiasEstimated[i]);
} else {
// bias fixed to zero, we don't need to create a modifier for this
rangeBias = null;
}
// range rate
final double rangeRateSigma = stationRangeRateSigma[i];
final Bias<RangeRate> rangeRateBias;
if (FastMath.abs(stationRangeRateBias[i]) >= Precision.SAFE_MIN || stationRangeRateBiasEstimated[i]) {
rangeRateBias = new Bias<RangeRate>(new String[] { stationNames[i] + "/range rate bias" }, new double[] { stationRangeRateBias[i] }, new double[] { rangeRateSigma }, new double[] { stationRangeRateBiasMin[i] }, new double[] { stationRangeRateBiasMax[i] });
rangeRateBias.getParametersDrivers().get(0).setSelected(stationRangeRateBiasEstimated[i]);
} else {
// bias fixed to zero, we don't need to create a modifier for this
rangeRateBias = null;
}
// angular biases
final double[] azELSigma = new double[] { stationAzimuthSigma[i], stationElevationSigma[i] };
final Bias<AngularAzEl> azELBias;
if (FastMath.abs(stationAzimuthBias[i]) >= Precision.SAFE_MIN || FastMath.abs(stationElevationBias[i]) >= Precision.SAFE_MIN || stationAzElBiasesEstimated[i]) {
azELBias = new Bias<AngularAzEl>(new String[] { stationNames[i] + "/az bias", stationNames[i] + "/el bias" }, new double[] { stationAzimuthBias[i], stationElevationBias[i] }, azELSigma, new double[] { stationAzimuthBiasMin[i], stationElevationBiasMin[i] }, new double[] { stationAzimuthBiasMax[i], stationElevationBiasMax[i] });
azELBias.getParametersDrivers().get(0).setSelected(stationAzElBiasesEstimated[i]);
azELBias.getParametersDrivers().get(1).setSelected(stationAzElBiasesEstimated[i]);
} else {
// bias fixed to zero, we don't need to create a modifier for this
azELBias = null;
}
// Refraction correction
final AngularRadioRefractionModifier refractionCorrection;
if (stationElevationRefraction[i]) {
final double altitude = station.getBaseFrame().getPoint().getAltitude();
final AtmosphericRefractionModel refractionModel = new EarthITU453AtmosphereRefraction(altitude);
refractionCorrection = new AngularRadioRefractionModifier(refractionModel);
} else {
refractionCorrection = null;
}
// Tropospheric correction
final RangeTroposphericDelayModifier rangeTroposphericCorrection;
if (stationRangeTropospheric[i]) {
final SaastamoinenModel troposphericModel = SaastamoinenModel.getStandardModel();
rangeTroposphericCorrection = new RangeTroposphericDelayModifier(troposphericModel);
} else {
rangeTroposphericCorrection = null;
}
stations.put(stationNames[i], new StationData(station, rangeSigma, rangeBias, rangeRateSigma, rangeRateBias, azELSigma, azELBias, refractionCorrection, rangeTroposphericCorrection));
}
return stations;
}
use of org.orekit.estimation.measurements.AngularAzEl in project Orekit by CS-SI.
the class OrbitDetermination method run.
private void run(final File input, final File home) throws IOException, IllegalArgumentException, OrekitException, ParseException {
// read input parameters
KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<ParameterKey>(ParameterKey.class);
try (final FileInputStream fis = new FileInputStream(input)) {
parser.parseInput(input.getAbsolutePath(), fis);
}
// log file
final String baseName;
final PrintStream logStream;
if (parser.containsKey(ParameterKey.OUTPUT_BASE_NAME) && parser.getString(ParameterKey.OUTPUT_BASE_NAME).length() > 0) {
baseName = parser.getString(ParameterKey.OUTPUT_BASE_NAME);
logStream = new PrintStream(new File(home, baseName + "-log.out"), "UTF-8");
} else {
baseName = null;
logStream = null;
}
final RangeLog rangeLog = new RangeLog(home, baseName);
final RangeRateLog rangeRateLog = new RangeRateLog(home, baseName);
final AzimuthLog azimuthLog = new AzimuthLog(home, baseName);
final ElevationLog elevationLog = new ElevationLog(home, baseName);
final PositionLog positionLog = new PositionLog(home, baseName);
final VelocityLog velocityLog = new VelocityLog(home, baseName);
try {
// gravity field
final NormalizedSphericalHarmonicsProvider gravityField = createGravityField(parser);
// Orbit initial guess
final Orbit initialGuess = createOrbit(parser, gravityField.getMu());
// IERS conventions
final IERSConventions conventions;
if (!parser.containsKey(ParameterKey.IERS_CONVENTIONS)) {
conventions = IERSConventions.IERS_2010;
} else {
conventions = IERSConventions.valueOf("IERS_" + parser.getInt(ParameterKey.IERS_CONVENTIONS));
}
// central body
final OneAxisEllipsoid body = createBody(parser);
// propagator builder
final NumericalPropagatorBuilder propagatorBuilder = createPropagatorBuilder(parser, conventions, gravityField, body, initialGuess);
// estimator
final BatchLSEstimator estimator = createEstimator(parser, propagatorBuilder);
// measurements
final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
for (final String fileName : parser.getStringsList(ParameterKey.MEASUREMENTS_FILES, ',')) {
measurements.addAll(readMeasurements(new File(input.getParentFile(), fileName), createStationsData(parser, body), createPVData(parser), createSatRangeBias(parser), createWeights(parser), createRangeOutliersManager(parser), createRangeRateOutliersManager(parser), createAzElOutliersManager(parser), createPVOutliersManager(parser)));
}
for (ObservedMeasurement<?> measurement : measurements) {
estimator.addMeasurement(measurement);
}
// estimate orbit
estimator.setObserver(new BatchLSObserver() {
private PVCoordinates previousPV;
{
previousPV = initialGuess.getPVCoordinates();
final String header = "iteration evaluations ΔP(m) ΔV(m/s) RMS nb Range nb Range-rate nb Angular nb PV%n";
System.out.format(Locale.US, header);
if (logStream != null) {
logStream.format(Locale.US, header);
}
}
/**
* {@inheritDoc}
*/
@Override
public void evaluationPerformed(final int iterationsCount, final int evaluationsCount, final Orbit[] orbits, final ParameterDriversList estimatedOrbitalParameters, final ParameterDriversList estimatedPropagatorParameters, final ParameterDriversList estimatedMeasurementsParameters, final EstimationsProvider evaluationsProvider, final LeastSquaresProblem.Evaluation lspEvaluation) {
PVCoordinates currentPV = orbits[0].getPVCoordinates();
final String format0 = " %2d %2d %16.12f %s %s %s %s%n";
final String format = " %2d %2d %13.6f %12.9f %16.12f %s %s %s %s%n";
final EvaluationCounter<Range> rangeCounter = new EvaluationCounter<Range>();
final EvaluationCounter<RangeRate> rangeRateCounter = new EvaluationCounter<RangeRate>();
final EvaluationCounter<AngularAzEl> angularCounter = new EvaluationCounter<AngularAzEl>();
final EvaluationCounter<PV> pvCounter = new EvaluationCounter<PV>();
for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimator.getLastEstimations().entrySet()) {
if (entry.getKey() instanceof Range) {
@SuppressWarnings("unchecked") EstimatedMeasurement<Range> evaluation = (EstimatedMeasurement<Range>) entry.getValue();
rangeCounter.add(evaluation);
} else if (entry.getKey() instanceof RangeRate) {
@SuppressWarnings("unchecked") EstimatedMeasurement<RangeRate> evaluation = (EstimatedMeasurement<RangeRate>) entry.getValue();
rangeRateCounter.add(evaluation);
} else if (entry.getKey() instanceof AngularAzEl) {
@SuppressWarnings("unchecked") EstimatedMeasurement<AngularAzEl> evaluation = (EstimatedMeasurement<AngularAzEl>) entry.getValue();
angularCounter.add(evaluation);
} else if (entry.getKey() instanceof PV) {
@SuppressWarnings("unchecked") EstimatedMeasurement<PV> evaluation = (EstimatedMeasurement<PV>) entry.getValue();
pvCounter.add(evaluation);
}
}
if (evaluationsCount == 1) {
System.out.format(Locale.US, format0, iterationsCount, evaluationsCount, lspEvaluation.getRMS(), rangeCounter.format(8), rangeRateCounter.format(8), angularCounter.format(8), pvCounter.format(8));
if (logStream != null) {
logStream.format(Locale.US, format0, iterationsCount, evaluationsCount, lspEvaluation.getRMS(), rangeCounter.format(8), rangeRateCounter.format(8), angularCounter.format(8), pvCounter.format(8));
}
} else {
System.out.format(Locale.US, format, iterationsCount, evaluationsCount, Vector3D.distance(previousPV.getPosition(), currentPV.getPosition()), Vector3D.distance(previousPV.getVelocity(), currentPV.getVelocity()), lspEvaluation.getRMS(), rangeCounter.format(8), rangeRateCounter.format(8), angularCounter.format(8), pvCounter.format(8));
if (logStream != null) {
logStream.format(Locale.US, format, iterationsCount, evaluationsCount, Vector3D.distance(previousPV.getPosition(), currentPV.getPosition()), Vector3D.distance(previousPV.getVelocity(), currentPV.getVelocity()), lspEvaluation.getRMS(), rangeCounter.format(8), rangeRateCounter.format(8), angularCounter.format(8), pvCounter.format(8));
}
}
previousPV = currentPV;
}
});
Orbit estimated = estimator.estimate()[0].getInitialState().getOrbit();
// compute some statistics
for (final Map.Entry<ObservedMeasurement<?>, EstimatedMeasurement<?>> entry : estimator.getLastEstimations().entrySet()) {
if (entry.getKey() instanceof Range) {
@SuppressWarnings("unchecked") EstimatedMeasurement<Range> evaluation = (EstimatedMeasurement<Range>) entry.getValue();
rangeLog.add(evaluation);
} else if (entry.getKey() instanceof RangeRate) {
@SuppressWarnings("unchecked") EstimatedMeasurement<RangeRate> evaluation = (EstimatedMeasurement<RangeRate>) entry.getValue();
rangeRateLog.add(evaluation);
} else if (entry.getKey() instanceof AngularAzEl) {
@SuppressWarnings("unchecked") EstimatedMeasurement<AngularAzEl> evaluation = (EstimatedMeasurement<AngularAzEl>) entry.getValue();
azimuthLog.add(evaluation);
elevationLog.add(evaluation);
} else if (entry.getKey() instanceof PV) {
@SuppressWarnings("unchecked") EstimatedMeasurement<PV> evaluation = (EstimatedMeasurement<PV>) entry.getValue();
positionLog.add(evaluation);
velocityLog.add(evaluation);
}
}
System.out.println("Estimated orbit: " + estimated);
if (logStream != null) {
logStream.println("Estimated orbit: " + estimated);
}
final ParameterDriversList orbitalParameters = estimator.getOrbitalParametersDrivers(true);
final ParameterDriversList propagatorParameters = estimator.getPropagatorParametersDrivers(true);
final ParameterDriversList measurementsParameters = estimator.getMeasurementsParametersDrivers(true);
int length = 0;
for (final ParameterDriver parameterDriver : orbitalParameters.getDrivers()) {
length = FastMath.max(length, parameterDriver.getName().length());
}
for (final ParameterDriver parameterDriver : propagatorParameters.getDrivers()) {
length = FastMath.max(length, parameterDriver.getName().length());
}
for (final ParameterDriver parameterDriver : measurementsParameters.getDrivers()) {
length = FastMath.max(length, parameterDriver.getName().length());
}
displayParametersChanges(System.out, "Estimated orbital parameters changes: ", false, length, orbitalParameters);
if (logStream != null) {
displayParametersChanges(logStream, "Estimated orbital parameters changes: ", false, length, orbitalParameters);
}
displayParametersChanges(System.out, "Estimated propagator parameters changes: ", true, length, propagatorParameters);
if (logStream != null) {
displayParametersChanges(logStream, "Estimated propagator parameters changes: ", true, length, propagatorParameters);
}
displayParametersChanges(System.out, "Estimated measurements parameters changes: ", true, length, measurementsParameters);
if (logStream != null) {
displayParametersChanges(logStream, "Estimated measurements parameters changes: ", true, length, measurementsParameters);
}
System.out.println("Number of iterations: " + estimator.getIterationsCount());
System.out.println("Number of evaluations: " + estimator.getEvaluationsCount());
rangeLog.displaySummary(System.out);
rangeRateLog.displaySummary(System.out);
azimuthLog.displaySummary(System.out);
elevationLog.displaySummary(System.out);
positionLog.displaySummary(System.out);
velocityLog.displaySummary(System.out);
if (logStream != null) {
logStream.println("Number of iterations: " + estimator.getIterationsCount());
logStream.println("Number of evaluations: " + estimator.getEvaluationsCount());
rangeLog.displaySummary(logStream);
rangeRateLog.displaySummary(logStream);
azimuthLog.displaySummary(logStream);
elevationLog.displaySummary(logStream);
positionLog.displaySummary(logStream);
velocityLog.displaySummary(logStream);
}
rangeLog.displayResiduals();
rangeRateLog.displayResiduals();
azimuthLog.displayResiduals();
elevationLog.displayResiduals();
positionLog.displayResiduals();
velocityLog.displayResiduals();
} finally {
if (logStream != null) {
logStream.close();
}
rangeLog.close();
rangeRateLog.close();
azimuthLog.close();
elevationLog.close();
positionLog.close();
velocityLog.close();
}
}
use of org.orekit.estimation.measurements.AngularAzEl in project Orekit by CS-SI.
the class AngularRadioRefractionModifier method modify.
@Override
public void modify(final EstimatedMeasurement<AngularAzEl> estimated) throws OrekitException {
final AngularAzEl measure = estimated.getObservedMeasurement();
final GroundStation station = measure.getStation();
final SpacecraftState state = estimated.getStates()[0];
final double correction = angularErrorRadioRefractionModel(station, state);
// update estimated value taking into account the tropospheric elevation corection.
// The tropospheric elevation correction is directly added to the elevation.
final double[] oldValue = estimated.getEstimatedValue();
final double[] newValue = oldValue.clone();
// consider only effect on elevation
newValue[1] = newValue[1] + correction;
estimated.setEstimatedValue(newValue[0], newValue[1]);
}
use of org.orekit.estimation.measurements.AngularAzEl in project Orekit by CS-SI.
the class AngularIonosphericDelayModifier method modify.
@Override
public void modify(final EstimatedMeasurement<AngularAzEl> estimated) throws OrekitException {
final AngularAzEl measure = estimated.getObservedMeasurement();
final GroundStation station = measure.getStation();
final SpacecraftState state = estimated.getStates()[0];
final double delay = angularErrorIonosphericModel(station, state);
// Delay is taken into account to shift the spacecraft position
final double dt = delay / Constants.SPEED_OF_LIGHT;
// Position of the spacecraft shifted of dt
final SpacecraftState transitState = state.shiftedBy(-dt);
// Update estimated value taking into account the ionospheric delay.
final AbsoluteDate date = transitState.getDate();
final Vector3D position = transitState.getPVCoordinates().getPosition();
final Frame inertial = transitState.getFrame();
// Elevation and azimuth in radians
final double elevation = station.getBaseFrame().getElevation(position, inertial, date);
final double baseAzimuth = station.getBaseFrame().getAzimuth(position, inertial, date);
final double twoPiWrap = MathUtils.normalizeAngle(baseAzimuth, measure.getObservedValue()[0]) - baseAzimuth;
final double azimuth = baseAzimuth + twoPiWrap;
// Update estimated value taking into account the ionospheric delay.
// Azimuth - elevation values
estimated.setEstimatedValue(azimuth, elevation);
}
Aggregations