UpdaterZeroVelocity class
Will try to detect and then update using zero velocity assumption.
Contents
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).
Constructors, destructors, conversion operators
-
UpdaterZeroVelocity(UpdaterOptions& options,
NoiseManager& noises,
std::shared_ptr<ov_core::
FeatureDatabase> db, std::shared_ptr<Propagator> prop, double gravity_mag, double zupt_max_velocity, double zupt_noise_multiplier, double zupt_max_disparity) - Default constructor for our zero velocity detector and updater.
Public functions
-
void feed_imu(const ov_core::
ImuData& message, double oldest_time = -1) - Feed function for inertial data.
- void clean_old_imu_measurements(double oldest_time)
- This will remove any IMU measurements that are older then the given measurement time.
- auto try_update(std::shared_ptr<State> state, double timestamp) -> bool
- Will first detect if the system is zero velocity, then will update.
Protected variables
- UpdaterOptions _options
- Options used during update (chi2 multiplier)
- NoiseManager _noises
- Container for the imu noise values.
-
std::shared_ptr<ov_core::
FeatureDatabase> _db - Feature tracker database with all features in it.
- std::shared_ptr<Propagator> _prop
- Our propagator!
- Eigen::Vector3d _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)
- double _zupt_max_disparity
- Max disparity (pixels) that we should consider a zupt with.
- std::map<int, double> chi_squared_table
- Chi squared 95th percentile table (lookup would be size of residual)
-
std::vector<ov_core::
ImuData> imu_data - Our history of IMU messages (time, angular, linear)
- double last_prop_time_offset
- Estimate for time offset at last propagation time.
- double last_zupt_state_timestamp
- Last timestamp we did zero velocity update with.
- int last_zupt_count
- Number of times we have called update.
Function documentation
ov_msckf::
Default constructor for our zero velocity detector and updater.
| Parameters | |
|---|---|
| options | Updater options (chi2 multiplier) |
| noises | imu noise characteristics (continuous time) |
| db | Feature tracker database with all features in it |
| prop | Propagator class object which can predict the state forward in time |
| gravity_mag | Global gravity magnitude of the system (normally 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) |
| zupt_max_disparity | Max disparity we should consider to do a update with |
void ov_msckf::
Feed function for inertial data.
| Parameters | |
|---|---|
| message | Contains our timestamp and inertial information |
| oldest_time | Time that we can discard measurements before |
void ov_msckf::
This will remove any IMU measurements that are older then the given measurement time.
| Parameters | |
|---|---|
| oldest_time | Time that we can discard measurements before (in IMU clock) |
bool ov_msckf::
Will first detect if the system is zero velocity, then will update.
| Parameters | |
|---|---|
| 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 |