ZUPT integrated_accel_constraint Untested Code

[Bingo1024]

Hi, @goldbattle
I have a few questions about zero-velocity update that I would like to ask you.

1. I would like to ask about the meaning of "untested" in the line "bool integrated_accel_constraint = false; // untested". After my deduction based on the code formula, I don't think there's any problem with using velocity as an observation in this way, and I have not found any issues with the results obtained from running the dataset.Could you please explain the potential issues that may arise in this context?

  bool integrated_accel_constraint = false; // untested
  bool model_time_varying_bias = true;
  bool override_with_disparity_check = true;

2. Could you please provide me with theoretical documentation regarding this section of code? I am not quite sure why it is constructed in this way and when it is necessary to set explicitly_enforce_zero_motion = true.

  } else {

    // Propagate the state forward in time
    double time0_cam = last_zupt_state_timestamp;
    double time1_cam = timestamp;
    _prop->propagate_and_clone(state, time1_cam);

    // Create the update system!
    H = Eigen::MatrixXd::Zero(9, 15);
    res = Eigen::VectorXd::Zero(9);
    R = Eigen::MatrixXd::Identity(9, 9);

    // residual (order is ori, pos, vel)
    Eigen::Matrix3d R_GtoI0 = state->_clones_IMU.at(time0_cam)->Rot();
    Eigen::Vector3d p_I0inG = state->_clones_IMU.at(time0_cam)->pos();
    Eigen::Matrix3d R_GtoI1 = state->_clones_IMU.at(time1_cam)->Rot();
    Eigen::Vector3d p_I1inG = state->_clones_IMU.at(time1_cam)->pos();
    res.block(0, 0, 3, 1) = -log_so3(R_GtoI0 * R_GtoI1.transpose());
    res.block(3, 0, 3, 1) = p_I1inG - p_I0inG;
    res.block(6, 0, 3, 1) = state->_imu->vel();
    res *= -1;

    // jacobian (order is q0, p0, q1, p1, v0)
    Hx_order.clear();
    Hx_order.push_back(state->_clones_IMU.at(time0_cam));
    Hx_order.push_back(state->_clones_IMU.at(time1_cam));
    Hx_order.push_back(state->_imu->v());
    if (state->_options.do_fej) {
      R_GtoI0 = state->_clones_IMU.at(time0_cam)->Rot_fej();
    }
    H.block(0, 0, 3, 3) = Eigen::Matrix3d::Identity();
    H.block(0, 6, 3, 3) = -R_GtoI0;
    H.block(3, 3, 3, 3) = -Eigen::Matrix3d::Identity();
    H.block(3, 9, 3, 3) = Eigen::Matrix3d::Identity();
    H.block(6, 12, 3, 3) = Eigen::Matrix3d::Identity();

    // noise (order is ori, pos, vel)
    R.block(0, 0, 3, 3) *= std::pow(1e-2, 2);
    R.block(3, 3, 3, 3) *= std::pow(1e-1, 2);
    R.block(6, 6, 3, 3) *= std::pow(1e-1, 2);

    // finally update and remove the old clone
    StateHelper::EKFUpdate(state, Hx_order, H, res, R);
    StateHelper::marginalize(state, state->_clones_IMU.at(time1_cam));
    state->_clones_IMU.erase(time1_cam);
  }

3. When I set explicitly_enforce_zero_motion to true, my code does not run properly and the program stops at the line Hx_order.push_back(state->_clones_IMU.at(time0_cam)). It seems that the program cannot find the IMU data for this timestamp. I noticed that when the code is running normally, the value of time0_cam at the first entry to this line is 0, which seems abnormal to me.
4. At this point, the old cloned data should be deleted for time0_cam instead of time1_cam, right?

    // finally update and remove the old clone
    StateHelper::EKFUpdate(state, Hx_order, H, res, R);
    StateHelper::marginalize(state, state->_clones_IMU.at(time1_cam));
    state->_clones_IMU.erase(time1_cam);

[Bingo1024]

Hi, @goldbattle
I believe there is an error in the velocity integration in this section. In this loop, the integration always starts from the state of the first frame, and the integration time is the time interval between two IMU frames. I think the velocity should be updated in each loop and the integration should use the velocity of the previous IMU frame instead of always using the first frame.

res.block(6 * i + 3, 0, 3, 1) = -w_accel_v * (state->_imu->vel() - _gravity * dt + state->_imu->Rot().transpose() * a_hat * dt);

[goldbattle]

Sorry, this is quite a bit of comments to answer all at once, so it will take me a while to get to them all.
We don't really support any of the disabled code, and they should probably be removed for clarity.
I think I left them in from testing different implementations of the zero velocity update.

I would like to ask about the meaning of "untested" in the line "bool integrated_accel_constraint = false; // untested". After my deduction based on the code formula, I don't think there's any problem with using velocity as an observation in this way, and I have not found any issues with the results obtained from running the dataset. Could you please explain the potential issues that may arise in this context?

There are a couple of perspectives on the zero velocity update. One can directly do an update saying that the v_IinG = 0_3x1 but the problem with this is how to pick the noise for this update. We care about the consistency of our estimator and picking the wrong noise here would directly cause for an inconsistency if the detection is not perfect. This then comes to the current implementation which says that the current measurements should be zero. This we can reason a bit more about the noise levels here as compared to the velocity update.

For constraining the velocity, the current velocity in the state is at the v_Ik_in_G and not the v_Ik+1_in_G (current time) that we wish to have. So one would need to propagate the velocity to the current frame and then enforce the constraint. I don't remember exactly what the integrated_accel_constraint is doing, so need some time to look into that.

Could you please provide me with theoretical documentation regarding this section of code? I am not quite sure why it is constructed in this way and when it is necessary to set explicitly_enforce_zero_motion = true.

The idea here is to propagate the state forward and say that the relative orientation and position have not changed, and the velocity should be equal to zero. I don't remember if this ever worked well, nor do I expect the current implementation to work as it has not been tested and the codebase has evolved over many years.

At this point, the old cloned data should be deleted for time0_cam instead of time1_cam, right?

If I remember right, the newest clone is the one we want to delete. This is basically a last in first out LIFO, as we have feature observations we want to try to update with at time0_cam once we start moving again. This paper might be of interested on cloning logic changes one could try: https://ieeexplore.ieee.org/document/6696807

[goldbattle]

This code should either be removed or updated to work and exposed to the config.