From 2f36abf602c269dcfa515c65c610ac42c48a4c18 Mon Sep 17 00:00:00 2001 From: Steven Hiscocks Date: Fri, 23 Aug 2024 10:20:56 +0100 Subject: [PATCH] Fix ASD with OOSM When using ASD with out of sequence measurements, the covariance prior to OOSM measurement would go negative. Through testing, it's highlighted that setting the correlation matrices predicted covariance to that one based on original posterior (i.e. approx value it would have been set to if measurement was received in sequence) seems to resolve the issue. --- stonesoup/predictor/asd.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/stonesoup/predictor/asd.py b/stonesoup/predictor/asd.py index 86ccbd935..1149627dd 100644 --- a/stonesoup/predictor/asd.py +++ b/stonesoup/predictor/asd.py @@ -180,13 +180,19 @@ def predict(self, prior, timestamp, **kwargs): P_right_lower = prior.multi_covar[t_index * ndim:, t_index * ndim:] + # Calculate what would be the orignal predicted covariance + p_state_pred = GaussianState( + x_pred_m, correlation_matrices[t_index]['P'], prior.timestamp) + p_p_pred = self._predicted_covariance( + p_state_pred, predict_over_interval=predict_over_interval) + # add new correlation matrix with the present time step correlation_matrices[t_index] = pred_from_corr_matrices = \ correlation_matrices[t_index].copy() - pred_from_corr_matrices['P_pred'] = p_pred_m + pred_from_corr_matrices['P_pred'] = p_p_pred pred_from_corr_matrices['F'] = transition_matrix_m pred_from_corr_matrices['PFP'] = ( - pred_from_corr_matrices['P'] @ transition_matrix_m.T @ np.linalg.inv(p_pred_m)) + pred_from_corr_matrices['P'] @ transition_matrix_m.T @ np.linalg.inv(p_p_pred)) correlation_matrices.insert(t_index, {}) correlation_matrices[t_index]['F'] = transition_matrix_m_plus_1