算法-IMM

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()));
        }
    }
}
相关推荐
艾莉丝努力练剑39 分钟前
【LeetCode&数据结构】单链表的应用——反转链表问题、链表的中间节点问题详解
c语言·开发语言·数据结构·学习·算法·leetcode·链表
_殊途2 小时前
《Java HashMap底层原理全解析(源码+性能+面试)》
java·数据结构·算法
珊瑚里的鱼6 小时前
LeetCode 692题解 | 前K个高频单词
开发语言·c++·算法·leetcode·职场和发展·学习方法
秋说7 小时前
【PTA数据结构 | C语言版】顺序队列的3个操作
c语言·数据结构·算法
lifallen7 小时前
Kafka 时间轮深度解析:如何O(1)处理定时任务
java·数据结构·分布式·后端·算法·kafka
liupenglove7 小时前
自动驾驶数据仓库:时间片合并算法。
大数据·数据仓库·算法·elasticsearch·自动驾驶
python_tty8 小时前
排序算法(二):插入排序
算法·排序算法
然我8 小时前
面试官:如何判断元素是否出现过?我:三种哈希方法任你选
前端·javascript·算法
F_D_Z9 小时前
【EM算法】三硬币模型
算法·机器学习·概率论·em算法·极大似然估计
秋说9 小时前
【PTA数据结构 | C语言版】字符串插入操作(不限长)
c语言·数据结构·算法