Here is the code I used for Maxime’s method:
# 'filt' is the KalmanEstimator and 'meas' an ArrayList of ObservedMeasurement
final_state_prop = filt.processMeasurements(meas)
# Setup the final propagator with Partial Derivatives
final_state_propagator = final_state_prop[0]
partial_derivatives = PartialDerivativesEquations('KalmanProp_PDE', final_state_propagator)
final_kalman_state = final_state_propagator.getInitialState()
final_state_pde = partial_derivatives.setInitialJacobians(final_kalman_state)
final_state_propagator.resetInitialState(final_state_pde)
# Get Jacobians Mapper and reduce Final Covariance to a 6x6
mapper = [partial_derivatives.getMapper()]
# 'cov_out' is a list of covariance matrices for each step during the KalmanEstimation
final_kalman_covar = cov_out[-1]
final_state_covar = MatrixUtils.createRealIdentityMatrix(6)
for i in range(6):
for j in range(6):
final_state_covar.setEntry(i, j, final_kalman_covar.getEntry(i, j))
final_state_covar = [final_state_covar]
# The MasterMode propagator subclass
class FixedStepCovarHandler(PythonOrekitFixedStepHandler):
def init(self, state, *args):
self.states_dict = {}
# 'filt_incl_covar' is a config Boolean specifying if covariance propagation is going to be used
if filt_incl_covar:
self.cov_dict = {}
self.mapper = mapper[0]
self.init_date = state.date
self.init_covar = final_state_covar[0]
def handleStep(self, state, is_last):
epoch = state.date.toString()
pvc = state.pVCoordinates
self.states_dict[epoch] = [pvc.position.x, pvc.position.y, pvc.position.z, pvc.velocity.x, pvc.velocity.y, pvc.velocity.z]
# 'filt_incl_covar' is a config Boolean specifying if covariance propagation is going to be used
if filt_incl_covar:
stm = get_stm(state, self.mapper)
t_step = state.date.durationFrom(self.init_date)
prop_covar = stm.multiply(self.init_covar.multiply(stm.transpose())).add(
xform_vnc_mat_to_j2k(state, get_proc_noise_mat(t_step)).getSubMatrix(0, 5, 0, 5))
# Outputs J2000 covariance lower-triangular for covariance in OEM ephemeris
j2k_covar = [[], [], [], [], [], []]
k = 1
for i in range(6):
for j in range(k):
j2k_covar[i].append(prop_covar.getEntry(i, j))
k += 1
self.cov_dict[epoch] = j2k_covar
if is_last:
pass
# Function to get the STM
def get_stm(state_in, mapper):
stm = MatrixUtils.createRealIdentityMatrix(6)
dYdY0 = JArray('object')(6)
for i in range(6):
dYdY0[i] = JArray('double')([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
mapper.getStateJacobian(state_in, dYdY0)
for i in range(6):
jOrb = 0
for j in range(6):
line = dYdY0[i]
line2 = JArray_double.cast_(line)
stm.setEntry(i, jOrb, line2[j])
jOrb += 1
return stm
# Function to transform VNC to J2000
def xform_vnc_mat_to_j2k(curr_state, cov_mat_vnc):
num_params = cov_mat_vnc.rowDimension
jacobian_double_array = JArray_double2D(num_params, num_params)
LOFType.VNC.transformFromInertial(curr_state.getDate(), curr_state.getPVCoordinates()).getInverse().getJacobian(
CartesianDerivativesFilter.USE_PV, jacobian_double_array)
jacobian = Array2DRowRealMatrix(jacobian_double_array)
cov_mat_j2k_java = jacobian.multiply(cov_mat_vnc.multiply(jacobian.transpose()))
return cov_mat_j2k_java
# Process noise function
def get_proc_noise_mat(t):
q = Array2DRowRealMatrix(6, 6)
for j in range(3):
q.setEntry(j, j, (vnc_acc_sgma[j]**2 * t**4 / 3))
q.setEntry(j, j+3, (vnc_acc_sgma[j]**2 * t**3 / 2))
for k in range(3, 6):
q.setEntry(k, k, (vnc_acc_sgma[k-3]**2 * t**2))
q.setEntry(k, k-3, (vnc_acc_sgma[k-3]**2 * t**3 / 2))
return RealMatrix.cast_(q)