Weird behaviour in UKF

Niek · March 19, 2024, 8:30pm

Hey all,

I am working on an OD project and I have written my own UKF code. I get very strange results when adding different forces. When I add either spherical harmonics (0,0 Kepler), (1,0) or (1,1) also with Kepler gravity and drag or 3BP(moon), or 3BP(sun), the filter runs fine, however, when I add spherical harmonics (2,0) it completely fails. To propagate the dynamics in the filter I use orekit’s numerical propagator and to calculate the G matrix, I use the measurement generator (to convert cartesian state to observations). I tried parameter tuning for both the P0, Q, sigma points calculation parameters (kappa, lambda, beta, alpha) but nothing seems to work. I don’t know if my code is just wrong somewhere, but because the other cases work, I feel like it works fine. Does anyone know why I have this weird behaviour?

I have an example run here with the following filter parameters:
Initial guess is the true reference trajectory.
for P0 : std_p = 1 km, std_v = 1m/s
for Q: std_a = 1e-3 m/s2
force model : J2
measurements: range, azimuth, elevation, range rate
R: very small, close to 0, only noise errors.
propagator: dormant prince (minstep = 1e-3, maxstep = 1, dp = 1)
update time = 5s

These are the measurements (87 in total):

Estimation residuals:

Position error between estimated state and reference

Velocity error between estimated state and reference

Cheers,

Niek

MaximeJ · March 20, 2024, 9:07am

Hi @Niek,

That’s very weird, could you share some sample code?

(1,0) and (1,1) are null for Earth so the culprit must be how you add the spherical harmonics perturbation.
What I don’t get is, as you’re simulating and estimating the measurements with the same model, you should not see a potentially ill-configured perturbation…

Cheers,
Maxime

Niek · March 20, 2024, 9:19am

That was exactly my thought as well!

So my code is a bit of a mess but I’ll try to walk you through it!

This function is to generate the reference trajectory:

def getReference(x0 = None, t0 = None, integrationTime = None, 
                 write = True, file_name='reference_trajectory.txt', 
                 file_dir='data_files', from_file = False):
    """
    Generate reference orbit
    
    Paremeters:
        - x0:           initial state np.array([px, py, pz, vx, vy, vz])
        - t0:           initial time (datetime)
        - dt:           time step for state interpolation (s)
        - mass:         spacecraft mass (kg)
        - write:        write to file (boolean)
        - file_name:    name of file
        - file_dir:     directory where the file will be stored
        
    Returns:
        - array with time, px, py, pz, vx, vy, vz
    """
    if from_file:
        data = read2file(file_name, file_directory=file_dir)
        t, px, py, pz, vx, vy, vz = data[0], data[1], data[2], data[3], data[4], data[5], data[6]
    else:
        px0, py0, pz0, vx0, vy0, vz0 = x0
        
        pos = Vector3D(float(px0), float(py0), float(pz0))
        vel = Vector3D(float(vx0), float(vy0), float(vz0))
        
        t0_absolute = datetime_to_absolutedate(t0)
    
        tpv0 = TimeStampedPVCoordinates(t0_absolute, pos, vel)
        initialOrbit = CartesianOrbit(tpv0, frames['eci'], EARTH_MU)
        initialSpacecraftState = SpacecraftState(initialOrbit, satellite['mass'])
        
        if propagatorSettings['integration_method'] == 'DOPRI':
            integrator = getDORPIIntegrator(initialOrbit)
        elif propagatorSettings['integration_method'] == 'RK4':
            integrator = getRK4Integrator()

        # Here I select which force model I want to use
        if propagatorSettings['propagation_method'] == 'Kepler':
            propagator = getKeplerPropagator(integrator, initialSpacecraftState)
        elif propagatorSettings['propagation_method'] == 'J2':
            propagator = getJ2Propagator(integrator, initialSpacecraftState)
        elif propagatorSettings['propagation_method'] == 'Advanced':
            propagator = getAdvancedPropagator(integrator, initialSpacecraftState)
    
    
        time_array = [t0_absolute.shiftedBy(float(dt)) for dt in np.arange(0, integrationTime, observationSettings['update_time'])]
    
        state = [propagator.propagate(tt) for tt in time_array]
        numSteps = len(state)
        px = np.zeros(numSteps)
        py = np.zeros(numSteps)
        pz = np.zeros(numSteps)
        vx = np.zeros(numSteps)
        vy = np.zeros(numSteps)
        vz = np.zeros(numSteps)
        t = []
    
        for i in range(numSteps):
            px[i] = float(state[i].getPVCoordinates().getPosition().x)
            py[i] = float(state[i].getPVCoordinates().getPosition().y)
            pz[i] = float(state[i].getPVCoordinates().getPosition().z)
            vx[i] = float(state[i].getPVCoordinates().getVelocity().x)
            vy[i] = float(state[i].getPVCoordinates().getVelocity().y)
            vz[i] = float(state[i].getPVCoordinates().getVelocity().z)
            
            t.append(absolutedate_to_datetime(time_array[i]))
        
        if write:
            write2file(t, px, py, pz, vx, vy, vz,file_name=file_name,file_directory=file_dir)
    return np.array([t, px, py, pz, vx, vy, vz]).T

I have three different force models:

def getKeplerPropagator(integrator, initialSpacecraftState):
    propagator = NumericalPropagator(integrator)
    propagator.setOrbitType(OrbitType.CARTESIAN)
    propagator.setInitialState(initialSpacecraftState)
    propagator.addForceModel(NewtonianAttraction(EARTH_MU))
    return propagator

def getAdvancedPropagator(integrator, initialSpacecraftState):
    propagator = NumericalPropagator(integrator)
    propagator.setOrbitType(OrbitType.CARTESIAN)
    propagator.setInitialState(initialSpacecraftState)
    
    isotropic_drag = IsotropicDrag(satellite['A'], satellite['CD'])
    drag = DragForce(environment['atmosphere'], isotropic_drag)
    gravityProvider = GravityFieldFactory.getNormalizedProvider(10,10)
    radiation = IsotropicRadiationClassicalConvention(satellite['A'], satellite['abs_coef'], satellite['CR'])
    srp = SolarRadiationPressure(environment['sun'], environment['earth'], radiation)
    
    #   Add forces to propagator
    propagator.addForceModel(HolmesFeatherstoneAttractionModel(frames['ecef'], gravityProvider))
    propagator.addForceModel(ThirdBodyAttraction(environment['moon']))    
    propagator.addForceModel(ThirdBodyAttraction(environment['sun']))
    propagator.addForceModel(ThirdBodyAttraction(environment['mars']))
    propagator.addForceModel(ThirdBodyAttraction(environment['jupiter']))
    propagator.addForceModel(ThirdBodyAttraction(environment['saturn']))
    propagator.addForceModel(drag)
    propagator.addForceModel(srp)
    return propagator

def getJ2Propagator(integrator, initialSpacecraftState):
    propagator = NumericalPropagator(integrator)
    propagator.setOrbitType(OrbitType.CARTESIAN)
    propagator.setInitialState(initialSpacecraftState)
    gravityProvider = GravityFieldFactory.getNormalizedProvider(2,0)
    #   Add forces to propagator
    propagator.addForceModel(HolmesFeatherstoneAttractionModel(frames['ecef'], gravityProvider))
    return propagator

For the measurement builder I have something similar to this:

def getTheoreticalMeasurements(x0 = None, t0 = None, integrationTime = None, 
                               write = True, file_name='theoretical_radar_measurements.txt', 
                               file_dir='data_files', from_file = False):
    if from_file:
        data = read2file(file_name, file_directory= file_dir)
        t, r, a, e, rr = data[0], data[1], data[2], data[3], data[4]
    else:
        px0, py0, pz0, vx0, vy0, vz0 = x0
        
        pos = Vector3D(float(px0), float(py0), float(pz0))
        vel = Vector3D(float(vx0), float(vy0), float(vz0))
        
        t0_absolute = datetime_to_absolutedate(t0)
        te_absolute = t0_absolute.shiftedBy(integrationTime)
    
        tpv0 = TimeStampedPVCoordinates(t0_absolute, pos, vel)
        initialOrbit = CartesianOrbit(tpv0, frames['eci'], EARTH_MU)
        
        integrator = getDOPRIBuilder()
        propagator_ = getKeplerPropagatorBuilder(integrator, initialOrbit)
        
        if propagatorSettings['integration_method'] == 'DOPRI':
            integrator = getDOPRIBuilder()
        elif propagatorSettings['integration_method'] == 'RK4':
            integrator = getRK4Builder()
        

# Again, here I select which dynamics model to use
        if propagatorSettings['propagation_method'] == 'Kepler':
            propagator = getKeplerPropagatorBuilder(integrator, initialOrbit)
        elif propagatorSettings['propagation_method'] == 'J2':
            propagator = getJ2PropagatorBuilder(integrator, initialOrbit)
        elif propagatorSettings['propagation_method'] == 'Advanced':
            propagator = getAdvancedPropagatorBuilder(integrator, initialOrbit)
    
        propagator = propagator_.buildPropagator(propagator_.getSelectedNormalizedParameters())
        
        time_array = [t0_absolute.shiftedBy(float(dt)) for dt in np.arange(0, integrationTime, observationSettings['update_time'])]
    
        #   Generate range measurements
        rangeBuilder = RangeBuilder(
            observationSettings['noise_source'], 
            observationSettings['ground_station'],
            observationSettings['two_way'],
            observationSettings['std'],
            observationSettings['base_weight'],
            observationSettings['satellite']
            )
        dataSelectorRange = FixedStepSelector(observationSettings['update_time'],UTC)
        rangeScheduler = ContinuousScheduler(rangeBuilder, dataSelectorRange)
        rangeSubscriber = GatheringSubscriber()
        rangeGenerator = Generator()
        rangeGenerator.addPropagator(propagator)
        rangeGenerator.addSubscriber(rangeSubscriber)
        rangeGenerator.addScheduler(rangeScheduler)
        
        rangeGenerator.generate(t0_absolute, te_absolute)
        rangeMeasurements = rangeSubscriber.getGeneratedMeasurements()
        rangeArray = rangeMeasurements.toArray()
        numMeas = len(rangeArray)
        r = np.zeros(numMeas)
        t = []
        for i in range(numMeas):
            t.append(absolutedate_to_datetime(time_array[i]))
            r[i] = Range.cast_(rangeArray[i]).getObservedValue()[0]
        
        #   Generate angular measuremetns (azimuth and elevation)
        AzElBuilder = AngularAzElBuilder(
            observationSettings['noise_source'], 
            observationSettings['ground_station'],
            [observationSettings['std'], observationSettings['std']],
            [observationSettings['base_weight'], observationSettings['base_weight']],
            observationSettings['satellite']
            )
        dataSelectorAzEl = FixedStepSelector(observationSettings['update_time'], UTC)
        AzElScheduler = ContinuousScheduler(AzElBuilder, dataSelectorAzEl)
        AzElSubscriber = GatheringSubscriber()
        AzElGenerator = Generator()
        AzElGenerator.addPropagator(propagator)
        AzElGenerator.addSubscriber(AzElSubscriber)
        AzElGenerator.addScheduler(AzElScheduler)
        AzElGenerator.generate(t0_absolute, te_absolute)
        AzElMeasurements = AzElSubscriber.getGeneratedMeasurements()
        AzElArray = AzElMeasurements.toArray()
        
        a = np.zeros(numMeas)
        e = np.zeros(numMeas)
        for i in range(numMeas):
            a[i] = AngularAzEl.cast_(AzElArray[i]).getObservedValue()[0]
            e[i] = AngularAzEl.cast_(AzElArray[i]).getObservedValue()[1]
        
        
        #   Generate range rate measurements
        rangerateBuilder = RangeRateBuilder(
            observationSettings['noise_source'], 
            observationSettings['ground_station'],
            observationSettings['two_way'],
            observationSettings['std'],
            observationSettings['base_weight'],
            observationSettings['satellite']
            )
        dataSelectorRangeRate = FixedStepSelector(observationSettings['update_time'], UTC)
        rangerateScheduler = ContinuousScheduler(rangerateBuilder, dataSelectorRangeRate)
        rangerateSubscriber = GatheringSubscriber()
        rangerateGenerator = Generator()
        rangerateGenerator.addPropagator(propagator)
        rangerateGenerator.addSubscriber(rangerateSubscriber)
        rangerateGenerator.addScheduler(rangerateScheduler)
    
        rangerateGenerator.generate(t0_absolute, te_absolute)
        rangerateMeasurements = rangerateSubscriber.getGeneratedMeasurements()
        rangerateArray = rangerateMeasurements.toArray()
        rr = np.zeros(numMeas)
        for i in range(numMeas):
            rr[i] = RangeRate.cast_(rangerateArray[i]).getObservedValue()[0]
    
        if write:
            write2file(t, r, a, e, rr, file_name = file_name, file_directory=file_dir)
    return np.array([t, r, a, e, rr]).T

With the following, similar, dynamics models:

def getKeplerPropagatorBuilder(integrator, initialOrbit):
    propagatorBuilder = NumericalPropagatorBuilder(initialOrbit, integrator, PositionAngleType.TRUE, 1.0)
    propagatorBuilder.addForceModel(NewtonianAttraction(EARTH_MU))
    
    
    return propagatorBuilder

def getAdvancedPropagatorBuilder(integrator, initialOrbit):    
    propagatorBuilder = NumericalPropagatorBuilder(initialOrbit, integrator, PositionAngleType.TRUE, 1.0)


    isotropic_drag = IsotropicDrag(satellite['A'], satellite['CD'])
    drag = DragForce(environment['atmosphere'], isotropic_drag)
    gravityProvider = GravityFieldFactory.getNormalizedProvider(10,10)
    radiation = IsotropicRadiationClassicalConvention(satellite['A'], satellite['abs_coef'], satellite['CR'])
    srp = SolarRadiationPressure(environment['sun'], environment['earth'], radiation)
    
    #   Add forces to propagator
    propagatorBuilder.addForceModel(HolmesFeatherstoneAttractionModel(frames['ecef'], gravityProvider))
    propagatorBuilder.addForceModel(ThirdBodyAttraction(environment['moon']))    
    propagatorBuilder.addForceModel(ThirdBodyAttraction(environment['sun']))
    propagatorBuilder.addForceModel(ThirdBodyAttraction(environment['mars']))
    propagatorBuilder.addForceModel(ThirdBodyAttraction(environment['jupiter']))
    propagatorBuilder.addForceModel(ThirdBodyAttraction(environment['saturn']))
    propagatorBuilder.addForceModel(drag)
    propagatorBuilder.addForceModel(srp)

    return propagatorBuilder

def getJ2PropagatorBuilder(integrator, initialOrbit):
    propagatorBuilder = NumericalPropagatorBuilder(initialOrbit, integrator, PositionAngleType.TRUE, 1.0)
    gravityProvider = GravityFieldFactory.getNormalizedProvider(2,0)
    #   Add forces to propagator
    propagatorBuilder.addForceModel(HolmesFeatherstoneAttractionModel(frames['ecef'], gravityProvider))
    return propagatorBuilder

I think these are the most important snippets of the code, but I uploaded the code below:

main.py (153.5 KB)

Thanks for responding!

MaximeJ · March 20, 2024, 9:55am

Wow that’s a lot of code !

I don’t think I’ll have time to test it today.

Just something I’ve seen, although it shouldn’t fix your issue.
The mass must be explicitly set within a PropagatorBuilder with method setMass. If you don’t the default mass (1000 kg) will be used.
So in your code you will have a discrepancy between the Propagators that have a mass of 2133.730kg and the PropagatorBuilder for measurements’ generation that have a mass of 1000kg.

Niek · March 20, 2024, 11:46am

I know, it is a lot of code but most of it is sumplementary. To run the code you also need the following file, it’s used in the getPd() function (takes SNR from radar measurement and gives the detection chance).

I already want to thank you for looking into this, I don’t really know what is going on and some help is really appreciated!

ROC_Pd_SW3_Pfa7_k4_n12.txt (20.5 KB)

MaximeJ · March 20, 2024, 12:54pm

Thanks @Niek

Apart from the mass issue I mentioned earlier there is something not clear to me in your code.
Sometimes when you create a propagator builder you do not use it because you had an object propagator_ = KeplerianPropagatorBuilder (with an underscore at the end of propagator_) that you always use.
See for example the methods getTheoreticalMeasurements, calc_G, calc_G_UT.
I suspect your measurements are created with a Keplerian model while the reference trajectory is with J2.

And, what is the Unscented Batch filter? I’ve never heard of this filter!

Niek · March 20, 2024, 1:52pm

oh my… that was the problem, I feel so stupid. Sometimes a fresh pair of eyes does the job, thank you so much!

The unscented batch filter is a batch filter based on the unscented transform. I have the paper by Park et. el. here:
Satellite_orbit_determination_using_a_batch_filter_based_on_the_unscented_transformation.pdf (591.6 KB)

MaximeJ · March 20, 2024, 3:01pm

You’re welcome!

Thank you for the paper, I wasn’t aware of it.
It could be a nice addition to Orekit, and we already have all the building blocks.

Serrof · March 20, 2024, 5:47pm

Hi there,

I’m not sure what your end goal is but I would just add that you probably don’t need third bodies other than Sun and Moon. Bare in mind than including negligible perturbations will degrade computational performance.

Cheers,
Romain.

bcazabonne · March 20, 2024, 9:13pm

My next reading