深蓝学院-多传感器融合定位-第7章作业
深蓝学院-多传感器融合定位-第7章作业
- 1. 及格题
- 2. 良好题
-
- Parameter - verison 1
- Parameter - verison 2
- Parameter - verison 3
- Parameter - verison 4
- Parameter - verison 5
- 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(); // 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();
//
// TODO: perform Kalman correct:
//
P_ = (MatrixP::Identity() - K*G) * P_; // fix this
X_ = X_ + K * (Y - G*X_); // fix this
void 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.500344
mean 0.902466
median 0.894143
min 0.161967
rmse 0.917469
sse 3696.964333
std 0.165240
Result - fused
APE w.r.t. full transformation (unit-less)
max 1.549884
mean 0.898618
median 0.898169
min 0.034132
rmse 0.916721
sse 3690.936975
std 0.181281
Parameter - verison 2
process:
gyro: 1.0e-3
accel: 2.5e-2
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.500344
mean 0.901874
median 0.893900
min 0.161967
rmse 0.916922
sse 3690.876634
std 0.165440
Result - fused
APE w.r.t. full transformation (unit-less)
max 1.539430
mean 0.898373
median 0.897470
min 0.058437
rmse 0.916334
sse 3686.143600
std 0.180540
Parameter - verison 3
process:
gyro: 1.0e-5
accel: 2.5e-6
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)
(not aligned)
max 1.136680
mean 0.231626
median 0.164014
min 0.017465
rmse 0.289811
sse 368.550415
std 0.174183
Result - fused
APE w.r.t. full transformation (unit-less)
(not aligned)
max 1.013023
mean 0.258347
median 0.200838
min 0.019931
rmse 0.309749
sse 421.004733
std 0.170884
Parameter - verison 4
process:
gyro: 1.0e-5
accel: 2.5e-6
bias_accel: 2.5e-3
bias_gyro: 1.0e-4
bias_flag: True
measurement:
pose:
pos: 1.0e-4
ori: 1.0e-5
pos: 1.0e-4
vel: 2.5e-3
Result - laser
APE w.r.t. full transformation (unit-less)
max 1.500344
mean 0.901922
median 0.893900
min 0.161967
rmse 0.916961
sse 3691.186379
std 0.165389
Result - fused
APE w.r.t. full transformation (unit-less)
max 1.530646
mean 0.900078
median 0.896358
min 0.033429
rmse 0.917532
sse 3695.785994
std 0.178115
Parameter - verison 5
process:
gyro: 1.0e-6
accel: 2.5e-7
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.500344
mean 0.902180
median 0.894120
min 0.161967
rmse 0.917157
sse 3692.766704
std 0.165068
Result - fused
APE w.r.t. full transformation (unit-less)
max 1.554055
mean 0.898355
median 0.896839
min 0.058776
rmse 0.916404
sse 3686.702189
std 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-5
accel: 2.5e-6
bias_accel: 2.5e-3
bias_gyro: 1.0e-4
bias_flag: False
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.136680
mean 0.231211
median 0.164014
min 0.017465
rmse 0.289257
sse 367.644384
std 0.173813
Result - fused
APE w.r.t. full transformation (unit-less)
max 1.012434
mean 0.257726
median 0.201350
min 0.008808
rmse 0.309306
sse 420.375118
std 0.171020
下面是KITTI数据集跑出来的效果图。
黄色线表示ground-truth，蓝色和红色分别是LiDar单独和LiDar/IMU融合的轨迹。（这里几乎重合，原因是这个参数调得很好，红红火火哈哈哈哈）
