The key idea of the zero velocity update (ZUPT) is to allow for the system to reduce its uncertainty leveraging motion knowledge (i.e. leverage the fact that the system is stationary).
This is of particular importance in cases where we have a monocular system without any temporal SLAM features. In this case, if we are stationary we will be unable to triangulate features and thus will be unable to update the system. This can be avoided by either using a stereo system or temporal SLAM features.
One problem that both of these don't solve is the issue of dynamic environmental objects. In a typical autonomous car scenario the sensor system will become stationary at stop lights in which dynamic objects, such as other cars crossing the intersection, can quickly corrupt the system.
A zero velocity update and skipping feature tracking can address these issues if we are able to classify the cases where the sensor system is at rest.
Zero Velocity Detection
1 2 3 4 5 6 7 8
// Check if we are currently zero velocity // We need to pass the chi2 and not be above our velocity threshold if (!disparity_passed && (chi2 > _options.chi2_multipler * chi2_check || state->_imu->vel().norm() > _zupt_max_velocity)) { last_zupt_state_timestamp = 0.0; printf(YELLOW "[ZUPT]: rejected |v_IinG| = %.3f (chi2 %.3f > %.3f)\n" RESET, state->_imu->vel().norm(), chi2, _options.chi2_multipler * chi2_check); returnfalse; } printf(CYAN "[ZUPT]: accepted |v_IinG| = %.3f (chi2 %.3f < %.3f)\n" RESET, state->_imu->vel().norm(), chi2, _options.chi2_multipler * chi2_check);
Inertial-based Detection
\[
\tilde{\mathbf{z}}^{\top}\left(\mathbf{H P H}^{\top}+\alpha \mathbf{R}\right)^{-1} \tilde{\mathbf{z}}<\chi^{2}
\]
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
// Chi2 distance check // NOTE: we also append the propagation we "would do before the update" if this was to be accepted // NOTE: we don't propagate first since if we fail the chi2 then we just want to return and do normal logic Eigen::MatrixXd P_marg = StateHelper::get_marginal_covariance(state, Hx_order); P_marg.block(3, 3, 6, 6) += Q_bias; Eigen::MatrixXd S = H * P_marg * H.transpose() + R; double chi2 = res.dot(S.llt().solve(res));
// Get our threshold (we precompute up to 1000 but handle the case that it is more) double chi2_check; if (res.rows() < 1000) { chi2_check = chi_squared_table[res.rows()]; } else { boost::math::chi_squared chi_squared_dist(res.rows()); chi2_check = boost::math::quantile(chi_squared_dist, 0.95); printf(YELLOW "[ZUPT]: chi2_check over the residual limit - %d\n" RESET, (int)res.rows()); }
Residual \(\tilde{\mathbf{z}}\)
To perform update, we create a synthetic "measurement" which says that the current true acceleration and angular velocity is zero.
// Check if the image disparity bool disparity_passed = false; if (override_with_disparity_check) { // Get features seen from this image, and the previous image double time0_cam = state->_timestamp; double time1_cam = timestamp; std::vector<std::shared_ptr<Feature>> feats0 = _db->features_containing(time0_cam, false, true);
// Compute the disparity double average_disparity = 0.0; double num_features = 0.0; for (auto &feat : feats0) {
// Get the two uvs for both times for (auto &campairs : feat->timestamps) {
// First find the two timestamps size_t camid = campairs.first; auto it0 = std::find(feat->timestamps.at(camid).begin(), feat->timestamps.at(camid).end(), time0_cam); auto it1 = std::find(feat->timestamps.at(camid).begin(), feat->timestamps.at(camid).end(), time1_cam); if (it0 == feat->timestamps.at(camid).end() || it1 == feat->timestamps.at(camid).end()) continue; auto idx0 = std::distance(feat->timestamps.at(camid).begin(), it0); auto idx1 = std::distance(feat->timestamps.at(camid).begin(), it1);
// 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);