ov_msckf::Propagator class

Performs the state covariance and mean propagation using imu measurements.

Contents

We will first select what measurements we need to propagate with. We then compute the state transition matrix at each step and update the state and covariance. For derivations look at IMU Propagation Derivations page which has detailed equations.

Public types

struct NoiseManager
Struct of our imu noise parameters.

Public static functions

static auto select_imu_readings(const std::vector<ov_core::ImuData>& imu_data, double time0, double time1, bool warn = true) -> std::vector<ov_core::ImuData>
Helper function that given current imu data, will select imu readings between the two times.
static auto interpolate_data(const ov_core::ImuData& imu_1, const ov_core::ImuData& imu_2, double timestamp) -> ov_core::ImuData
Nice helper function that will linearly interpolate between two imu messages.

Constructors, destructors, conversion operators

Propagator(NoiseManager noises, double gravity_mag)
Default constructor.

Public functions

void feed_imu(const ov_core::ImuData& message, double oldest_time = -1)
Stores incoming inertial readings.
void propagate_and_clone(std::shared_ptr<State> state, double timestamp)
Propagate state up to given timestamp and then clone.
auto fast_state_propagate(std::shared_ptr<State> state, double timestamp, Eigen::Matrix<double, 13, 1>& state_plus, Eigen::Matrix<double, 12, 12>& covariance) -> bool
Gets what the state and its covariance will be at a given timestamp.

Protected functions

void predict_and_compute(std::shared_ptr<State> state, const ov_core::ImuData& data_minus, const ov_core::ImuData& data_plus, Eigen::Matrix<double, 15, 15>& F, Eigen::Matrix<double, 15, 15>& Qd)
Propagates the state forward using the imu data and computes the noise covariance and state-transition matrix of this interval.
void predict_mean_discrete(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat1, const Eigen::Vector3d& a_hat1, const Eigen::Vector3d& w_hat2, const Eigen::Vector3d& a_hat2, Eigen::Vector4d& new_q, Eigen::Vector3d& new_v, Eigen::Vector3d& new_p)
Discrete imu mean propagation.
void predict_mean_rk4(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat1, const Eigen::Vector3d& a_hat1, const Eigen::Vector3d& w_hat2, const Eigen::Vector3d& a_hat2, Eigen::Vector4d& new_q, Eigen::Vector3d& new_v, Eigen::Vector3d& new_p)
RK4 imu mean propagation.

Protected variables

double last_prop_time_offset
Estimate for time offset at last propagation time.
NoiseManager _noises
Container for the noise values.
std::vector<ov_core::ImuData> imu_data
Our history of IMU messages (time, angular, linear)
Eigen::Vector3d _gravity
Gravity vector.

Function documentation

static std::vector<ov_core::ImuData> ov_msckf::Propagator::select_imu_readings(const std::vector<ov_core::ImuData>& imu_data, double time0, double time1, bool warn = true)

Helper function that given current imu data, will select imu readings between the two times.

Parameters
imu_data IMU data we will select measurements from
time0 Start timestamp
time1 End timestamp
warn If we should warn if we don't have enough IMU to propagate with (e.g. fast prop will get warnings otherwise)
Returns Vector of measurements (if we could compute them)

This will create measurements that we will integrate with, and an extra measurement at the end. We use the interpolate_data() function to "cut" the imu readings at the begining and end of the integration. The timestamps passed should already take into account the time offset values.

static ov_core::ImuData ov_msckf::Propagator::interpolate_data(const ov_core::ImuData& imu_1, const ov_core::ImuData& imu_2, double timestamp)

Nice helper function that will linearly interpolate between two imu messages.

Parameters
imu_1 imu at begining of interpolation interval
imu_2 imu at end of interpolation interval
timestamp Timestamp being interpolated to

This should be used instead of just "cutting" imu messages that bound the camera times Give better time offset if we use this function, could try other orders/splines if the imu is slow.

ov_msckf::Propagator::Propagator(NoiseManager noises, double gravity_mag)

Default constructor.

Parameters
noises imu noise characteristics (continuous time)
gravity_mag Global gravity magnitude of the system (normally 9.81)

void ov_msckf::Propagator::feed_imu(const ov_core::ImuData& message, double oldest_time = -1)

Stores incoming inertial readings.

Parameters
message Contains our timestamp and inertial information
oldest_time Time that we can discard measurements before

void ov_msckf::Propagator::propagate_and_clone(std::shared_ptr<State> state, double timestamp)

Propagate state up to given timestamp and then clone.

Parameters
state Pointer to state
timestamp Time to propagate to and clone at (CAM clock frame)

This will first collect all imu readings that occured between the current state time and the new time we want the state to be at. If we don't have any imu readings we will try to extrapolate into the future. After propagating the mean and covariance using our dynamics, We clone the current imu pose as a new clone in our state.

bool ov_msckf::Propagator::fast_state_propagate(std::shared_ptr<State> state, double timestamp, Eigen::Matrix<double, 13, 1>& state_plus, Eigen::Matrix<double, 12, 12>& covariance)

Gets what the state and its covariance will be at a given timestamp.

Parameters
state Pointer to state
timestamp Time to propagate to (IMU clock frame)
state_plus The propagated state (q_GtoI, p_IinG, v_IinI, w_IinI)
covariance The propagated covariance (q_GtoI, p_IinG, v_IinI, w_IinI)
Returns True if we were able to propagate the state to the current timestep

This can be used to find what the state will be in the "future" without propagating it. This will propagate a clone of the current IMU state and its covariance matrix. This is typically used to provide high frequency pose estimates between updates.

void ov_msckf::Propagator::predict_and_compute(std::shared_ptr<State> state, const ov_core::ImuData& data_minus, const ov_core::ImuData& data_plus, Eigen::Matrix<double, 15, 15>& F, Eigen::Matrix<double, 15, 15>& Qd) protected

Propagates the state forward using the imu data and computes the noise covariance and state-transition matrix of this interval.

Parameters
state Pointer to state
data_minus imu readings at beginning of interval
data_plus imu readings at end of interval
F State-transition matrix over the interval
Qd Discrete-time noise covariance over the interval

This function can be replaced with analytical/numerical integration or when using a different state representation. This contains our state transition matrix along with how our noise evolves in time. If you have other state variables besides the IMU that evolve you would add them here. See the Discrete-time Error-state Propagation page for details on how this was derived.

void ov_msckf::Propagator::predict_mean_discrete(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat1, const Eigen::Vector3d& a_hat1, const Eigen::Vector3d& w_hat2, const Eigen::Vector3d& a_hat2, Eigen::Vector4d& new_q, Eigen::Vector3d& new_v, Eigen::Vector3d& new_p) protected

Discrete imu mean propagation.

Parameters
state Pointer to state
dt Time we should integrate over
w_hat1 Angular velocity with bias removed
a_hat1 Linear acceleration with bias removed
w_hat2 Next angular velocity with bias removed
a_hat2 Next linear acceleration with bias removed
new_q The resulting new orientation after integration
new_v The resulting new velocity after integration
new_p The resulting new position after integration

See IMU Propagation Derivations for details on these equations.

\begin{align*} \text{}^{I_{k+1}}_{G}\hat{\bar{q}} &= \exp\bigg(\frac{1}{2}\boldsymbol{\Omega}\big({\boldsymbol{\omega}}_{m,k}-\hat{\mathbf{b}}_{g,k}\big)\Delta t\bigg) \text{}^{I_{k}}_{G}\hat{\bar{q}} \\ ^G\hat{\mathbf{v}}_{k+1} &= \text{}^G\hat{\mathbf{v}}_{I_k} - {}^G\mathbf{g}\Delta t +\text{}^{I_k}_G\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t\\ ^G\hat{\mathbf{p}}_{I_{k+1}} &= \text{}^G\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k} \Delta t - \frac{1}{2}{}^G\mathbf{g}\Delta t^2 + \frac{1}{2} \text{}^{I_k}_{G}\hat{\mathbf{R}}^\top(\mathbf{a}_{m,k} - \hat{\mathbf{b}}_{\mathbf{a},k})\Delta t^2 \end{align*}

void ov_msckf::Propagator::predict_mean_rk4(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat1, const Eigen::Vector3d& a_hat1, const Eigen::Vector3d& w_hat2, const Eigen::Vector3d& a_hat2, Eigen::Vector4d& new_q, Eigen::Vector3d& new_v, Eigen::Vector3d& new_p) protected

RK4 imu mean propagation.

Parameters
state Pointer to state
dt Time we should integrate over
w_hat1 Angular velocity with bias removed
a_hat1 Linear acceleration with bias removed
w_hat2 Next angular velocity with bias removed
a_hat2 Next linear acceleration with bias removed
new_q The resulting new orientation after integration
new_v The resulting new velocity after integration
new_p The resulting new position after integration

See this wikipedia page on Runge-Kutta Methods. We are doing a RK4 method, this wolframe page has the forth order equation defined below. We define function $ f(t,y) $ where y is a function of time t, see IMU Kinematics for the definition of the continous-time functions.

\begin{align*} {k_1} &= f({t_0}, y_0) \Delta t \\ {k_2} &= f( {t_0}+{\Delta t \over 2}, y_0 + {1 \over 2}{k_1} ) \Delta t \\ {k_3} &= f( {t_0}+{\Delta t \over 2}, y_0 + {1 \over 2}{k_2} ) \Delta t \\ {k_4} &= f( {t_0} + {\Delta t}, y_0 + {k_3} ) \Delta t \\ y_{0+\Delta t} &= y_0 + \left( {{1 \over 6}{k_1} + {1 \over 3}{k_2} + {1 \over 3}{k_3} + {1 \over 6}{k_4}} \right) \end{align*}