Inter-satellite range measurements in Kalman estimator with EphemerisPropagatorBuilder

[prestonb]

Hello all,

When using an EphemerisPropagatorBuilder in the propagation configuration of the remote satellite of a inter-satellite range measurement in the Kalman Estimator, the estimator produces theoretical measurements that are a few hundred meters off of the expected value. The remote satellite isn’t being estimated by the Kalman estimator. However if a numerical propagator builder is used, the theoretical measurements produced are very close to the observed values.

The EphemerisPropagatorBuilder is made using saved states from the same numerical propagator builder/numerical propagator.

Is this an unintended or overlooked use case of the EphemerisPropagatorBuilder?

I went a created a test that demonstrates the behavior as part of debugging my code.

package org.orekit.estimation.sequential;
 
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.orekit.attitudes.LofOffset;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.estimation.EstimationTestUtils;
import org.orekit.estimation.measurements.EstimatedMeasurementBase.Status;
import org.orekit.estimation.measurements.InterSatellitesRange;
import org.orekit.estimation.measurements.InterSatellitesRangeMeasurementCreator;
import org.orekit.estimation.measurements.ObservedMeasurement;
import org.orekit.estimation.measurements.modifiers.Bias;
import org.orekit.estimation.measurements.modifiers.OutlierFilter;
import org.orekit.forces.ForceModel;
import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel;
import org.orekit.forces.gravity.OceanTides;
import org.orekit.forces.gravity.potential.GravityFieldFactory;
import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.LOFType;
import org.orekit.orbits.CartesianOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.PositionAngleType;
import org.orekit.propagation.BoundedPropagator;
import org.orekit.propagation.EphemerisGenerator;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.SpacecraftStateInterpolator;
import org.orekit.propagation.conversion.*;
import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.propagation.sampling.OrekitFixedStepHandler;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.TimeScalesFactory;
import org.orekit.utils.*;
import org.orekit.utils.ParameterDriversList.DelegatingDriver;
 
import java.util.ArrayList;
import java.util.List;
 
public class KalmanEstimatorInterSatelliteRangeTest {
 
    @BeforeAll
    static void configureData() {
        EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    }
 
    /** Test with the remote satellite being out of the plane of the local satellite. */
    @Test
    void testInterSatellitesRangeWithRemoteBoundedPropagatorBuilder() {
        final AbsoluteDate date = new AbsoluteDate("2024-05-23T00:00:33.111419", TimeScalesFactory.getGPS());
        final Vector3D localPosition = new Vector3D(-37205.80484266836, 3504532.953116421, 6230273.661843251);
        final Vector3D localVelocity = new Vector3D(-1062.2390057078976, 6442.740442569944, -3622.17095245251);
        final TimeStampedPVCoordinates localPV = new TimeStampedPVCoordinates(date, localPosition, localVelocity);
        final CartesianOrbit localOrbit = new CartesianOrbit(localPV, FramesFactory.getEME2000(), Constants.WGS84_EARTH_MU);
        final double remoteSatelliteClockOffset = -2.0E-7;
        final double localSatelliteClockOffset = 6.3E-7;
 
        final Vector3D remotePosition = new Vector3D(-532734.1485239803, 1384032.5346367855, 6991640.04315555);
        final Vector3D remoteVelocity = new Vector3D(-4669.520076713515, 5637.939547078139, -1468.6682081317026);
        final TimeStampedPVCoordinates remotePV = new TimeStampedPVCoordinates(date, remotePosition, remoteVelocity);
        final CartesianOrbit remoteOrbit = new CartesianOrbit(remotePV, FramesFactory.getEME2000(), Constants.WGS84_EARTH_MU);
        System.out.println("Remote with numerical builder");
        genericTest(localOrbit, localSatelliteClockOffset, remoteSatelliteClockOffset, remoteOrbit, true);
        System.out.println("Remote with ephemeris builder");
        genericTest(localOrbit, localSatelliteClockOffset, remoteSatelliteClockOffset, remoteOrbit, false);
    }
 
    /** Generic test to test whether changing the propagator builder for the remote satellite that is not being estimated effects
     *  whether the kalman filter accepts or rejects a measurement.
     * @param localOrbit local satellite initial orbit
     * @param localSatelliteClockOffset local satellite clock offset
     * @param remoteSatelliteClockOffset remote satellite clock offeset
     * @param remoteOrbit remote satellite initial orbit
     * @param remoteAsNumerical flag to choose the propagator builder for the remote satellite, true=numerical false=ephemeris
     */
    public void genericTest(final CartesianOrbit localOrbit,
                            final double localSatelliteClockOffset,
                            final double remoteSatelliteClockOffset,
                            final CartesianOrbit remoteOrbit,
                            final boolean remoteAsNumerical) {
        // create the propagator builders
        final NumericalPropagatorBuilder localNumericalBuilder = createNumericalPropagator(localOrbit);
        final NumericalPropagatorBuilder remoteNumericalBuilder = createNumericalPropagator(remoteOrbit);
 
        // create the propagators
        final NumericalPropagator localPropagator = (NumericalPropagator) localNumericalBuilder.buildPropagator();
        final NumericalPropagator remotePropagator = (NumericalPropagator) remoteNumericalBuilder.buildPropagator();
 
        // generate the ephemeris
        final AbsoluteDate start = localOrbit.getDate();
        final AbsoluteDate stop = localOrbit.getDate().shiftedBy(3.0*localOrbit.getKeplerianPeriod());
 
        final EphemerisGenerator remoteGenerator = remotePropagator.getEphemerisGenerator();
        final FixedStepStateHandler remoteHandler = new FixedStepStateHandler();
        remotePropagator.setStepHandler(60, remoteHandler);
        remotePropagator.propagate(start.shiftedBy(-60), stop.shiftedBy(60));
        final BoundedPropagator remoteEphemeris = remoteGenerator.getGeneratedEphemeris();
 
        // create the inter-satellite range measurements to the remote SV
        final InterSatellitesRangeMeasurementCreator creator = new InterSatellitesRangeMeasurementCreator(remoteEphemeris, localSatelliteClockOffset, remoteSatelliteClockOffset);
        localPropagator.setStepHandler(45, creator);
        localPropagator.propagate(start, stop);
        final List<ObservedMeasurement<?>> interSVRangeMeasurements = creator.getMeasurements();
 
        // we are creating nearly perfect inter-satellite range measurements
        // bias should be close to zero and the outlier filter shouldn't reject any measurements
        final Bias<InterSatellitesRange> interSVRangeBias = new Bias<>(new String[] { "InterSVRange-bias" },
                                                                       new double[] { 0 },
                                                                       new double[] { 1 },
                                                                       new double[] { -100 },
                                                                       new double[] { 100 });
        final OutlierFilter<InterSatellitesRange> filter = new OutlierFilter<>(0, 5);
        final ParameterDriversList measurementParameters = new ParameterDriversList();
        final ParameterDriver biasDriver = interSVRangeBias.getParametersDrivers().get(0);
        biasDriver.setSelected(true);
        measurementParameters.add(biasDriver);
        for (final ObservedMeasurement<?> measurement : interSVRangeMeasurements) {
            ((InterSatellitesRange) measurement).addModifier(interSVRangeBias);
            ((InterSatellitesRange) measurement).addModifier(filter);
        }
 
        final KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
 
        // set up the orbital noise
        final double[] orbitalP = {1e4, 4e3, 1, 5e-3, 6e-5, 1e-4};
        final RealMatrix cartesianOrbitalP = MatrixUtils.createRealDiagonalMatrix(orbitalP);
        final double[] orbitalQ = {1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10};
        final RealMatrix cartesianOrbitalQ = MatrixUtils.createRealDiagonalMatrix(orbitalQ);
        final double[][] dYdC = new double[6][6];
        localOrbit.getJacobianWrtCartesian(localNumericalBuilder.getPositionAngleType(), dYdC);
        final RealMatrix jac = MatrixUtils.createRealMatrix(dYdC);
        final RealMatrix initialP = jac.multiply(cartesianOrbitalP.multiplyTransposed(jac.transpose()));
        final RealMatrix initialQ = jac.multiply(cartesianOrbitalQ.multiplyTransposed(jac.transpose()));
        final ConstantProcessNoise noise = new ConstantProcessNoise(initialP, initialQ);
 
        // set up measurement noise
        final RealMatrix initialPMeas = MatrixUtils.createRealDiagonalMatrix(new double[] {3});
        final RealMatrix initialQMeas = MatrixUtils.createRealDiagonalMatrix(new double[] {1e-3});
        final ConstantProcessNoise measNoise = new ConstantProcessNoise(initialPMeas, initialQMeas);
 
        // configure the remote satellite propagator builder
        final PropagatorBuilder remoteBuilder;
        if (remoteAsNumerical) {
            remoteBuilder = remoteNumericalBuilder;
        } else {
            final SpacecraftStateInterpolator interpolator = new SpacecraftStateInterpolator(4, remoteOrbit.getFrame());
            remoteBuilder = new EphemerisPropagatorBuilder(remoteHandler.getStates(), interpolator, new LofOffset(remoteOrbit.getFrame(), LOFType.LVLH_CCSDS));
        }
        final ParameterDriversList orbitalDriverList = remoteBuilder.getOrbitalParametersDrivers();
        final ParameterDriversList orbitalPropDriverList = remoteBuilder.getPropagationParametersDrivers();
        for (final DelegatingDriver driver : orbitalDriverList.getDrivers()) {
            driver.setSelected(false);
        }
        for (final DelegatingDriver driver : orbitalPropDriverList.getDrivers()) {
            driver.setSelected(false);
        }
 
        // add the propagation configurations
        kalmanBuilder.addPropagationConfiguration(localNumericalBuilder, noise);
        kalmanBuilder.addPropagationConfiguration(remoteBuilder, null);
        kalmanBuilder.estimatedMeasurementsParameters(measurementParameters, measNoise);
 
        // build the kalman estimator
        final KalmanEstimator estimator = kalmanBuilder.build();
 
        // set the observer
        final KalmanObserver observer = new KalmanObserver() {
            @Override
            public void evaluationPerformed(KalmanEstimation estimation) {
                // test the measurements
                // System.out.println(estimation.getCorrectedMeasurement().getStatus().toString());
                final double[] theoretical = estimation.getCorrectedMeasurement().getEstimatedValue();
                final double[] observed = estimation.getCorrectedMeasurement().getObservedValue();
                final double residual = theoretical[0] - observed[0];
                Assertions.assertEquals(0, residual, 1);
                Assertions.assertEquals(Status.PROCESSED, estimation.getCorrectedMeasurement().getStatus());
            }
        };
        estimator.setObserver(observer);
 
        // process the measurements
        for (final ObservedMeasurement<?> measurement : interSVRangeMeasurements) {
            estimator.estimationStep(measurement);
        }
    }
 
    /** Create a numerical propagator with gravity and ocean tides.
     *
     * @param initialOrbit initial satellite orbit
     * @return numerical propagator builder
     */
    public NumericalPropagatorBuilder createNumericalPropagator(final Orbit initialOrbit) {
        final IERSConventions conventions = IERSConventions.IERS_2010;
 
        // Build Propagator
        final double minStep = 1.0e-8;
        final double maxStep = 600;
        final double dP = 0.001;
        final ODEIntegratorBuilder builder = new DormandPrince853IntegratorBuilder(minStep, maxStep, dP);
        final double positionScale = 10;
        final NumericalPropagatorBuilder propagatorBuilder = new NumericalPropagatorBuilder(initialOrbit, builder, PositionAngleType.TRUE, positionScale);
 
        // set mass
        final double mass = 1000;
        propagatorBuilder.setMass(mass);
 
        // Add gravity
        final Frame bodyFrame = FramesFactory.getITRF(conventions, true);
        final double equatorialRadius = Constants.EGM96_EARTH_EQUATORIAL_RADIUS;
        final double flattening = Constants.IERS2010_EARTH_FLATTENING;
        final OneAxisEllipsoid body = new OneAxisEllipsoid(equatorialRadius, flattening, bodyFrame);
        final NormalizedSphericalHarmonicsProvider gravityField = GravityFieldFactory.getNormalizedProvider(50, 50);
        final ForceModel gravityModel = new HolmesFeatherstoneAttractionModel(body.getBodyFrame(), gravityField);
        propagatorBuilder.addForceModel(gravityModel);
 
        // Set Attitude
        propagatorBuilder.setAttitudeProvider(new LofOffset(initialOrbit.getFrame(), LOFType.LVLH_CCSDS));
 
        // Add ocean tides
        final int oceanDegree = 4;
        final int oceanOrder = 4;
        final ForceModel tidesModel = new OceanTides(body.getBodyFrame(),
                gravityField.getAe(),
                gravityField.getMu(),
                oceanDegree,
                oceanOrder,
                conventions,
                TimeScalesFactory.getUT1(conventions, true));
        propagatorBuilder.addForceModel(tidesModel);
 
        return propagatorBuilder;
    }
 
    private static class FixedStepStateHandler implements OrekitFixedStepHandler {
 
        final List<SpacecraftState> states;
 
        public FixedStepStateHandler() {
            states = new ArrayList<>();
        }
 
        public List<SpacecraftState> getStates() {
            return this.states;
        }
 
        @Override
        public void handleStep(SpacecraftState state) {
            states.add(state);
        }
 
        @Override
        public void finish(final SpacecraftState finalState) {
            states.add(finalState);
        }
    }
 
}

[MaximeJ]

Hi @prestonb,

I’ve reproduced your error and it looks like a bug to me.

I’ve checked the innovations (predicted - observed) and they’re close to 0, so I don’t think it comes from the propagation of the remote satellite itself.

Using the debugger it seems it happens only at the very end of the estimation step. So the orbit of the “local” satellite is actually estimated correctly.

I think the problem comes from KalmanModel.finalizeEstimation. Here the attribute initialState of Ephemeris is not reset after each call to propagate. Thus the corrected state of the “remote” propagator is always equal to the state at dt=0, it is as if the remote satellite wasn’t moving. This explains why the corrected intersallite range measurement keeps growing with time.

Could you please open an issue on the forge and reference this conversation into it?

Cheers,
Maxime

[MaximeJ]

Confirmed,

Here’s a “dirty fix” to make it work.

In KalmanModel.finalizeEstimation, l. 360, replace:

        // Get the estimated propagator (mirroring parameter update in the builder)
        // and the estimated spacecraft state
        final Propagator[] estimatedPropagators = getEstimatedPropagators();
        for (int k = 0; k < estimatedPropagators.length; ++k) {
            setCorrectedSpacecraftState(estimatedPropagators[k].getInitialState(), k);
        }

With:

        // Get the estimated propagator (mirroring parameter update in the builder)
        // and the estimated spacecraft state
        final Propagator[] estimatedPropagators = getEstimatedPropagators();
        for (int k = 0; k < estimatedPropagators.length; ++k) {
            final SpacecraftState initialState = estimatedPropagators[k].getInitialState();

            if (initialState.getDate().isEqualTo(observedMeasurement.getDate())) {
                setCorrectedSpacecraftState(initialState, k);
            } else {
                setCorrectedSpacecraftState(estimatedPropagators[k].propagate(observedMeasurement.getDate()), k);
            }
        }

I haven’t checked the estimated states of the local satellite but the innovations and residuals of the inter-satellite range measurements are actually better with the ephemeris than with the numerical propagator. See the figures below.
I don’t know why. Neither do I have an explanation for the two spikes around measures 180 and 320…

Cheers,
Maxime

[prestonb]

Thanks for the response Maxime!

I went an created an issue on the gitlab.

-Preston

[MaximeJ]

Thanks a lot Preston!