use of org.orekit.KeyValueFileParser in project Orekit by CS-SI.
the class OrbitDeterminationTest method run.
private ResultOD run(final File input, final boolean print) throws IOException, IllegalArgumentException, OrekitException, ParseException {
// read input parameters
KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<ParameterKey>(ParameterKey.class);
parser.parseInput(input.getAbsolutePath(), new FileInputStream(input));
// log file
final RangeLog rangeLog = new RangeLog();
final RangeRateLog rangeRateLog = new RangeRateLog();
final AzimuthLog azimuthLog = new AzimuthLog();
final ElevationLog elevationLog = new ElevationLog();
final PositionLog positionLog = new PositionLog();
final VelocityLog velocityLog = new VelocityLog();
// gravity field
GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-5c.gfc", true));
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);
}
if (print) {
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);
}
/**
* {@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));
} 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));
}
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);
}
}
// parmaters and measurements.
final ParameterDriversList propagatorParameters = estimator.getPropagatorParametersDrivers(true);
final ParameterDriversList measurementsParameters = estimator.getMeasurementsParametersDrivers(true);
// instation of results
return new ResultOD(propagatorParameters, measurementsParameters, estimator.getIterationsCount(), estimator.getEvaluationsCount(), estimated.getPVCoordinates(), rangeLog.createStatisticsSummary(), rangeRateLog.createStatisticsSummary(), azimuthLog.createStatisticsSummary(), elevationLog.createStatisticsSummary(), positionLog.createStatisticsSummary(), velocityLog.createStatisticsSummary(), estimator.getPhysicalCovariances(1.0e-10));
}
use of org.orekit.KeyValueFileParser in project Orekit by CS-SI.
the class KalmanOrbitDeterminationTest method runReference.
/**
* Use the physical models in the input file
* Incorporate the initial reference values
* And run the propagation until the last measurement to get the reference orbit at the same date
* as the Kalman filter
* @param input Input configuration file
* @param orbitType Orbit type to use (calculation and display)
* @param refPosition Initial reference position
* @param refVelocity Initial reference velocity
* @param refPropagationParameters Reference propagation parameters
* @param kalmanFinalDate The final date of the Kalman filter
* @return The reference orbit at the same date as the Kalman filter
* @throws IOException Input file cannot be opened
* @throws IllegalArgumentException Issue in key/value reading of input file
* @throws OrekitException An Orekit exception... should be explicit
* @throws ParseException Parsing of the input file or measurement file failed
*/
private Orbit runReference(final File input, final OrbitType orbitType, final Vector3D refPosition, final Vector3D refVelocity, final ParameterDriversList refPropagationParameters, final AbsoluteDate kalmanFinalDate) throws IOException, IllegalArgumentException, OrekitException, ParseException {
// Read input parameters
KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<ParameterKey>(ParameterKey.class);
parser.parseInput(input.getAbsolutePath(), new FileInputStream(input));
// Gravity field
GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-5c.gfc", true));
final NormalizedSphericalHarmonicsProvider gravityField = createGravityField(parser);
// Orbit initial guess
Orbit initialRefOrbit = new CartesianOrbit(new PVCoordinates(refPosition, refVelocity), parser.getInertialFrame(ParameterKey.INERTIAL_FRAME), parser.getDate(ParameterKey.ORBIT_DATE, TimeScalesFactory.getUTC()), gravityField.getMu());
// Convert to desired orbit type
initialRefOrbit = orbitType.convertType(initialRefOrbit);
// 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, initialRefOrbit);
// Force the selected propagation parameters to their reference values
if (refPropagationParameters != null) {
for (DelegatingDriver refDriver : refPropagationParameters.getDrivers()) {
for (DelegatingDriver driver : propagatorBuilder.getPropagationParametersDrivers().getDrivers()) {
if (driver.getName().equals(refDriver.getName())) {
driver.setValue(refDriver.getValue());
}
}
}
}
// Build the reference propagator
final NumericalPropagator propagator = propagatorBuilder.buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
// Propagate until last date and return the orbit
return propagator.propagate(kalmanFinalDate).getOrbit();
}
use of org.orekit.KeyValueFileParser in project Orekit by CS-SI.
the class KalmanOrbitDeterminationTest method run.
/**
* Function running the Kalman filter estimation.
* @param input Input configuration file
* @param orbitType Orbit type to use (calculation and display)
* @param print Choose whether the results are printed on console or not
* @param cartesianOrbitalP Orbital part of the initial covariance matrix in Cartesian formalism
* @param cartesianOrbitalQ Orbital part of the process noise matrix in Cartesian formalism
* @param propagationP Propagation part of the initial covariance matrix
* @param propagationQ Propagation part of the process noise matrix
* @param measurementP Measurement part of the initial covariance matrix
* @param measurementQ Measurement part of the process noise matrix
*/
private ResultKalman run(final File input, final OrbitType orbitType, final boolean print, final RealMatrix cartesianOrbitalP, final RealMatrix cartesianOrbitalQ, final RealMatrix propagationP, final RealMatrix propagationQ, final RealMatrix measurementP, final RealMatrix measurementQ) throws IOException, IllegalArgumentException, OrekitException, ParseException {
// Read input parameters
KeyValueFileParser<ParameterKey> parser = new KeyValueFileParser<ParameterKey>(ParameterKey.class);
parser.parseInput(input.getAbsolutePath(), new FileInputStream(input));
// Log files
final RangeLog rangeLog = new RangeLog();
final RangeRateLog rangeRateLog = new RangeRateLog();
final AzimuthLog azimuthLog = new AzimuthLog();
final ElevationLog elevationLog = new ElevationLog();
final PositionLog positionLog = new PositionLog();
final VelocityLog velocityLog = new VelocityLog();
// Gravity field
GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-5c.gfc", true));
final NormalizedSphericalHarmonicsProvider gravityField = createGravityField(parser);
// Orbit initial guess
Orbit initialGuess = createOrbit(parser, gravityField.getMu());
// Convert to desired orbit type
initialGuess = orbitType.convertType(initialGuess);
// 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);
// 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)));
}
// Building the Kalman filter:
// - Gather the estimated measurement parameters in a list
// - Prepare the initial covariance matrix and the process noise matrix
// - Build the Kalman filter
// --------------------------------------------------------------------
// Build the list of estimated measurements
final ParameterDriversList estimatedMeasurementsParameters = new ParameterDriversList();
for (ObservedMeasurement<?> measurement : measurements) {
final List<ParameterDriver> drivers = measurement.getParametersDrivers();
for (ParameterDriver driver : drivers) {
if (driver.isSelected()) {
// Add the driver
estimatedMeasurementsParameters.add(driver);
}
}
}
// Sort the list lexicographically
estimatedMeasurementsParameters.sort();
// Orbital covariance matrix initialization
// Jacobian of the orbital parameters w/r to Cartesian
final double[][] dYdC = new double[6][6];
initialGuess.getJacobianWrtCartesian(propagatorBuilder.getPositionAngle(), dYdC);
final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
RealMatrix orbitalP = Jac.multiply(cartesianOrbitalP.multiply(Jac.transpose()));
// Orbital process noise matrix
RealMatrix orbitalQ = Jac.multiply(cartesianOrbitalQ.multiply(Jac.transpose()));
// Build the full covariance matrix and process noise matrix
final int nbPropag = (propagationP != null) ? propagationP.getRowDimension() : 0;
final int nbMeas = (measurementP != null) ? measurementP.getRowDimension() : 0;
final RealMatrix initialP = MatrixUtils.createRealMatrix(6 + nbPropag + nbMeas, 6 + nbPropag + nbMeas);
final RealMatrix Q = MatrixUtils.createRealMatrix(6 + nbPropag + nbMeas, 6 + nbPropag + nbMeas);
// Orbital part
initialP.setSubMatrix(orbitalP.getData(), 0, 0);
Q.setSubMatrix(orbitalQ.getData(), 0, 0);
// Propagation part
if (propagationP != null) {
initialP.setSubMatrix(propagationP.getData(), 6, 6);
Q.setSubMatrix(propagationQ.getData(), 6, 6);
}
// Measurement part
if (measurementP != null) {
initialP.setSubMatrix(measurementP.getData(), 6 + nbPropag, 6 + nbPropag);
Q.setSubMatrix(measurementQ.getData(), 6 + nbPropag, 6 + nbPropag);
}
// Build the Kalman
KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
kalmanBuilder.builder(propagatorBuilder);
kalmanBuilder.estimatedMeasurementsParameters(estimatedMeasurementsParameters);
kalmanBuilder.initialCovarianceMatrix(initialP);
kalmanBuilder.processNoiseMatrixProvider(new ConstantProcessNoise(Q));
final KalmanEstimator kalman = kalmanBuilder.build();
// Add an observer
kalman.setObserver(new KalmanObserver() {
/**
* Date of the first measurement.
*/
private AbsoluteDate t0;
/**
* {@inheritDoc}
* @throws OrekitException
*/
@Override
@SuppressWarnings("unchecked")
public void evaluationPerformed(final KalmanEstimation estimation) throws OrekitException {
// Current measurement number, date and status
final EstimatedMeasurement<?> estimatedMeasurement = estimation.getCorrectedMeasurement();
final int currentNumber = estimation.getCurrentMeasurementNumber();
final AbsoluteDate currentDate = estimatedMeasurement.getDate();
final EstimatedMeasurement.Status currentStatus = estimatedMeasurement.getStatus();
// Current estimated measurement
final ObservedMeasurement<?> observedMeasurement = estimatedMeasurement.getObservedMeasurement();
// Measurement type & Station name
String measType = "";
String stationName = "";
// Register the measurement in the proper measurement logger
if (observedMeasurement instanceof Range) {
// Add the tuple (estimation, prediction) to the log
rangeLog.add(currentNumber, (EstimatedMeasurement<Range>) estimatedMeasurement);
// Measurement type & Station name
measType = "RANGE";
stationName = ((EstimatedMeasurement<Range>) estimatedMeasurement).getObservedMeasurement().getStation().getBaseFrame().getName();
} else if (observedMeasurement instanceof RangeRate) {
rangeRateLog.add(currentNumber, (EstimatedMeasurement<RangeRate>) estimatedMeasurement);
measType = "RANGE_RATE";
stationName = ((EstimatedMeasurement<RangeRate>) estimatedMeasurement).getObservedMeasurement().getStation().getBaseFrame().getName();
} else if (observedMeasurement instanceof AngularAzEl) {
azimuthLog.add(currentNumber, (EstimatedMeasurement<AngularAzEl>) estimatedMeasurement);
elevationLog.add(currentNumber, (EstimatedMeasurement<AngularAzEl>) estimatedMeasurement);
measType = "AZ_EL";
stationName = ((EstimatedMeasurement<AngularAzEl>) estimatedMeasurement).getObservedMeasurement().getStation().getBaseFrame().getName();
} else if (observedMeasurement instanceof PV) {
positionLog.add(currentNumber, (EstimatedMeasurement<PV>) estimatedMeasurement);
velocityLog.add(currentNumber, (EstimatedMeasurement<PV>) estimatedMeasurement);
measType = "PV";
}
// Header
if (print) {
if (currentNumber == 1) {
// Set t0 to first measurement date
t0 = currentDate;
// Print header
final String formatHeader = "%-4s\t%-25s\t%15s\t%-10s\t%-10s\t%-20s\t%20s\t%20s";
String header = String.format(Locale.US, formatHeader, "Nb", "Epoch", "Dt[s]", "Status", "Type", "Station", "DP Corr", "DV Corr");
// Orbital drivers
for (DelegatingDriver driver : estimation.getEstimatedOrbitalParameters().getDrivers()) {
header += String.format(Locale.US, "\t%20s", driver.getName());
header += String.format(Locale.US, "\t%20s", "D" + driver.getName());
}
// Propagation drivers
for (DelegatingDriver driver : estimation.getEstimatedPropagationParameters().getDrivers()) {
header += String.format(Locale.US, "\t%20s", driver.getName());
header += String.format(Locale.US, "\t%20s", "D" + driver.getName());
}
// Measurements drivers
for (DelegatingDriver driver : estimation.getEstimatedMeasurementsParameters().getDrivers()) {
header += String.format(Locale.US, "\t%20s", driver.getName());
header += String.format(Locale.US, "\t%20s", "D" + driver.getName());
}
// Print header
System.out.println(header);
}
// Print current measurement info in terminal
String line = "";
// Line format
final String lineFormat = "%4d\t%-25s\t%15.3f\t%-10s\t%-10s\t%-20s\t%20.9e\t%20.9e";
// Orbital correction = DP & DV between predicted orbit and estimated orbit
final Vector3D predictedP = estimation.getPredictedSpacecraftStates()[0].getPVCoordinates().getPosition();
final Vector3D predictedV = estimation.getPredictedSpacecraftStates()[0].getPVCoordinates().getVelocity();
final Vector3D estimatedP = estimation.getCorrectedSpacecraftStates()[0].getPVCoordinates().getPosition();
final Vector3D estimatedV = estimation.getCorrectedSpacecraftStates()[0].getPVCoordinates().getVelocity();
final double DPcorr = Vector3D.distance(predictedP, estimatedP);
final double DVcorr = Vector3D.distance(predictedV, estimatedV);
line = String.format(Locale.US, lineFormat, currentNumber, currentDate.toString(), currentDate.durationFrom(t0), currentStatus.toString(), measType, stationName, DPcorr, DVcorr);
// Handle parameters printing (value and error)
int jPar = 0;
final RealMatrix Pest = estimation.getPhysicalEstimatedCovarianceMatrix();
// Orbital drivers
for (DelegatingDriver driver : estimation.getEstimatedOrbitalParameters().getDrivers()) {
line += String.format(Locale.US, "\t%20.9f", driver.getValue());
line += String.format(Locale.US, "\t%20.9e", FastMath.sqrt(Pest.getEntry(jPar, jPar)));
jPar++;
}
// Propagation drivers
for (DelegatingDriver driver : estimation.getEstimatedPropagationParameters().getDrivers()) {
line += String.format(Locale.US, "\t%20.9f", driver.getValue());
line += String.format(Locale.US, "\t%20.9e", FastMath.sqrt(Pest.getEntry(jPar, jPar)));
jPar++;
}
// Measurements drivers
for (DelegatingDriver driver : estimatedMeasurementsParameters.getDrivers()) {
line += String.format(Locale.US, "\t%20.9f", driver.getValue());
line += String.format(Locale.US, "\t%20.9e", FastMath.sqrt(Pest.getEntry(jPar, jPar)));
jPar++;
}
// Print the line
System.out.println(line);
}
}
});
// Process the list measurements
final Orbit estimated = kalman.processMeasurements(measurements).getInitialState().getOrbit();
// Get the last estimated physical covariances
final RealMatrix covarianceMatrix = kalman.getPhysicalEstimatedCovarianceMatrix();
// Parameters and measurements.
final ParameterDriversList propagationParameters = kalman.getPropagationParametersDrivers(true);
final ParameterDriversList measurementsParameters = kalman.getEstimatedMeasurementsParameters();
// Eventually, print parameter changes, statistics and covariances
if (print) {
// Display parameter change for non orbital drivers
int length = 0;
for (final ParameterDriver parameterDriver : propagationParameters.getDrivers()) {
length = FastMath.max(length, parameterDriver.getName().length());
}
for (final ParameterDriver parameterDriver : measurementsParameters.getDrivers()) {
length = FastMath.max(length, parameterDriver.getName().length());
}
if (propagationParameters.getNbParams() > 0) {
displayParametersChanges(System.out, "Estimated propagator parameters changes: ", true, length, propagationParameters);
}
if (measurementsParameters.getNbParams() > 0) {
displayParametersChanges(System.out, "Estimated measurements parameters changes: ", true, length, measurementsParameters);
}
// Measurements statistics summary
System.out.println("");
rangeLog.displaySummary(System.out);
rangeRateLog.displaySummary(System.out);
azimuthLog.displaySummary(System.out);
elevationLog.displaySummary(System.out);
positionLog.displaySummary(System.out);
velocityLog.displaySummary(System.out);
// Covariances and sigmas
displayFinalCovariances(System.out, kalman);
}
// Instantiation of the results
return new ResultKalman(propagationParameters, measurementsParameters, kalman.getCurrentMeasurementNumber(), estimated.getPVCoordinates(), rangeLog.createStatisticsSummary(), rangeRateLog.createStatisticsSummary(), azimuthLog.createStatisticsSummary(), elevationLog.createStatisticsSummary(), positionLog.createStatisticsSummary(), velocityLog.createStatisticsSummary(), covarianceMatrix);
}
Aggregations