Will try to detect and then update using zero velocity assumption.
Consider the case that a VIO unit remains stationary for a period time. Typically this can cause issues in a monocular system without SLAM features since no features can be triangulated. Additional, if features could be triangulated (e.g. stereo) the quality can be poor and hurt performance. If we can detect the cases where we are stationary then we can leverage this to prevent the need to do feature update during this period. The main application would be using this on a wheeled vehicle which needs to stop (e.g. stop lights or parking).
- UpdaterOptions _options
- Options used during update (chi2 multiplier)
- Container for the imu noise values.
- Eigen::Matrix<double, 3, 1> _gravity
- Gravity vector.
- double _zupt_max_velocity
- Max velocity (m/s) that we should consider a zupt with.
- double _zupt_noise_multiplier
- Multiplier of our IMU noise matrix (default should be 1.0)
- std::map<int, double> chi_squared_table
- Chi squared 95th percentile table (lookup would be size of residual)
- Our history of IMU messages (time, angular, linear)
- double last_prop_time_offset
- Estimate for time offset at last propagation time.
UpdaterZeroVelocity:: UpdaterZeroVelocity(UpdaterOptions& options,
Propagator:: NoiseManager& noises,
Default constructor for our zero velocity detector and updater.
|options||Updater options (chi2 multiplier)|
|noises||imu noise characteristics (continuous time)|
|gravity||Global gravity of the system (normally [0,0,9.81])|
|zupt_max_velocity||Max velocity we should consider to do a update with|
|zupt_noise_multiplier||Multiplier of our IMU noise matrix (default should be 1.0)|
UpdaterZeroVelocity:: feed_imu(double timestamp,
Stores incoming inertial readings.
|timestamp||Timestamp of imu reading|
|wm||Gyro angular velocity reading|
|am||Accelerometer linear acceleration reading|
UpdaterZeroVelocity:: try_update(State* state,
Will first detect if the system is zero velocity, then will update.
|state||State of the filter|
|timestamp||Next camera timestamp we want to see if we should propagate to.|
|Returns||True if the system is currently at zero velocity|