Commit 5690ccef authored by Bryan Cazabonne's avatar Bryan Cazabonne
Browse files

Added ephemeris based estimation.

Fixes #726
parent 170233b0
......@@ -22,6 +22,7 @@ import java.util.stream.Collectors;
import org.hipparchus.exception.LocalizedCoreFormats;
import org.hipparchus.exception.MathIllegalArgumentException;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.util.FastMath;
import org.orekit.attitudes.Attitude;
import org.orekit.attitudes.AttitudeProvider;
......@@ -30,6 +31,7 @@ import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
import org.orekit.frames.Frame;
import org.orekit.orbits.Orbit;
import org.orekit.propagation.AbstractMatricesHarvester;
import org.orekit.propagation.BoundedPropagator;
import org.orekit.propagation.SpacecraftState;
import org.orekit.time.AbsoluteDate;
......@@ -293,6 +295,15 @@ public class Ephemeris extends AbstractAnalyticalPropagator implements BoundedPr
return managed;
}
/** {@inheritDoc} */
@Override
protected AbstractMatricesHarvester createHarvester(final String stmName, final RealMatrix initialStm,
final DoubleArrayDictionary initialJacobianColumns) {
// In order to not throw an Orekit exception during ephemeris based orbit determination
// The default behavior of the method is overrided to return a null parameter
return null;
}
/** Internal PVCoordinatesProvider for attitude computation. */
private static class LocalPVProvider implements PVCoordinatesProvider, Serializable {
......
/* Copyright 2022 Bryan Cazabonne
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* Bryan Cazabonne licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.propagation.conversion;
import java.util.List;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.estimation.leastsquares.AbstractBatchLSModel;
import org.orekit.estimation.leastsquares.BatchLSModel;
import org.orekit.estimation.leastsquares.ModelObserver;
import org.orekit.estimation.measurements.ObservedMeasurement;
import org.orekit.estimation.sequential.AbstractKalmanModel;
import org.orekit.estimation.sequential.CovarianceMatrixProvider;
import org.orekit.estimation.sequential.KalmanModel;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.Propagator;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.analytical.Ephemeris;
import org.orekit.utils.ParameterDriversList;
/** Builder for Ephemeris propagator.
* @author Bryan Cazabonne
* @since 11.3
*/
public class EphemerisPropagatorBuilder extends AbstractPropagatorBuilder implements OrbitDeterminationPropagatorBuilder {
/** List of spacecraft states. */
private final List<SpacecraftState> states;
/** The extrapolation threshold beyond which the propagation will fail. **/
private final double extrapolationThreshold;
/** Number of points to use in interpolation. */
private final int interpolationPoints;
/** Attitude provider. */
private final AttitudeProvider provider;
/** Constructor.
* @param states list of spacecraft states
* @param interpolationPoints number of points to use in interpolation
* @param extrapolationThreshold the extrapolation threshold beyond which the propagation will fail
* @param attitudeProvider
* @param angle attitude provider
*/
public EphemerisPropagatorBuilder(final List<SpacecraftState> states,
final int interpolationPoints,
final double extrapolationThreshold,
final AttitudeProvider attitudeProvider) {
super(states.get(0).getOrbit(), PositionAngle.TRUE, 1.0, false, attitudeProvider);
deselectDynamicParameters();
this.states = states;
this.interpolationPoints = interpolationPoints;
this.extrapolationThreshold = extrapolationThreshold;
this.provider = attitudeProvider;
}
/** {@inheritDoc}. */
@Override
public Propagator buildPropagator(final double[] normalizedParameters) {
return new Ephemeris(states, interpolationPoints, extrapolationThreshold, provider);
}
/** {@inheritDoc} */
@Override
public AbstractBatchLSModel buildLSModel(final OrbitDeterminationPropagatorBuilder[] builders,
final List<ObservedMeasurement<?>> measurements,
final ParameterDriversList estimatedMeasurementsParameters,
final ModelObserver observer) {
return new BatchLSModel(builders, measurements, estimatedMeasurementsParameters, observer);
}
/** {@inheritDoc} */
@Override
public AbstractKalmanModel buildKalmanModel(final List<OrbitDeterminationPropagatorBuilder> propagatorBuilders,
final List<CovarianceMatrixProvider> covarianceMatricesProviders,
final ParameterDriversList estimatedMeasurementsParameters,
final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
return new KalmanModel(propagatorBuilders, covarianceMatricesProviders, estimatedMeasurementsParameters, measurementProcessNoiseMatrix);
}
}
/* Copyright 2002-2022 CS GROUP
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.estimation;
import java.util.Arrays;
import java.util.List;
import org.hipparchus.util.FastMath;
import org.orekit.bodies.CelestialBody;
import org.orekit.bodies.CelestialBodyFactory;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.estimation.measurements.GroundStation;
import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
import org.orekit.frames.EOPHistory;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.TopocentricFrame;
import org.orekit.models.earth.displacement.StationDisplacement;
import org.orekit.models.earth.displacement.TidalDisplacement;
import org.orekit.orbits.Orbit;
import org.orekit.time.TimeScale;
import org.orekit.time.TimeScalesFactory;
import org.orekit.time.UT1Scale;
import org.orekit.utils.Constants;
import org.orekit.utils.IERSConventions;
public class EphemerisContext implements StationDataProvider {
public IERSConventions conventions;
public OneAxisEllipsoid earth;
public CelestialBody sun;
public CelestialBody moon;
public UnnormalizedSphericalHarmonicsProvider gravity;
public TimeScale utc;
public UT1Scale ut1;
public Orbit initialOrbit;
public StationDisplacement[] displacements;
public List<GroundStation> stations;
public EphemerisContext() {
this.conventions = IERSConventions.IERS_2010;
this.earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
Constants.WGS84_EARTH_FLATTENING,
FramesFactory.getITRF(conventions, true));
final EOPHistory eopHistory = FramesFactory.getEOPHistory(conventions, true);
this.ut1 = TimeScalesFactory.getUT1(eopHistory);
this.displacements = new StationDisplacement[] {
new TidalDisplacement(Constants.EIGEN5C_EARTH_EQUATORIAL_RADIUS,
Constants.JPL_SSD_SUN_EARTH_PLUS_MOON_MASS_RATIO,
Constants.JPL_SSD_EARTH_MOON_MASS_RATIO,
CelestialBodyFactory.getSun(), CelestialBodyFactory.getMoon(),
conventions, false)
};
this.stations = Arrays.asList(createStation(-53.05388, -75.01551, 1750.0, "Isla Desolación"),
createStation( 62.29639, -7.01250, 880.0, "Slættaratindur"));
}
GroundStation createStation(double latitudeInDegrees, double longitudeInDegrees,
double altitude, String name) {
final GeodeticPoint gp = new GeodeticPoint(FastMath.toRadians(latitudeInDegrees),
FastMath.toRadians(longitudeInDegrees),
altitude);
return new GroundStation(new TopocentricFrame(earth, gp, name),
ut1.getEOPHistory(), displacements);
}
@Override
public List<GroundStation> getStations() {
return stations;
}
}
/* Copyright 2022 Bryan Cazabonne
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* Bryan Cazabonne licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.estimation.leastsquares;
import java.util.ArrayList;
import java.util.List;
import org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer;
import org.hipparchus.util.FastMath;
import org.junit.Assert;
import org.junit.Before;
import org.junit.Test;
import org.orekit.Utils;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.EphemerisContext;
import org.orekit.estimation.KeplerianEstimationTestUtils;
import org.orekit.estimation.measurements.AngularAzEl;
import org.orekit.estimation.measurements.AngularAzElMeasurementCreator;
import org.orekit.estimation.measurements.ObservedMeasurement;
import org.orekit.estimation.measurements.Range;
import org.orekit.estimation.measurements.RangeMeasurementCreator;
import org.orekit.estimation.measurements.RangeRateMeasurementCreator;
import org.orekit.estimation.measurements.modifiers.Bias;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.orbits.KeplerianOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.Propagator;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.analytical.Ephemeris;
import org.orekit.propagation.analytical.KeplerianPropagator;
import org.orekit.propagation.conversion.EphemerisPropagatorBuilder;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.DateComponents;
import org.orekit.time.TimeComponents;
import org.orekit.time.TimeScalesFactory;
public class EphemerisBatchLSEstimatorTest {
private AbsoluteDate initDate;
private AbsoluteDate finalDate;
private Frame inertialFrame;
private Propagator propagator;
private EphemerisContext context;
@Before
public void setUp() throws IllegalArgumentException, OrekitException {
Utils.setDataRoot("regular-data");
initDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
TimeComponents.H00,
TimeScalesFactory.getUTC());
finalDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
TimeComponents.H00,
TimeScalesFactory.getUTC());
double a = 7187990.1979844316;
double e = 0.5e-4;
double i = 1.7105407051081795;
double omega = 1.9674147913622104;
double OMEGA = FastMath.toRadians(261);
double lv = 0;
double mu = 3.9860047e14;
inertialFrame = FramesFactory.getEME2000();
Orbit initialState = new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
inertialFrame, initDate, mu);
propagator = new KeplerianPropagator(initialState);
context = new EphemerisContext();
}
@Test
public void testRangeWithBias() {
double dt = finalDate.durationFrom(initDate);
double timeStep = dt / 20.0;
List<SpacecraftState> states = new ArrayList<SpacecraftState>();
for(double t = 0 ; t <= dt; t+=timeStep) {
states.add(propagator.propagate(initDate.shiftedBy(t)));
}
final Ephemeris ephemeris = new Ephemeris(states, 3);
final double refBias = 1234.56;
final List<ObservedMeasurement<?>> measurements =
KeplerianEstimationTestUtils.createMeasurements(ephemeris,
new RangeMeasurementCreator(context, refBias),
1.0, 5.0, 10.0);
// estimated bias
final Bias<Range> rangeBias = new Bias<Range>(new String[] {"rangeBias"}, new double[] {0.0},
new double[] {1.0},
new double[] {0.0}, new double[] {10000.0});
rangeBias.getParametersDrivers().get(0).setSelected(true);
// create orbit estimator
final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
new EphemerisPropagatorBuilder(states, 3,
ephemeris.getExtrapolationThreshold(),
ephemeris.getAttitudeProvider()));
for (final ObservedMeasurement<?> range : measurements) {
((Range) range).addModifier(rangeBias);
estimator.addMeasurement(range);
}
estimator.setParametersConvergenceThreshold(1.0e-2);
estimator.setMaxIterations(30);
estimator.setMaxEvaluations(30);
// estimate
estimator.estimate();
// verify
Assert.assertEquals(refBias, estimator.getMeasurementsParametersDrivers(true).getDrivers().get(0).getValue(), 0.01);
Assert.assertEquals(1, estimator.getMeasurementsParametersDrivers(true).getNbParams());
Assert.assertEquals(0, estimator.getOrbitalParametersDrivers(true).getNbParams());
Assert.assertEquals(0, estimator.getPropagatorParametersDrivers(true).getNbParams());
}
@Test
public void testRangeRateWithClockDrift() {
double dt = finalDate.durationFrom(initDate);
double timeStep = dt / 20.0;
List<SpacecraftState> states = new ArrayList<SpacecraftState>();
for(double t = 0 ; t <= dt; t+=timeStep) {
states.add(propagator.propagate(initDate.shiftedBy(t)));
}
final Ephemeris ephemeris = new Ephemeris(states, 3);
final double refClockBias = 653.47e-11;
final RangeRateMeasurementCreator creator = new RangeRateMeasurementCreator(context, false, refClockBias);
creator.getSatellite().getClockDriftDriver().setSelected(true);
final List<ObservedMeasurement<?>> measurements =
KeplerianEstimationTestUtils.createMeasurements(ephemeris, creator,
1.0, 5.0, 10.0);
// create orbit estimator
final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
new EphemerisPropagatorBuilder(states, 3,
ephemeris.getExtrapolationThreshold(),
ephemeris.getAttitudeProvider()));
for (final ObservedMeasurement<?> rangeRate : measurements) {
estimator.addMeasurement(rangeRate);
}
estimator.setParametersConvergenceThreshold(1.0e-2);
estimator.setMaxIterations(30);
estimator.setMaxEvaluations(30);
// estimate
estimator.estimate();
// verify
Assert.assertEquals(refClockBias, estimator.getMeasurementsParametersDrivers(true).getDrivers().get(0).getValue(), 1.0e-15);
Assert.assertEquals(1, estimator.getMeasurementsParametersDrivers(true).getNbParams());
Assert.assertEquals(0, estimator.getOrbitalParametersDrivers(true).getNbParams());
Assert.assertEquals(0, estimator.getPropagatorParametersDrivers(true).getNbParams());
}
@Test
public void testAzElWithBias() {
double dt = finalDate.durationFrom(initDate);
double timeStep = dt / 20.0;
List<SpacecraftState> states = new ArrayList<SpacecraftState>();
for(double t = 0 ; t <= dt; t+=timeStep) {
states.add(propagator.propagate(initDate.shiftedBy(t)));
}
final Ephemeris ephemeris = new Ephemeris(states, 3);
final double refAzBias = FastMath.toRadians(0.3);
final double refElBias = FastMath.toRadians(0.1);
final List<ObservedMeasurement<?>> measurements =
KeplerianEstimationTestUtils.createMeasurements(ephemeris,
new AngularAzElMeasurementCreator(context, refAzBias, refElBias),
1.0, 5.0, 10.0);
// estimated bias
final Bias<AngularAzEl> azElBias = new Bias<>(new String[] {"azBias", "elBias"}, new double[] {0.0, 0.0},
new double[] {1.0, 1.0},
new double[] {0.0, 0.0}, new double[] {2.0, 2.0});
azElBias.getParametersDrivers().get(0).setSelected(true);
azElBias.getParametersDrivers().get(1).setSelected(true);
// create orbit estimator
final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
new EphemerisPropagatorBuilder(states, 3,
ephemeris.getExtrapolationThreshold(),
ephemeris.getAttitudeProvider()));
for (final ObservedMeasurement<?> azEl : measurements) {
((AngularAzEl) azEl).addModifier(azElBias);
estimator.addMeasurement(azEl);
}
estimator.setParametersConvergenceThreshold(1.0e-2);
estimator.setMaxIterations(30);
estimator.setMaxEvaluations(30);
// estimate
estimator.estimate();
// verify
Assert.assertEquals(refAzBias, estimator.getMeasurementsParametersDrivers(true).getDrivers().get(0).getValue(), 1.0e-6);
Assert.assertEquals(refElBias, estimator.getMeasurementsParametersDrivers(true).getDrivers().get(1).getValue(), 1.0e-6);
Assert.assertEquals(2, estimator.getMeasurementsParametersDrivers(true).getNbParams());
Assert.assertEquals(0, estimator.getOrbitalParametersDrivers(true).getNbParams());
Assert.assertEquals(0, estimator.getPropagatorParametersDrivers(true).getNbParams());
}
}
......@@ -23,7 +23,7 @@ import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
import org.hipparchus.analysis.solvers.UnivariateSolver;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.util.FastMath;
import org.orekit.estimation.Context;
import org.orekit.estimation.StationDataProvider;
import org.orekit.frames.Frame;
import org.orekit.frames.Transform;
import org.orekit.propagation.SpacecraftState;
......@@ -33,16 +33,24 @@ import org.orekit.utils.ParameterDriver;
public class AngularAzElMeasurementCreator extends MeasurementCreator {
private final Context context;
private final StationDataProvider context;
private final ObservableSatellite satellite;
private final double azBias;
private final double elBias;
public AngularAzElMeasurementCreator(final Context context) {
public AngularAzElMeasurementCreator(final StationDataProvider context) {
this(context, 0., 0.);
}
public AngularAzElMeasurementCreator(final StationDataProvider context, final double azBias, final double elBias) {
this.context = context;
this.satellite = new ObservableSatellite(0);
this.azBias = azBias;
this.elBias = elBias;
}
public void init(SpacecraftState s0, AbsoluteDate t, double step) {
for (final GroundStation station : context.stations) {
for (final GroundStation station : context.getStations()) {
for (ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
station.getEastOffsetDriver(),
station.getNorthOffsetDriver(),
......@@ -62,7 +70,7 @@ public class AngularAzElMeasurementCreator extends MeasurementCreator {
}
public void handleStep(final SpacecraftState currentState) {
for (final GroundStation station : context.stations) {
for (final GroundStation station : context.getStations()) {
final AbsoluteDate date = currentState.getDate();
final Frame inertial = currentState.getFrame();
......@@ -91,11 +99,11 @@ public class AngularAzElMeasurementCreator extends MeasurementCreator {
// Elevation
angular[1] = station.getBaseFrame().getElevation(satelliteAtDeparture,
currentState.getFrame(),
currentState.getDate());
currentState.getDate()) + elBias;
// Azimuth
angular[0] = station.getBaseFrame().getAzimuth(satelliteAtDeparture,
currentState.getFrame(),
currentState.getDate());
currentState.getDate()) + azBias;
addMeasurement(new AngularAzEl(station, date, angular, sigma, baseweight, satellite));
}
......
......@@ -36,15 +36,25 @@ public class RangeMeasurementCreator extends MeasurementCreator {
private final StationDataProvider provider;
private final Vector3D antennaPhaseCenter;
private final ObservableSatellite satellite;
private final double bias;
public RangeMeasurementCreator(final StationDataProvider context) {
this(context, Vector3D.ZERO);
this(context, 0.0);
}
public RangeMeasurementCreator(final StationDataProvider context, final double bias) {
this(context, Vector3D.ZERO, bias);
}
public RangeMeasurementCreator(final StationDataProvider provider, final Vector3D antennaPhaseCenter) {
this.provider = provider;
this(provider, antennaPhaseCenter, 0.0);
}
public RangeMeasurementCreator(final StationDataProvider provider, final Vector3D antennaPhaseCenter, final double bias) {
this.provider = provider;
this.antennaPhaseCenter = antennaPhaseCenter;
this.satellite = new ObservableSatellite(0);
this.bias = bias;
}
public void init(SpacecraftState s0, AbsoluteDate t, double step) {
......@@ -100,7 +110,7 @@ public class RangeMeasurementCreator extends MeasurementCreator {
station.getOffsetToInertial(inertial, emissionDate.shiftedBy(clockOffset)).transformPosition(Vector3D.ZERO);
final double upLinkDistance = Vector3D.distance(position, stationAtEmission);
addMeasurement(new Range(station, true, receptionDate.shiftedBy(clockOffset),
0.5 * (downLinkDistance + upLinkDistance), 1.0, 10, satellite));
0.5 * (downLinkDistance + upLinkDistance) + bias, 1.0, 10, satellite));
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment