算法-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()));
        }
    }
}
相关推荐
敲代码的嘎仔27 分钟前
JavaWeb零基础学习Day4——Maven
java·开发语言·学习·算法·maven·javaweb·学习方法
聪明的笨猪猪1 小时前
hot 100 (1)—— 两数之和(哈希)
java·经验分享·算法·哈希算法
_dindong1 小时前
牛客101:链表
数据结构·c++·笔记·学习·算法·链表
wuk9981 小时前
基于位置式PID算法调节PWM占空比实现电机转速控制
单片机·嵌入式硬件·算法
不到满级不改名2 小时前
EM算法 & 隐马尔可夫模型
算法
workflower6 小时前
单元测试-例子
java·开发语言·算法·django·个人开发·结对编程
MicroTech20258 小时前
微算法科技(MLGO)研发突破性低复杂度CFG算法,成功缓解边缘分裂学习中的掉队者问题
科技·学习·算法
墨染点香8 小时前
LeetCode 刷题【126. 单词接龙 II】
算法·leetcode·职场和发展
aloha_7899 小时前
力扣hot100做题整理91-100
数据结构·算法·leetcode
Tiny番茄9 小时前
31.下一个排列
数据结构·python·算法·leetcode