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);