UnscentedKalmanFilter Covariance matrix issue and not coverging
It seems UKF does not consider the measurement noise. In UnscentedKalmanEstimator class, in the estimationStep method, the state is estimated with final ProcessEstimate estimate = filter.estimationStep(KalmanEstimatorUtil.decorate(observedMeasurement, referenceDate)); KalmanEstimatorUtil.decorate method pass identity matrix for all measurement types except PV and Position. Therefore the UKF tuning seems to be unpredictable. This is not related to EKF as the measurement matrix is scaled by the noise covariances.
This is also affecting parameters ( I have only tried solar radiation pressure) as well.
A possible solution would be to pass the covariance matrix to the Hipparchus Unscented Kalman filter code.
In KalmanEstimatorUtil class: `public static MeasurementDecorator decorateUnscented(final ObservedMeasurement<?> observedMeasurement, final AbsoluteDate referenceDate) {
// Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
// of the measurement on its non-diagonal elements.
// Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
final RealMatrix covariance;
if (observedMeasurement.getMeasurementType().equals(PV.MEASUREMENT_TYPE)) {
// For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
final PV pv = (PV) observedMeasurement;
covariance =MatrixUtils.createRealMatrix(pv.getCovarianceMatrix());
// MatrixUtils.createRealMatrix(pv.getCorrelationCoefficientsMatrix());
} else if (observedMeasurement.getMeasurementType().equals(Position.MEASUREMENT_TYPE)) {
// For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
final Position position = (Position) observedMeasurement;
covariance = MatrixUtils.createRealMatrix(position.getCovarianceMatrix());
//covariance = MatrixUtils.createRealMatrix(position.getCorrelationCoefficientsMatrix());
} else {
// For other measurements we do not have a covariance matrix.
// Thus the correlation coefficients matrix is an identity matrix.
covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
final double[] sigma = observedMeasurement.getTheoreticalStandardDeviation();
for(int i=0;i<sigma.length;i++) {
covariance.setEntry(i, i, sigma[i]*sigma[i]);
}
}
return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);
}
` In UnscentedKalmanFilterModel class
`public Propagator[] estimationStep(final ObservedMeasurement<?> observedMeasurement) { final MeasurementDecorator decoratedMeasurement = KalmanEstimatorUtil.decorateUnscented(observedMeasurement, referenceDate);
final ProcessEstimate estimate = filter.estimationStep(decoratedMeasurement);
processModel.finalizeEstimation(observedMeasurement, estimate);
if (observer != null) {
observer.evaluationPerformed(processModel);
}
return processModel.getEstimatedPropagators();
}
`