From bbd8d3440f7cfa45147033c5cbcc17de0f9992b2 Mon Sep 17 00:00:00 2001 From: DoHaiSon Date: Thu, 23 Oct 2025 14:48:27 +0800 Subject: [PATCH] fix: restore from Gibbs indices to measurement indices --- cpp_ms_glmb_ukf/src/run_filter.hpp | 16 ++++++++++++---- ms_glmb_ukf/run_filter.py | 5 +++++ 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/cpp_ms_glmb_ukf/src/run_filter.hpp b/cpp_ms_glmb_ukf/src/run_filter.hpp index e49642c..584af86 100644 --- a/cpp_ms_glmb_ukf/src/run_filter.hpp +++ b/cpp_ms_glmb_ukf/src/run_filter.hpp @@ -404,13 +404,21 @@ class MSGLMB { if (update_hypcmp_tmp(ivec, 0) < 0) { // check death target in only one sensor off_vec.push_back(ivec); stemp += avqs(tindices(ivec)); - } else if (update_hypcmp_tmp(ivec, 0) == 0) { - not_off_vec.push_back(ivec); - stemp += avps(tindices(ivec)); - } else { // >0 + } else { not_off_vec.push_back(ivec); stemp += avps(tindices(ivec)); } + } + // Restore from Gibbs indices to measurement indices + for (int s = 0; s < mModel.N_sensors; s++) { + for (int i : not_off_vec) { + if (update_hypcmp_tmp(i, s) > 0) { + update_hypcmp_tmp(i, s) = mindices[s](update_hypcmp_tmp(i, s)); + } + } + } + + for (int ivec = 0; ivec < update_hypcmp_tmp.rows(); ivec++) { for (int s = 0; s < update_hypcmp_tmp.cols(); s++) { int mIndex = update_hypcmp_tmp(ivec, s); // measurement index if (mIndex > 0) { diff --git a/ms_glmb_ukf/run_filter.py b/ms_glmb_ukf/run_filter.py index 9967410..5e83e24 100644 --- a/ms_glmb_ukf/run_filter.py +++ b/ms_glmb_ukf/run_filter.py @@ -343,6 +343,11 @@ def msjointpredictupdate(self, model, filter, meas, k): for hidx in range(len(uasses)): update_hypcmp_tmp = uasses[hidx] off_idx = update_hypcmp_tmp[:, 0] < 0 + + # Restore from Gibbs indices to measurement indices + for s in range(model.N_sensors): + update_hypcmp_tmp[~off_idx, s] = mindices[s][update_hypcmp_tmp[~off_idx, s]] + aug_idx = np.column_stack((tindices, update_hypcmp_tmp)) # [tindices, 1 + update_hypcmp_tmp] mis_idx = update_hypcmp_tmp == 0 det_idx = update_hypcmp_tmp > 0