深蓝学院-多传感器融合定位-第7章作业

最新推荐文章于 2023-03-26 12:03:29 发布

Weiheng-Summer

最新推荐文章于 2023-03-26 12:03:29 发布

阅读量3.3k

点赞数 3

分类专栏： SLAM 深蓝学院-多传感器融合自动驾驶文章标签： slam 自动驾驶

本文链接：https://blog.csdn.net/weixin_42113967/article/details/115052948

版权

本文介绍了在深蓝学院多传感器融合定位课程中，使用Error-State Kalman Filter进行LiDAR和IMU数据融合的作业实现。作者详细探讨了从及格到优秀各个级别的参数调整，包括Version 1到6的效果，通过APE指标展示了不同版本的定位精度。在优秀题中，作者引入了避免随机游走bias的策略，并展示了在KITTI数据集上的优秀运行结果。

摘要由CSDN通过智能技术生成

深蓝学院-多传感器融合定位-第7章作业

1. 及格题
2. 良好题
3. 优秀题
- Parameter - verison 6 (without Rand-Walk Bias)

Github: https://github.com/WeihengXia0123/LiDar-SLAM

在这里插入图片描述

1. 及格题

(大概整了一两天，终于可以编译了…关键问题在于，docker的环境可能被很多次作业打乱了。在助教葛大佬的提示下，新创建了一个assignments/来放第七章作业的代码，神奇地解决了初始代码的编译问题)

及格部分的代码就是按照深蓝07章课件中的Error-State Kalman Filter的公式来。

注意：和EKF有点区别。

//
// TODO: perform Kalman prediction
//
X_ = F * X_;                                          // fix this
P_ = F * P_ * F.transpose() + B * Q_ * B.transpose(); // fix this


//
// TODO: set measurement:
// 误差：预测值　－　观测值
//
Eigen::Vector3d P_nn_obs = pose_.block<3,1>(0,3) - T_nb.block<3,1>(0,3);             // fix this
Eigen::Matrix3d R_nn_obs = T_nb.block<3,3>(0,0).transpose() * pose_.block<3,3>(0,0); // fix this

YPose_.block<3, 1>(0, 0) = P_nn_obs;
YPose_.block<3, 1>(3, 0) = Sophus::SO3d::vee(R_nn_obs - Eigen::Matrix3d::Identity());

Y = YPose_;

// set measurement equation:
G = GPose_;

//
// TODO: set Kalman gain:
//
MatrixRPose R = RPose_; // fix this
MatrixCPose C = Eigen::MatrixXd::Identity(6,6);
K = P_ * G.transpose() * (G*P_*G.transpose() + C*R*C.transpose()).inverse