-
Notifications
You must be signed in to change notification settings - Fork 652
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #79 from rpng/feat_zupt
Development v2.2 - Zero Velocity Update
- Loading branch information
Showing
53 changed files
with
1,341,897 additions
and
20,211 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -3,7 +3,7 @@ | |
| /* | ||
| This file is part of m.css. | ||
| Copyright © 2017, 2018, 2019 Vladimír Vondruš <[email protected]> | ||
| Copyright © 2017, 2018, 2019, 2020 Vladimír Vondruš <[email protected]> | ||
| Permission is hereby granted, free of charge, to any person obtaining a | ||
| copy of this software and associated documentation files (the "Software"), | ||
|
|
@@ -422,6 +422,9 @@ mark { | |
| background-color: #f0c63e; | ||
| color: #4c93d3; | ||
| } | ||
| .m-link-wrap { | ||
| word-break: break-all; | ||
| } | ||
| pre, code { | ||
| font-family: 'Source Code Pro', monospace, monospace, monospace; | ||
| font-size: 0.8em; | ||
|
|
@@ -2556,6 +2559,23 @@ article:last-child, article section:last-child { margin-bottom: 0; } | |
| .m-code .il { color: #0000cf; font-weight: bold } | ||
|
|
||
| .m-console .hll { background-color: #ffffcc } | ||
| .m-console .g-AnsiBackgroundBlack { background-color: #232627 } | ||
| .m-console .g-AnsiBackgroundBlue { background-color: #1d99f3 } | ||
| .m-console .g-AnsiBackgroundBrightBlack { background-color: #7f8c8d } | ||
| .m-console .g-AnsiBackgroundBrightBlue { background-color: #3daee9 } | ||
| .m-console .g-AnsiBackgroundBrightCyan { background-color: #16a085 } | ||
| .m-console .g-AnsiBackgroundBrightGreen { background-color: #1cdc9a } | ||
| .m-console .g-AnsiBackgroundBrightMagenta { background-color: #8e44ad } | ||
| .m-console .g-AnsiBackgroundBrightRed { background-color: #c0392b } | ||
| .m-console .g-AnsiBackgroundBrightWhite { background-color: #ffffff } | ||
| .m-console .g-AnsiBackgroundBrightYellow { background-color: #fdbc4b } | ||
| .m-console .g-AnsiBackgroundCyan { background-color: #1abc9c } | ||
| .m-console .g-AnsiBackgroundDefault { background-color: #fcfcfc } | ||
| .m-console .g-AnsiBackgroundGreen { background-color: #11d116 } | ||
| .m-console .g-AnsiBackgroundMagenta { background-color: #9b59b6 } | ||
| .m-console .g-AnsiBackgroundRed { background-color: #ed1515 } | ||
| .m-console .g-AnsiBackgroundWhite { background-color: #fcfcfc } | ||
| .m-console .g-AnsiBackgroundYellow { background-color: #f67400 } | ||
| .m-console .g-AnsiBlack { color: #232627 } | ||
| .m-console .g-AnsiBlue { color: #1d99f3 } | ||
| .m-console .g-AnsiBrightBlack { color: #7f8c8d; font-weight: bold } | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,79 @@ | ||
| /** | ||
|
|
||
|
|
||
| @page update-zerovelocity 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. | ||
|
|
||
|
|
||
| @section update-zerovelocity-meas 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. | ||
|
|
||
| \f{align*}{ | ||
| \mathbf{a} &= \mathbf{0} \\ | ||
| \boldsymbol{\omega} &= \mathbf{0} | ||
| \f} | ||
|
|
||
| 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: | ||
|
|
||
| \f{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 | ||
| \f} | ||
|
|
||
| It is important to note that here our actual measurement is the true \f$\mathbf{a}\f$ and \f$\boldsymbol{\omega}\f$ and thus we will have the following residual where we will subtract the synthetic "measurement" and our measurement function: | ||
|
|
||
| \f{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} | ||
| \f} | ||
|
|
||
| Where we have the following Jacobians in respect to our state: | ||
|
|
||
| \f{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} | ||
| \f} | ||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
| @section update-zerovelocity-detect Zero Velocity Detection | ||
|
|
||
| Zero velocity detection in itself is a challenging problem which has seen many different works tried to address this issue @cite Wagstaff2017IPIN, @cite Ramanandan2011TITS, @cite Davidson2009ENC. | ||
| 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, @cite Wagstaff2017IPIN and @cite Ramanandan2011TITS, 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). | ||
|
|
||
|
|
||
| We approach this detection problem based on tuning of a \f$\chi^2\f$, 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: | ||
|
|
||
| \f{align*}{ | ||
| \tilde{\mathbf{z}}^\top(\mathbf{H}\mathbf{P}\mathbf{H}^\top + \mathbf{R})^{-1}\tilde{\mathbf{z}} < \chi^2 | ||
| \f} | ||
|
|
||
| We found that in the real world experiments, typically the inertial measurement noise \f$\mathbf{R}\f$ needs to be inflated by 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. | ||
|
|
||
|
|
||
|
|
||
| */ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.