算法-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()));
        }
    }
}
相关推荐
轮到我狗叫了1 小时前
栈的应用,力扣394.字符串解码力扣946.验证栈序列力扣429.N叉树的层序遍历力扣103.二叉树的锯齿形层序遍历
java·算法·leetcode
pursuit_csdn1 小时前
力扣 238. 除自身以外数组的乘积
数据结构·算法·leetcode
skaiuijing3 小时前
Sparrow系列拓展篇:消息队列和互斥锁等IPC机制的设计
c语言·开发语言·算法·操作系统·arm
C++忠实粉丝4 小时前
计算机网络socket编程(5)_TCP网络编程实现echo_server
网络·c++·网络协议·tcp/ip·计算机网络·算法
kim56595 小时前
excel版数独游戏(已完成)
算法·游戏·excel·数独
cv君5 小时前
【AI最前线】DP双像素sensor相关的AI算法全集:深度估计、图像去模糊去雨去雾恢复、图像重建、自动对焦
算法
Ocean☾5 小时前
C语言-详细讲解-P1217 [USACO1.5] 回文质数 Prime Palindromes
c语言·数据结构·算法
沐泽Mu5 小时前
嵌入式学习-C嘎嘎-Day08
开发语言·c++·算法
Non importa5 小时前
汉诺塔(hanio)--C语言函数递归
c语言·开发语言·算法·学习方法
ac-er88886 小时前
PHP 二分法查找算法
开发语言·算法·php