算法-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()));
        }
    }
}
相关推荐
pystraf2 分钟前
P8310 〈 TREEのOI 2022 Spring 〉Essential Operations Solution
数据结构·c++·算法·线段树·洛谷
ゞ 正在缓冲99%…11 分钟前
leetcode994.腐烂的橘子
java·算法·leetcode·广度优先
·醉挽清风·14 分钟前
学习笔记—数据结构—二叉树(链式)
c语言·数据结构·c++·笔记·学习·算法
梭七y16 分钟前
【力扣hot100题】(028)删除链表的倒数第N个节点
算法·leetcode·链表
卡尔曼的BD SLAMer17 分钟前
期刊 | 不收版面费与审稿费的电子通信类期刊
算法·信息与通信·信号处理
rainoway41 分钟前
全量加载、懒加载、延迟加载、虚拟列表、canvas、异步分片
前端·算法·性能优化
梭七y44 分钟前
【力扣hot100题】(029)两两交换链表中的节点
算法·leetcode·链表
lyxstart1 小时前
记录个人算法学习4:寻找两个正序数组的中位数
算法
eason_fan1 小时前
前端手撕代码(富途)
算法·面试
在下小航1 小时前
二分查找老出错?5分钟搞懂二分查找常见陷阱
算法·面试