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 thisYPose_.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(); // 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 thisYPose_.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();   //
// TODO: perform Kalman correct:
//P_ = (MatrixP::Identity() - K*G) * P_; // fix this
X_ = X_ + K * (Y - G*X_);              // fix thisvoid ErrorStateKalmanFilter::EliminateError(void) {//// TODO: correct state estimation using the state of ESKF//// a. position:pose_.block<3, 1>(0, 3) =pose_.block<3, 1>(0, 3) - X_.block<3,1>(INDEX_ERROR_POS, 0); // fix this// b. velocity:vel_ = vel_ - X_.block<3,1>(INDEX_ERROR_VEL, 0);                     // fix this// c. orientation:Eigen::Matrix3d R_nn =Sophus::SO3d::hat(X_.block<3, 1>(INDEX_ERROR_ORI, 0)).matrix();pose_.block<3, 3>(0, 0) = pose_.block<3, 3>(0, 0) * (Eigen::MatrixXd::Identity(3,3) - R_nn); // fix this// d. gyro bias:if (COV.PROCESS.BIAS_FLAG && IsCovStable(INDEX_ERROR_GYRO)) {gyro_bias_ += X_.block<3, 1>(INDEX_ERROR_GYRO, 0);}// e. accel bias:if (COV.PROCESS.BIAS_FLAG && IsCovStable(INDEX_ERROR_ACCEL)) {accl_bias_ += X_.block<3, 1>(INDEX_ERROR_ACCEL, 0);}
}

滤波功能基本正常。

2. 良好题

Parameter - verison 1

​        process:
​            gyro: 1.0e-4
​            accel: 2.5e-3
​            bias_accel: 2.5e-3
​            bias_gyro: 1.0e-4
​            bias_flag: True
​        measurement:
​            pose:
​                pos: 1.0e-3
​                ori: 1.0e-4
​            pos: 1.0e-4
​            vel: 2.5e-3

Result - laser
APE w.r.t. full transformation (unit-less)

   max   1.500344mean    0.902466
median  0.894143min 0.161967rmse    0.917469sse 3696.964333std  0.165240

Result - fused
APE w.r.t. full transformation (unit-less)

   max   1.549884mean    0.898618
median  0.898169min 0.034132rmse    0.916721sse 3690.936975std  0.181281

Parameter - verison 2

        process:gyro: 1.0e-3accel: 2.5e-2bias_accel: 2.5e-3bias_gyro: 1.0e-4bias_flag: Truemeasurement:pose:pos: 1.0e-3ori: 1.0e-4pos: 1.0e-4vel: 2.5e-3

Result - laser
APE w.r.t. full transformation (unit-less)

   max   1.500344mean    0.901874
median  0.893900min 0.161967rmse    0.916922sse 3690.876634std  0.165440

Result - fused

APE w.r.t. full transformation (unit-less)

   max   1.539430mean    0.898373
median  0.897470min 0.058437rmse    0.916334sse 3686.143600std  0.180540

Parameter - verison 3

        process:gyro: 1.0e-5accel: 2.5e-6bias_accel: 2.5e-3bias_gyro: 1.0e-4bias_flag: Truemeasurement:pose:pos: 1.0e-3ori: 1.0e-4pos: 1.0e-4vel: 2.5e-3

Result - laser
APE w.r.t. full transformation (unit-less)
(not aligned)

   max   1.136680mean    0.231626
median  0.164014min 0.017465rmse    0.289811sse 368.550415std   0.174183

Result - fused

APE w.r.t. full transformation (unit-less)
(not aligned)

   max   1.013023mean    0.258347
median  0.200838min 0.019931rmse    0.309749sse 421.004733std   0.170884

Parameter - verison 4

        process:gyro: 1.0e-5accel: 2.5e-6bias_accel: 2.5e-3bias_gyro: 1.0e-4bias_flag: Truemeasurement:pose:pos: 1.0e-4ori: 1.0e-5pos: 1.0e-4vel: 2.5e-3

Result - laser
APE w.r.t. full transformation (unit-less)

   max   1.500344mean    0.901922
median  0.893900min 0.161967rmse    0.916961sse 3691.186379std  0.165389

Result - fused

APE w.r.t. full transformation (unit-less)

   max   1.530646mean    0.900078
median  0.896358min 0.033429rmse    0.917532sse 3695.785994std  0.178115

Parameter - verison 5

        process:gyro: 1.0e-6accel: 2.5e-7bias_accel: 2.5e-3bias_gyro: 1.0e-4bias_flag: Truemeasurement:pose:pos: 1.0e-3ori: 1.0e-4pos: 1.0e-4vel: 2.5e-3

Result - laser
APE w.r.t. full transformation (unit-less)

   max   1.500344mean    0.902180
median  0.894120min 0.161967rmse    0.917157sse 3692.766704std  0.165068

Result - fused
APE w.r.t. full transformation (unit-less)

   max   1.554055mean    0.898355
median  0.896839min 0.058776rmse    0.916404sse 3686.702189std  0.180979

3. 优秀题

我加了个bias_flag，只要设为Flase就可以不考虑随即游走的bias。

  // d. gyro bias:if (COV.PROCESS.BIAS_FLAG && IsCovStable(INDEX_ERROR_GYRO)) {gyro_bias_ += X_.block<3, 1>(INDEX_ERROR_GYRO, 0);}// e. accel bias:if (COV.PROCESS.BIAS_FLAG && IsCovStable(INDEX_ERROR_ACCEL)) {accl_bias_ += X_.block<3, 1>(INDEX_ERROR_ACCEL, 0);}

由于跑出来太花时间，这里直只放了一组数据：

Parameter - verison 6 (without Rand-Walk Bias)

       process:gyro: 1.0e-5accel: 2.5e-6bias_accel: 2.5e-3bias_gyro: 1.0e-4bias_flag: Falsemeasurement:pose:pos: 1.0e-3ori: 1.0e-4pos: 1.0e-4vel: 2.5e-3

Result - laser
APE w.r.t. full transformation (unit-less)

   max   1.136680mean    0.231211
median  0.164014min 0.017465rmse    0.289257sse 367.644384std   0.173813

Result - fused

APE w.r.t. full transformation (unit-less)

   max   1.012434mean    0.257726
median  0.201350min 0.008808rmse    0.309306sse 420.375118std   0.171020

下面是KITTI数据集跑出来的效果图。

黄色线表示ground-truth，蓝色和红色分别是LiDar单独和LiDar/IMU融合的轨迹。（这里几乎重合，原因是这个参数调得很好，红红火火哈哈哈哈）

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

1. 深蓝学院-多传感器融合定位课程-第2章-3D激光里程计I

   专栏文章: 深蓝学院-多传感器融合定位课程-第1章-概述_goldqiu的博客-CSDN博客 github保存了相关代码和学习笔记: Shenlan-Course-Multi-Sensor-Fusio ...

2. 深蓝学院-多传感器融合定位课程-第5章-惯性导航原理及误差分析

   专栏文章: 深蓝学院-多传感器融合定位课程-第1章-概述_goldqiu的博客-CSDN博客 深蓝学院-多传感器融合定位课程-第2章-3D激光里程计I_goldqiu的博客-CSDN博客 深蓝学院-多 ...

3. 多传感器融合定位 第四章 点云地图构建及基于点云地图定位

   多传感器融合定位 第四章 点云地图构建及基于点云地图定位 代码下载 https://github.com/kahowang/sensor-fusion-for-localization-and-map ...

4. 多传感器融合定位 第六章 惯性导航结算及误差模型

   多传感器融合定位 第六章 惯性导航结算及误差模型 参考博客:深蓝学院-多传感器融合定位-第6章作业 代码下载:https://github.com/kahowang/sensor-fusion-for ...

5. 看深蓝学院多传感器融合课程的笔记

   贝叶斯推断和贝叶斯公式是整个卡尔曼的核心. 贝叶斯滤波比卡尔曼滤波比卡尔曼滤波更宽泛. 误差作为状态量 融合工程师很多时候是调参工程师,卡尔曼滤波的Q R,需要调参  掌握观测方程的推导.13章第一 ...

6. 多传感器融合定位 第八章 基于滤波的融合方法进阶

   多传感器融合定位 第八章 基于滤波的融合方法进阶 参考博客:深蓝学院-多传感器融合定位-第8章作业 代码下载:https://github.com/kahowang/sensor-fusion-for ...

7. 多传感器融合定位ch1

   多传感器融合定位ch1 目录 多传感器融合定位ch1 1 数据集下载 2 数据集测试 深蓝学院多传感器融合定位课程笔记, 采用的软硬件环境如下 硬件: 华擎DeskMini X300W迷你主机 软件: ...

8. 多传感器融合定位三-3D激光里程计其三：点云畸变补偿

   多传感器融合定位三-3D激光里程计其三:点云畸变补偿 1. 产生原因 2. 补偿方法 Reference: 深蓝学院-多传感器融合 多传感器融合定位理论基础 文章跳转: 多传感器融合定位一-3D激光里 ...

9. 多传感器融合定位五-点云地图构建及定位

   多传感器融合定位五-点云地图构建及定位 1. 回环检测 1.1 基于Scan Context 1.2 基于直方图 2. 后端优化 2.1 后端优化基本原理 2.2 李群.李代数基本知识 2.3 李群. ...

最新文章

1. 美研究揭示大脑如何学习语言
2. 《转》java URL重写
3. 做企业网站为什么要服务器呢,企业为什么要建站？
4. python基础之元组定义进阶操作、字典定义进阶操作
5. 未能启用约束。一行或多行中包含违反非空、唯一或外键约束的值。
6. 天梯—打印沙漏以及剩余个数（C语言）
7. HCIE-Security Day9：5个实验理解NAT Server
8. Springboot项目中static文件和templates文件的区别
9. 怎么把文件传到虚拟机里
10. [python]python生成md5
11. eplan2.5安装教程
12. 如何阅读源码，阅读源码的难点和方法分析
13. nc工具测试tcp/udp协议
14. html与js的相互转化,JS和HTML互转
15. apex老是显示匹配服务器失败,Apex英雄与服务器不同步怎么办-服务器连接超时怎么办 - Iefans...
16. php服务器监控系统,91 Monitor
17. 神经网络有哪些基本功能,常见的神经网络有哪些
18. Keil MDK 选中相同变量 高亮突出显示
19. python淘宝爬虫登陆功能和下单功能_Python 爬虫实战5 模拟登录淘宝并获取所有订单...
20. 01: tornado基础篇

热门文章

1. 2022什么蓝牙耳机好用？适合618入手的无线蓝牙耳机推荐
2. 做压力测试总共需要几个步骤？
3. 关于pandas多级表头,输出Excel文件空白行问题
4. 用BCV备份Oracle数据库
5. c语言fclose导致系统崩溃,c语言中fopen之后不fclose会造成内存泄漏问题
6. 我去！这也太牛了，滴滴大牛都叹服的Java并发原理，深入剖析JDK源码！
7. 新华三 H3C ER2100V2 路由器组网配置
8. windows server Active Directory 域控卸载操作
9. HJ15 求int型正整数在内存中存储时1的个数
10. 计算机系统基础 实验——位运算