算法-卡尔曼运动模型公式推导CTRA

cpp 复制代码
if (model_ == "CTRA") {
            double px_pre, py_pre, velo_pre, yaw_pre, yawd_pre, a_pre;
            if (fabs(yawd_k) > 0.001) {
                px_pre = px_k + ((velo_k * yawd_k + a_k * yawd_k * deltat) * std::sin(yaw_k + yawd_k * deltat) +
                    a_k * std::cos(yaw_k + yawd_k * deltat) - velo_k * yawd_k * std::sin(yaw_k) -
                    a_k * std::cos(yaw_k)) / (yawd_k * yawd_k);
                py_pre = py_k + ((-velo_k * yawd_k - a_k * yawd_k * deltat) * std::cos(yaw_k + yawd_k * deltat) +
                    a_k * std::sin(yaw_k + yawd_k * deltat) + velo_k * yawd_k * std::cos(yaw_k) -
                    a_k * std::sin(yaw_k)) / (yawd_k * yawd_k);
            }

公式推导参考:https://zhuanlan.zhihu.com/p/389589611

相关推荐
郝学胜-神的一滴1 小时前
Effective STL 条款03:容器中的对象复制要廉价且正确
开发语言·数据结构·c++·算法·rpc·stl
秋天的怀念_David1 小时前
算法一千零一夜 · 第三十三夜 · 跳表
java·程序人生·算法
Pniubi4 小时前
力扣21合并两个有序链表
算法·leetcode·职场和发展
AIgorithmGEEK4 小时前
std::unordered_map 与 std::unordered_set:接口、算法技巧与权衡 (速查)
c++·算法·哈希表·负载因子·桶·unordered_map·unordered_set
上海潭龙东海科技有限公司6 小时前
智慧堆场动态模型三维实时孪生重构智能管控
大数据·网络·数据库·算法·重构
阿庆_AI研发工程师8 小时前
Stanford CS336 Lecture 10:大语言模型推理与性能优化
算法
用户7341681035488 小时前
基于MindSpore的深度残差收缩网络程序
算法
Pniubi9 小时前
力扣239.滑动窗口最大值(优先队列)
算法·leetcode·职场和发展
青少儿编程课堂10 小时前
第十八届蓝桥杯青少组图形化真题 + 全国青少年信息素养大赛 + 电子学会图形化考级:校园泡泡堂大挑战解析
c++·python·算法·bfs·信息学竞赛
超低空11 小时前
一文说清LRU算法
后端·算法·架构