Issue(?) with Unscented Kalman Filter

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.

Hi @Serrof,

Could it be that the nonlinear transformation from Cartesian to other coordinates introduces the “shift” in predicted mean?
In an unscented transform (UT) the transformed mean depends on the transformed sigma points. If the mapping is nonlinear, the UT mean can be different from mapping the mean directly (as the EKF does).
On Wikipedia there is a nice example of applying UT for coordinate transformation resulting in a different mean: Unscented transform - Wikipedia
Would that explain your result?

Best wishes,
David

Hi David,

you are right, I was getting confused. I actually thought that the so-called predicted measurement was the evaluation of the measurement model on the mean state, but no, if you look at the code, it’s the UT mean through it. And yeah a non-linear function implies that the UT mean is not in general the image of the original mean. The reason why the residual is basically zero in the Cartesian case is because I was using position measurements, so a linear function for this type of coordinates. It’s an “interesting” behaviour tho. I guess a smoother could reduce this “discrepancy”.

Cheers,
Romain.

1 Like