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]