trajectory-prediction程序的imm.cc中的以下代码的对应的算法原理在后面
cpp
void IMM_UKF::InputInteract() {
if (std::isnan(model_pro_(0)) || std::isnan(model_pro_(1)) || std::isnan(model_pro_(2)))
std::abort();
if (model_pro_.sum() != 0)
model_pro_ /= model_pro_.sum();
c_.fill(0.0);
// 遍历所有模型对,根据交互概率interact_pro_(i, j)和模型概率model_pro_(i),计算每个模型j的混合概率c_(j)。
// 这个混合概率表示在考虑模型间交互后,模型j被选中的概率。
for (int j = 0; j < model_size; ++j) {
model_X_[j] = imm_ukf_[j].Get_state();
model_P_[j] = imm_ukf_[j].Get_covariance();
for (int i = 0; i < model_size; ++i) {
c_(j) += interact_pro_(i, j) * model_pro_(i);
}
}
for (int j = 0; j < model_size; ++j) {
X_hat_[j].fill(0.);
P_hat_[j].fill(0.);
for (int i = 0; i < model_size; ++i) {
double u = ((interact_pro_(i, j) * model_pro_(i)) / c_(j));
X_hat_[j] += u * model_X_[i];
}
for (int i = 0; i < model_size; ++i) {
double u = (interact_pro_(i, j) * model_pro_(i)) / c_(j);
P_hat_[j] += (u * (model_P_[i] + (model_X_[i] - X_hat_[j]) * (model_X_[i] - X_hat_[j]).transpose()));
}
}
}