An problem with UKF in predict

[Freeskylover]

// predict
if(!use_imu) {
  pose_estimator->predict(stamp, Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero());
} else {
  std::lock_guard<std::mutex> lock(imu_data_mutex);
  auto imu_iter = imu_data.begin();
  for(imu_iter; imu_iter != imu_data.end(); imu_iter++) {
    if(stamp < (*imu_iter)->header.stamp) {
      break;
    }
    const auto& acc = (*imu_iter)->linear_acceleration;
    const auto& gyro = (*imu_iter)->angular_velocity;
    double gyro_sign = invert_imu ? -1.0 : 1.0;
    pose_estimator->predict((*imu_iter)->header.stamp, Eigen::Vector3f(acc.x, acc.y, acc.z), gyro_sign * Eigen::Vector3f(gyro.x, gyro.y, gyro.z));
  }
  imu_data.erase(imu_data.begin(), imu_iter);
}

**why using the imu_iter on the stamp with observation not the pre_stamp **

[koide3]

Hi @Freeskylover ,
It looks the code is fine, and I couldn't find what is the problem you are pointing out. Could you explain in details?

[Freeskylover]

the imu information timestamp(control_vector) is the same(maybe behind) with the observation timestamp in your code. why not is that the imu information timestamp is before the observation timestamp?

[koide3]

In this code, we give the predict function the timestamp of the IMU data (control vector) but not the point cloud (observation) timestamp, and "if(stamp < (*imu_iter)->header.stamp)" prevents feeding IMU data after the current observation timestamp. So, only IMU data before the observation timestamp are used to predict the UKF state. Is it clear to you?

[Freeskylover]

I understand it, thank for your reply, misunderstand it. you are right.