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

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