# 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 IMUDATA
- Struct for a single imu measurement (time, wm, am)
- struct NoiseManager
- Struct of our imu noise parameters.

## Public static functions

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

## Constructors, destructors, conversion operators

- Propagator(NoiseManager noises, Eigen::Vector3d gravity)
- Default constructor.

## Public functions

- void feed_imu(double timestamp, Eigen::Vector3d wm, Eigen::Vector3d am)
- Stores incoming inertial readings.
- void propagate_and_clone(State* state, double timestamp)
- Propagate state up to given timestamp and then clone.
- void fast_state_propagate(State* state, double timestamp, Eigen::Matrix<double, 13, 1>& state_plus)
- Gets what the state and its covariance will be at a given timestamp.

## Protected functions

- void predict_and_compute(State* state, const IMUDATA data_minus, const 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(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(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<IMUDATA> imu_data
- Our history of IMU messages (time, angular, linear)
- Eigen::Matrix<double, 3, 1> _gravity
- Gravity vector.

## Function documentation

###
static std::vector<IMUDATA> ov_msckf::Propagator:: select_imu_readings(const std::vector<IMUDATA>& imu_data,
double time0,
double time1)

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 |

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 IMUDATA ov_msckf::Propagator:: interpolate_data(const IMUDATA imu_1,
const 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,
Eigen::Vector3d gravity)

Default constructor.

Parameters | |
---|---|

noises | imu noise characteristics (continuous time) |

gravity | Global gravity of the system (normally [0,0,9.81]) |

###
void ov_msckf::Propagator:: feed_imu(double timestamp,
Eigen::Vector3d wm,
Eigen::Vector3d am)

Stores incoming inertial readings.

Parameters | |
---|---|

timestamp | Timestamp of imu reading |

wm | Gyro angular velocity reading |

am | Accelerometer linear acceleration reading |

###
void ov_msckf::Propagator:: propagate_and_clone(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 |

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.

###
void ov_msckf::Propagator:: fast_state_propagate(State* state,
double timestamp,
Eigen::Matrix<double, 13, 1>& state_plus)

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

Parameters | |
---|---|

state | Pointer to state |

timestamp | Time to propagate to |

state_plus | The propagated state (q_GtoI, p_IinG, v_IinG, w_IinI) |

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(State* state,
const IMUDATA data_minus,
const 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(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(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 where y is a function of time t, see IMU Kinematics for the definition of the continous-time functions.