class
PropagatorPerforms 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 statetransition 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_
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 statetransition 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  Statetransition matrix over the interval 
Qd  Discretetime 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 Discretetime Errorstate 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.
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 RungeKutta Methods. We are doing a RK4 method, this wolframe page has the forth order equation defined below. We define function where y is a function of time t, see IMU Kinematics for the definition of the continoustime functions.