Measurement Update Derivations » Zero Velocity Update

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.

Constant Velocity Synthetic Measurement

To perform update, we create a synthetic "measurement" which says that the current true acceleration and angular velocity is zero. As compared to saying the velocity is zero, we can model the uncertainty of these measurements based on the readings from our inertial measurement unit.

\begin{align*} \mathbf{a} &= \mathbf{0} \\ \boldsymbol{\omega} &= \mathbf{0} \end{align*}

It is important to realize this is not strictly enforcing zero velocity, but really a constant velocity. This means we can have a false detection at constant velocity times (zero acceleration), but this can be easily addressed by a velocity magnitude check. We have the following measurement equation relating this above synthetic "measurement" to the currently recorded inertial readings:

\begin{align*} \mathbf{a} &= \mathbf{a}_m - \mathbf{b}_a - {}^{I_k}_G\mathbf{R}{}^G\mathbf{g} - \mathbf{n}_a \\ \boldsymbol{\omega} &= \boldsymbol{\omega}_m - \mathbf{b}_g - \mathbf{n}_g \end{align*}

It is important to note that here our actual measurement is the true $\mathbf{a}$ and $\boldsymbol{\omega}$ and thus we will have the following residual where we will subtract the synthetic "measurement" and our measurement function:

\begin{align*} \tilde{\mathbf{z}} &= \begin{bmatrix} \mathbf{a} - \Big(\mathbf{a}_m - \mathbf{b}_a - {}^{I_k}_G\mathbf{R}{}^G\mathbf{g} - \mathbf{n}_a \Big) \\ \boldsymbol{\omega} - \Big(\boldsymbol{\omega}_m - \mathbf{b}_g - \mathbf{n}_g \Big) \end{bmatrix} &= \begin{bmatrix} - \Big(\mathbf{a}_m - \mathbf{b}_a - {}^{I_k}_G\mathbf{R}{}^G\mathbf{g} - \mathbf{n}_a \Big) \\ - \Big(\boldsymbol{\omega}_m - \mathbf{b}_g - \mathbf{n}_g \Big) \end{bmatrix} \end{align*}

Where we have the following Jacobians in respect to our state:

\begin{align*} \frac{\partial \tilde{\mathbf{z}}}{\partial {}^{I_k}_{G}\mathbf{R}} &= - \left\lfloor {}^{I_k}_G\mathbf{R}{}^G\mathbf{g} \times\right\rfloor \\ \frac{\partial \tilde{\mathbf{z}}}{\partial \mathbf{b}_a} &= \frac{\partial \tilde{\mathbf{z}}}{\partial \mathbf{b}_g} = - \mathbf{I}_{3\times 3} \end{align*}

Zero Velocity Detection

Zero velocity detection in itself is a challenging problem which has seen many different works tried to address this issue [41], [36], [9]. Most works boil down to simple thresholding and the approach is to try to determine the optimal threshold which allows for the best classifications of zero velocity update (ZUPT) portion of the trajectories. There have been other works, [41] and [36], which have looked at more complicated methods and try to address the issue that this threshold can be dependent on the type of different motions (such as running vs walking) and characteristics of the platform which the sensor is mounted on (we want to ignore vehicle engine vibrations and other non-essential observed vibrations).

Inertial-based Detection

We approach this detection problem based on tuning of a $\chi^2$ , chi-squared, thresholding based on the measurement model above. It is important to note that we also have a velocity magnitude check which is aimed at preventing constant velocity cases which have non-zero magnitude. More specifically, we perform the following threshold check to see if we are current at zero velocity:

\begin{align*} \tilde{\mathbf{z}}^\top(\mathbf{H}\mathbf{P}\mathbf{H}^\top + \alpha\mathbf{R})^{-1}\tilde{\mathbf{z}} < \chi^2 \end{align*}

We found that in the real world experiments, typically the inertial measurement noise $\mathbf{R}$ needs to be inflated by $\alpha\in[50,100]$ times to allow for proper detection. This can hint that we are using overconfident inertial noises, or that there are additional frequencies (such as the vibration of motors) which inject additional noises.

Disparity-based Detection

We additionally have a detection method which leverages the visual feature tracks. Given two sequential images, the assumption is if there is very little disparity change between feature tracks then we will be stationary. Thus we calculate the average disparity and threshold on this value.

\begin{align*} \frac{1}{N}\sum_{i=0}^N ||\mathbf{uv}_{k,i}-\mathbf{uv}_{k-1,i}|| < \Delta d \end{align*}

This seems to work reasonably well, but can fail if the environment is dynamic in nature, thus it can be useful to use both the inertial and disparity-based methods together in very dynamic environments.