I’m using BatchLSEstimator for my OD problem and I have an use-case that demands for getting:
a. State transition matrix, Φ(t, t0), of the state
b. Measurement Jacobian, H-matrix, with respect to the last estimated state
It would appreciated if someone could provide tips on how to efficiently extract / compute them after calling the estimator.estimate(); step. Or is it possible to leverage on OrbitDeterminationObserver to achieve this?
I also wonder if there’s any easy API already available. I noticed there exists StateTransitionMatrixGenerator but could not find documentation or relevant tests that supports for how/when to use it.
Any help would be really useful. Thank you in advance.
In an OrbitDeterminationObserver , in method evaluationPerformed(...) you can use lspEvaluation.getJacobian().
At the end of the estimation process, use estimator.getOptimum().getJacobian(), where estimator is your BatchLSEstimator.
Beware that both methods return a normalized matrix.
To “un-normalize” it, follow the same logic as in BatchLSEstimator.getPhysicalCovariances(…): divide each line by the σ of the measurement, and each column by the scale of the estimated parameter.
Once the estimation is finished.
Get the estimated propagator: final Propagator estimatedPropagator = estimator.estimate()[0];
Configure it for state transition matrix computation MatricesHarvester harvester = estimatedPropagator.setupMatricesComputation("STM", null, null);
Propagate from t0 to t: SpacecraftState state = estimatedPropagator.propagate(t0, t)
Get the orbital state transition matrix: RealMatrix dYdY0 = harvester.getStateTransitionMatrix(state);
Or the parameters’ state transition matrix RealMatrix dYdPp = harvester.getParametersJacobian(state);
Yes, you can skip it.
The link to the Kalman example was just to show the overall structure of the state transition matrix (see the comment for example).
On the measurement jacobian side, I’m a bit confused. The lspEvaluation.getJacobian() appears to be a matrix of n rows where n representing the number of flattened (angular = 2 rows) measurements where as the physicalCovariance is just one matrix at t0 where nxm .
Would you mind sharing a sample snippet on the un-normalize part please?
private RealMatrix computeMeasurementJacobianWrtCurrentOrbit(final EstimatedMeasurement<?> estimated,
final Orbit currentOrbit) {
final double[] sigma = estimated.getObservedMeasurement().getTheoreticalStandardDeviation();
// Partial derivatives of the current Cartesian coordinates with respect to current orbital state
final double[][] aCY = new double[6][6];
currentOrbit.getJacobianWrtParameters(builder.getPositionAngleType(), aCY); //dC/dY
final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);
// Jacobian of the measurement with respect to current Cartesian coordinates
final RealMatrix dMdC = new Array2DRowRealMatrix(estimated.getStateDerivatives(0), false);
// Jacobian of the measurement with respect to current orbital state
RealMatrix dMdY = dMdC.multiply(dCdY);
final ParameterDriversList driversList = getOrbitalParametersDrivers(true);
// Fill the normalized measurement matrix's columns related to estimated orbital parameters
for (int i = 0; i < dMdY.getRowDimension(); ++i) {
for (int j = 0; j < dMdY.getColumnDimension(); ++j) {
final ParameterDriver driver = driversList.getDrivers().get(j);
final double partial = dMdY.getEntry(i, j);
final double unNormalized = partial / sigma[i] / driver.getScale();
dMdY.setEntry(i, j, unNormalized);
}
}
return dMdY;
}
private RealMatrix computeMeasurementJacobianWrtPropagationParameters(final SpacecraftState currentState,
final MatricesHarvester harvester,
final RealMatrix dMdY,
final double[] sigma,
final double[] weight) {
// Jacobian of the measurement with respect to propagation parameters
final RealMatrix dYdPp = harvester.getParametersJacobian(currentState);
RealMatrix dMdPp = dMdY.multiply(dYdPp);
final ParameterDriversList driversList = getOrbitalParametersDrivers(true);
for (int i = 0; i < dMdPp.getRowDimension(); ++i) {
for (int j = 0; j < dMdPp.getColumnDimension(); ++j) {
final ParameterDriver driver = driversList.getDrivers().get(j);
final double partial = dMdPp.getEntry(i, j);
final double unNormalized = weight[i] * partial / sigma[i] / driver.getScale();
dMdPp.setEntry(i, j, unNormalized);
}
}
return dMdPp;
}
What you get from currentOrbit.getJacobianWrtParameters and harvester.getParametersJacobian is not normalized. So you don’t need to un-normalize it.
What is normalized is the output of lspEvaluation.getJacobian() in OrbitDeterminationObserver, or the output of BatchLSEstimator.getOptimum().getJacobian().
But if you don’t use them you don’t need to bother with normalization.
if I have to rely on lspEvaluation.getJacobian() then the method should look like this I suppose (based on your comment:
)
public RealMatrix getMeasurementJacobian(final LeastSquaresProblem.Evaluation lspEvaluation,
final EstimationsProvider evaluationsProvider,
final ParameterDriversList estimatedOrbitalParameters,
final ParameterDriversList estimatedPropagatorParameters,
final ParameterDriversList estimatedMeasurementsParameters) {
final RealMatrix jacobians;
// get the normalized matrix
jacobians = lspEvaluation.getJacobian().copy();
// retrieve the measurement sigmas
final double[] sigma = new double[jacobians.getRowDimension()];
int index = 0;
for (int number = 0; number < evaluationsProvider.getNumber(); number++) {
final EstimatedMeasurement<?> estimated = evaluationsProvider.getEstimatedMeasurement(number);
for (double s : estimated.getObservedMeasurement().getTheoreticalStandardDeviation()) {
sigma[index++] = s;
}
}
final double[] scale = getScale(
estimatedOrbitalParameters,
estimatedPropagatorParameters,
estimatedMeasurementsParameters,
jacobians.getColumnDimension());
// unnormalize the matrix, to retrieve physical jac
for (int i = 0; i < jacobians.getRowDimension(); ++i) {
for (int j = 0; j < jacobians.getColumnDimension(); ++j) {
jacobians.setEntry(i, j, jacobians.getEntry(i, j) / (sigma[i] * scale[j]));
}
}
return jacobians;
}
But the values appears to be inconsistent with dMdY and dMdPp from my previous comment. What could I be possibly going wrong?
Just to make sure I understand correctly. Could you please provide some light on what’s difference between from lspEvaluation.getJacobian() -> (un-normalized) vs this
// for first measurement
estimated = evaluationsProvider.getEstimatedMeasurement(0)
// Partial derivatives of the current Cartesian coordinates with respect to current orbital state
final double[][] aCY = new double[6][6];
currentState.getOrbit().getJacobianWrtParameters(builder.getPositionAngleType(), aCY); //dC/dY
final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);
// Jacobian of the measurement with respect to current Cartesian coordinates
final RealMatrix dMdC = new Array2DRowRealMatrix(estimated.getStateDerivatives(0), false);
final RealMatrix dMdY = dMdC.multiply(dCdY);
final RealMatrix dYdY0 = harvester.getStateTransitionMatrix(estimated.getStates()[0]);
// Jacobian of the measurement with respect to initial
final RealMatrix dMdY0 = dMdY.multiply(dYdY0);
I thought they are supposed to be the same, no? Apologies if my question sounds silly. Still learning OREKIT
Actually I figured out what’s the issue. Apparently when the model is in reverse propagation mode, the Jacobians are saved in reverse order but the evaluationProvider does not know that. and so my workaround to to force the for loop in the backward order.
Ah, good catch, note that the forward/backward propagation choice is automatic in the OD (see here) to optimize performances.
As for your first question, I think I forgot to link this piece of code that calculates the Jacobian in BatchLSModel. You’ll find here how the normalized matrix is computed.
Your question isn’t silly, it’s not obvious how the Jacobians are computed in Orekit!