Hi folks,
I’ve noticed a weird behavior with the UKF when the first measurement is at the same date than the initial guess.
Here is the sample code:
@ParameterizedTest
@EnumSource(OrbitType.class)
void test(final OrbitType estimatedOrbitType) {
// GIVEN
final Orbit orbit = new CartesianOrbit(
new PVCoordinates(new Vector3D(7e6, 1, 0), new Vector3D(1e1, 7e3, 1)),
FramesFactory.getGCRF(), AbsoluteDate.ARBITRARY_EPOCH, Constants.GRS80_EARTH_MU);
final ODEIntegratorBuilder integratorBuilder = new ClassicalRungeKuttaIntegratorBuilder(10);
final NumericalPropagatorBuilder propagatorBuilder = new NumericalPropagatorBuilder(
estimatedOrbitType.convertType(orbit),
integratorBuilder, PositionAngleType.TRUE, 1.e0);
final UnscentedKalmanEstimatorBuilder builder = new UnscentedKalmanEstimatorBuilder();
builder.unscentedTransformProvider(new MerweUnscentedTransform(6));
final StateCovariance cartesianCovariance = new StateCovariance(MatrixUtils
.createRealIdentityMatrix(6),
orbit.getDate(), orbit.getFrame(), OrbitType.CARTESIAN, PositionAngleType.MEAN);
final StateCovariance covariance = cartesianCovariance.changeCovarianceType(orbit,
estimatedOrbitType, propagatorBuilder.getPositionAngleType());
final CovarianceMatrixProvider matrixProvider = new ConstantProcessNoise(
covariance.getMatrix(), MatrixUtils.createRealMatrix(6, 6));
builder.addPropagationConfiguration(propagatorBuilder, matrixProvider);
final UnscentedKalmanEstimator estimator = builder.build();
final Observer observer = new Observer();
estimator.setObserver(observer);
final Position observedMeasurement = new Position(orbit.getDate(), orbit.getPosition(),
1, 1, new ObservableSatellite(0));
// WHEN
estimator.estimationStep(observedMeasurement);
// THEN
final Vector3D predictedPosition = new Vector3D(observer.predictedMeasurement.getEstimatedValue());
final Vector3D difference = observedMeasurement.getPosition().subtract(predictedPosition);
Assertions.assertEquals(0., difference.getNorm(), 1e-6);
}
private static class Observer implements KalmanObserver {
EstimatedMeasurement<?> predictedMeasurement;
@Override
public void evaluationPerformed(KalmanEstimation estimation) {
predictedMeasurement = estimation.getPredictedMeasurement();
}
}
Basically, if the estimated coordinates are not Cartesian, there seems to be some numerical noise or something, because the predicted residual (before update) is not as expected. There is a difference of about 0.1m here on positions, which is not negligible.
My naive expectation would be that, if the first measurement is at the same date than the initial guess, the predicted residual is basically fixed (uncertainties should have no impact). It is what I observe with the Extended Kalman Filter anyway.
@markrutten what do U think?
Cheers,
Romain.