I posted a similar question in another post.
which I think is open to the public.
I modified the decorate method in the KalmanEstimatorUtil and passed the unnormalized covariance matrix. I think this is alright.
I am using only TDOA measurements. The filter stabilizes with the changes.
Since most of the Kalman filter implementation is hidden, it is possible to create another method to create a MeasurementDecorate class instance 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 UnscentedKalmanEstimator class the estimationStep method has to be modified accordingly.
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();
}
I think this is correct. But not clear how using non-normalized covariance will affect Hipparchus UKF.