Help with Jacobian

[Niek]

Hey all,

I am working on some OD project and I want to implement my own batch filter and Kalman filters. I am currently working on getting the partials wrt the state. I have two questions, first, what does the estimate function do? I have to put in some iterations, evaluation and SpacecraftState, but what does it estimate and in what manner? Next, when I have this estimated state, I can get the partials using the function .getStateDerivative. This gives me the partials of the measurement wrt the cartesian state. However, when I plot the numeric values of these partials, they are constant, which should not happen over time. Am I doing something wrong or am I misunderstanding something?

My code is below,

Thanks in advance!

import datetime
import numpy as np
import orekit
orekit.initVM()

from orekit import JArray_double
from org.hipparchus.geometry.euclidean.threed import Vector3D
from org.orekit.utils import PVCoordinates, Constants, IERSConventions, TimeStampedPVCoordinates, AbsolutePVCoordinates
from org.orekit.frames import TopocentricFrame
from orekit.pyhelpers import setup_orekit_curdir, datetime_to_absolutedate
from org.orekit.time import TimeScalesFactory
from org.orekit.frames import FramesFactory
from org.orekit.estimation.measurements import ObservableSatellite, GroundStation, Range, AngularAzEl, RangeRate 
from org.orekit.estimation.measurements.generation import RangeBuilder,AngularAzElBuilder, RangeRateBuilder, Generator, ContinuousScheduler, GatheringSubscriber
from org.orekit.time import FixedStepSelector

from org.orekit.forces.gravity import NewtonianAttraction
from org.orekit.propagation import SpacecraftState
from org.hipparchus.ode.nonstiff import DormandPrince853Integrator
from org.orekit.propagation.conversion import ClassicalRungeKuttaIntegratorBuilder, DormandPrince853IntegratorBuilder
from org.orekit.propagation.conversion import NumericalPropagatorBuilder
from org.orekit.orbits import CartesianOrbit, PositionAngleType
from math import radians
from org.orekit.propagation.numerical import NumericalPropagator

from org.orekit.orbits import OrbitType

from org.orekit.bodies import OneAxisEllipsoid, GeodeticPoint
from org.orekit.forces.gravity import NewtonianAttraction
import matplotlib.pyplot as plt



setup_orekit_curdir()

#   Define initial conditions
intTime = 60.0 * 60.0 * 4
initPosition = Vector3D(5582294.559435759, 4349734.869382537, -13170.494688864354)
initVelocity = Vector3D(666.6371386727922, -847.3937617778438, 7428.834894791059)
initPV = PVCoordinates(initPosition, initVelocity)
initTime =  datetime.datetime(2024, 1, 29, 3, 3, 55, 960127)
initTimeAbsolute = datetime_to_absolutedate(initTime)
endTimeAbsolute = initTimeAbsolute.shiftedBy(intTime)
initTPV = TimeStampedPVCoordinates(initTimeAbsolute, initPV)

#   Define constants
equatorialRadius = Constants.WGS84_EARTH_EQUATORIAL_RADIUS
flattening = Constants.WGS84_EARTH_FLATTENING
mu = Constants.WGS84_EARTH_MU
utc = TimeScalesFactory.getUTC()


#   Intergration constants
dt = 5.0            # [seconds] for RK4
minStep = 0.001     # [seconds] for DOPRI
maxStep = 300.0     # [seconds] for DOPRI
dP = 1.0           # [meters]  for DOPRI

#   Define frames
lat_gs = radians(52.247019)
lon_gs = radians(6.769447)
alt_gs = 100.0
eci = FramesFactory.getEME2000()
ecef = FramesFactory.getITRF(IERSConventions.IERS_2010, True)
earthShape = OneAxisEllipsoid(equatorialRadius, flattening, ecef)
geoPoint = GeodeticPoint(lat_gs, lon_gs, alt_gs)
topo = TopocentricFrame(earthShape, geoPoint, 'GroundStation')
initATPV = AbsolutePVCoordinates(eci, initTimeAbsolute, initPosition, initVelocity)
initialOrbit = CartesianOrbit(initTPV, eci, mu)
mass = 2400.0

initState = SpacecraftState(initialOrbit, mass)

#   Propagate reference trajectory
orbitType = OrbitType.CARTESIAN
tol = NumericalPropagator.tolerances(dP, initialOrbit, orbitType)
integratorNum = DormandPrince853Integrator(minStep, maxStep, JArray_double.cast_(tol[0]), JArray_double.cast_(tol[1]) )

propagatorNum = NumericalPropagator(integratorNum)
propagatorNum.setOrbitType(orbitType)
propagatorNum.setInitialState(initState)
propagatorNum.addForceModel(NewtonianAttraction(mu))

t = [initTimeAbsolute.shiftedBy(float(dt)) for dt in np.arange(0, intTime, dt)]

state = [propagatorNum.propagate(tt) for tt in t]
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)
absState = []

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)
    absState.append(AbsolutePVCoordinates(eci, t[i], state[i].getPVCoordinates().getPosition(), state[i].getPVCoordinates().getVelocity()))
    
#   Create Scheduler
noiseSource = None
groundStation = GroundStation(topo)
twoWay = True
std = 0.0
baseWeight = 1.0
sat = ObservableSatellite(0)

rangeBuilder = RangeBuilder(noiseSource, groundStation, twoWay, std, baseWeight, sat)
AzElBuilder = AngularAzElBuilder(noiseSource, groundStation, [std, std], [baseWeight, baseWeight], sat)
rangerateBuilder = RangeRateBuilder(noiseSource, groundStation ,twoWay, std, baseWeight, sat)

dataSelectorRange = FixedStepSelector(dt,utc)
dataSelectorAzEl = FixedStepSelector(dt, utc)
dataSelectorRangeRate = FixedStepSelector(dt, utc)

rangeScheduler = ContinuousScheduler(rangeBuilder, dataSelectorRange)
AzElScheduler = ContinuousScheduler(AzElBuilder, dataSelectorAzEl)
rangerateScheduler = ContinuousScheduler(rangerateBuilder, dataSelectorRangeRate)

#   Create Propagator
angleType = PositionAngleType.TRUE
# ODESolverBuilder = ClassicalRungeKuttaIntegratorBuilder(dt)     # TODO: test difference RK4 vs DOPRI
ODESolverBuilder = DormandPrince853IntegratorBuilder(minStep, maxStep, dP)

propagatorBuilder = NumericalPropagatorBuilder(initialOrbit, ODESolverBuilder, angleType, 1.0)
propagatorBuilder.addForceModel(NewtonianAttraction(mu))

SCState = SpacecraftState(initialOrbit)

prop = propagatorBuilder.buildPropagator(propagatorBuilder.getSelectedNormalizedParameters())

#   Creat Subscriber
rangeSubscriber = GatheringSubscriber()
AzElSubscriber = GatheringSubscriber()
rangerateSubscriber = GatheringSubscriber()

#   Create Generator
rangeGenerator = Generator()
AzElGenerator = Generator()
rangerateGenerator = Generator()

#   Add Propagator
rangeGenerator.addPropagator(prop)
AzElGenerator.addPropagator(prop)
rangerateGenerator.addPropagator(prop)


#   Add Subscriber
rangeGenerator.addSubscriber(rangeSubscriber)
AzElGenerator.addSubscriber(AzElSubscriber)
rangerateGenerator.addSubscriber(rangerateSubscriber)

#   Add Scheduler
rangeGenerator.addScheduler(rangeScheduler)
AzElGenerator.addScheduler(AzElScheduler)
rangerateGenerator.addScheduler(rangerateScheduler)



rangeGenerator.generate(initTimeAbsolute, endTimeAbsolute)
rangeMeasurements = rangeSubscriber.getGeneratedMeasurements()
rangeArray = rangeMeasurements.toArray()
numMeas = len(rangeArray)
rangeObject = []
rangeMeasurementsNumeric = np.zeros(numMeas)
for i in range(numMeas):
    rangeObject.append(Range.cast_(rangeArray[i]))
    rangeMeasurementsNumeric[i] = Range.cast_(rangeArray[i]).getObservedValue()[0]


AzElGenerator.generate(initTimeAbsolute, endTimeAbsolute)
AzElMeasurements = AzElSubscriber.getGeneratedMeasurements()
AzElArray = AzElMeasurements.toArray()
azimuthMeasurementsNumeric = np.zeros(numMeas)
azelObject = []
elevationMeasurementsNumeric = np.zeros(numMeas)
for i in range(numMeas):
    azelObject.append(AngularAzEl.cast_(AzElArray[i]))
    azimuthMeasurementsNumeric[i] = AngularAzEl.cast_(AzElArray[i]).getObservedValue()[0]
    elevationMeasurementsNumeric[i] = AngularAzEl.cast_(AzElArray[i]).getObservedValue()[1]

rangerateGenerator.generate(initTimeAbsolute, endTimeAbsolute)
rangerateMeasurements = rangerateSubscriber.getGeneratedMeasurements()
rangerateArray = rangerateMeasurements.toArray()
rangerateObject = []
rangerateMeasurementsNumeric = np.zeros(numMeas)


for i in range(numMeas):
    rangerateObject.append(RangeRate.cast_(rangerateArray[i]))
    rangerateMeasurementsNumeric[i] = RangeRate.cast_(rangerateArray[i]).getObservedValue()[0]

drdX = np.zeros((numMeas, 6))
dazdX = np.zeros((numMeas, 6))
deldX = np.zeros((numMeas, 6))
drrdX = np.zeros((numMeas, 6))
for i in range(numMeas):
    ran = rangeObject[i]
    azel = azelObject[i]
    rr = rangerateObject[i]
    sc_ = SpacecraftState(absState[i])
    est_r = ran.estimate(0,10,sc_)
    est_azel = azel.estimate(i,10,sc_)
    est_rr = rr.estimate(i,10,sc_)
    
    
    drdX_ = est_r.getStateDerivatives(0)
    drdX_ = JArray_double.cast_(drdX_[0])
    
    dazeldX_ = est_azel.getStateDerivatives(0)
    dazdX_ = JArray_double.cast_(dazeldX_[0])
    deldX_ = JArray_double.cast_(dazeldX_[1])
    
    drrdX_ = est_rr.getStateDerivatives(0)
    drrdX_ = JArray_double.cast_(drrdX_[0])
    for k in range(len(drdX_)):
        drdX[:,k] = drdX_[k]
        dazdX[:,k] = dazdX_[k]
        deldX[:,k] = deldX_[k]
        drrdX[:,k] = drrdX_[k]

[MaximeJ]

Hi @Niek,

Sorry for the late reply.
Did you manage to fix your problem already?

@Niek

first, what does the estimate function do? I have to put in some iterations, evaluation and SpacecraftState, but what does it estimate and in what manner?

It estimates the measurement and its derivatives with respect to the spacecraft state. By “estimate” we mean “get the theoretical value (i.e. from modelling)” of the measurement. This estimation is then compared to the real measurement to produce the residuals for the OD algorithm.
“estimate” also moves the spacecraft state in input so that light time delay is taken into account in the measurement.
(iteration, evaluation) are used by the batch least-square OD filter to store the iteration and evaluation number of the algorithm.

@Niek

Am I doing something wrong or am I misunderstanding something?

The derivatives shouldn’t be constant indeed. Are the values also constant? I took a very quick look at your code but I couldn’t find the problem. Maybe you’re always computing or plotting the values of the same estimated measurement?