search

hku-mars / FAST_LIO

A computationally efficient and robust LiDAR-inertial odometry (LIO) package
GNU General Public License v2.0
2.78k stars 936 forks source link

Fast-LIO + ZUPT diverge #248

Closed NEU-LC closed 1 year ago

NEU-LC commented 1 year ago

Hello, Thanks for contributing to the excellent repo! I am adding ZUPT update based on Fast-lio, but when I add zero-speed observations and cancel Lidar point2plane observations, it will cause the system to diverge. 

void LaserMapping::ObsModel(
    const state_ikfom& states,
    esekfom::dyn_share_datastruct<double>& ekfom_data) {
  uint32_t measurement_size = 6;
  ekfom_data.h = Eigen::MatrixXd::Zero(measurement_size, 1);
  ekfom_data.h_x = Eigen::MatrixXd::Zero(measurement_size, 15);
  ekfom_data.h.block<3, 1>(0, 0) = -states.vel;
  ekfom_data.h.block<3, 1>(3, 0) = last_iekf_estimator_.get_x().pos - states.pos;
  ekfom_data.h_x.block<3, 3>(0, 12) = -Eigen::Matrix3d::Identity();
  ekfom_data.h_x.block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity();
}
stale[bot] commented 1 year ago

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.