算法-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 小时前
无人机三轴稳定化控制(1)____飞机的稳定控制逻辑
c语言·单片机·算法·无人机
循环过三天1 小时前
3-1 PID算法改进(积分部分)
笔记·stm32·单片机·学习·算法·pid
闪电麦坤951 小时前
数据结构:二维数组(2D Arrays)
数据结构·算法
凌肖战2 小时前
力扣网C语言编程题:快慢指针来解决 “寻找重复数”
c语言·算法·leetcode
埃菲尔铁塔_CV算法2 小时前
基于 TOF 图像高频信息恢复 RGB 图像的原理、应用与实现
人工智能·深度学习·数码相机·算法·目标检测·计算机视觉
NAGNIP3 小时前
一文搞懂FlashAttention怎么提升速度的?
人工智能·算法
Codebee3 小时前
OneCode图生代码技术深度解析:从可视化设计到注解驱动实现的全链路架构
css·人工智能·算法
刘大猫263 小时前
Datax安装及基本使用
java·人工智能·算法
Gyoku Mint6 小时前
深度学习×第4卷:Pytorch实战——她第一次用张量去拟合你的轨迹
人工智能·pytorch·python·深度学习·神经网络·算法·聚类