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 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.
static auto compute_H_Dw(std::shared_ptr<State> state, const Eigen::Vector3d& w_uncorrected) -> Eigen::MatrixXd
compute the Jacobians for Dw
static auto compute_H_Da(std::shared_ptr<State> state, const Eigen::Vector3d& a_uncorrected) -> Eigen::MatrixXd
compute the Jacobians for Da
static auto compute_H_Tg(std::shared_ptr<State> state, const Eigen::Vector3d& a_inI) -> Eigen::MatrixXd
compute the Jacobians for Tg

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 clean_old_imu_measurements(double oldest_time)
This will remove any IMU measurements that are older then the given measurement time.
void invalidate_cache()
Will invalidate the cache used for fast propagation.
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::MatrixXd& F, Eigen::MatrixXd& 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_hat, const Eigen::Vector3d& a_hat, 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.
void compute_Xi_sum(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat, const Eigen::Vector3d& a_hat, Eigen::Matrix<double, 3, 18>& Xi_sum)
Analytically compute the integration components based on ACI^2.
void predict_mean_analytic(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat, const Eigen::Vector3d& a_hat, Eigen::Vector4d& new_q, Eigen::Vector3d& new_v, Eigen::Vector3d& new_p, Eigen::Matrix<double, 3, 18>& Xi_sum)
Analytically predict IMU mean based on ACI^2.
void compute_F_and_G_analytic(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat, const Eigen::Vector3d& a_hat, const Eigen::Vector3d& w_uncorrected, const Eigen::Vector3d& a_uncorrected, const Eigen::Vector4d& new_q, const Eigen::Vector3d& new_v, const Eigen::Vector3d& new_p, const Eigen::Matrix<double, 3, 18>& Xi_sum, Eigen::MatrixXd& F, Eigen::MatrixXd& G)
Analytically compute state transition matrix F and noise Jacobian G based on ACI^2.
void compute_F_and_G_discrete(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat, const Eigen::Vector3d& a_hat, const Eigen::Vector3d& w_uncorrected, const Eigen::Vector3d& a_uncorrected, const Eigen::Vector4d& new_q, const Eigen::Vector3d& new_v, const Eigen::Vector3d& new_p, Eigen::MatrixXd& F, Eigen::MatrixXd& G)
compute state transition matrix F and noise Jacobian G

Protected variables

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.

static Eigen::MatrixXd ov_msckf::Propagator::compute_H_Dw(std::shared_ptr<State> state, const Eigen::Vector3d& w_uncorrected)

compute the Jacobians for Dw

Parameters
state Pointer to state
w_uncorrected Angular velocity in a frame with bias and gravity sensitivity removed

See IMU Reading Linearization for details.

\begin{align*} \mathbf{H}_{Dw,kalibr} & = \begin{bmatrix} {}^w\hat{w}_1 \mathbf{I}_3 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_2\mathbf{e}_3 & {}^w\hat{w}_3 \mathbf{e}_3 \end{bmatrix} \\ \mathbf{H}_{Dw,rpng} & = \begin{bmatrix} {}^w\hat{w}_1\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_1 & {}^w\hat{w}_2\mathbf{e}_2 & {}^w\hat{w}_3 \mathbf{I}_3 \end{bmatrix} \end{align*}

static Eigen::MatrixXd ov_msckf::Propagator::compute_H_Da(std::shared_ptr<State> state, const Eigen::Vector3d& a_uncorrected)

compute the Jacobians for Da

Parameters
state Pointer to state
a_uncorrected Linear acceleration in gyro frame with bias removed

See IMU Reading Linearization for details.

\begin{align*} \mathbf{H}_{Da,kalibr} & = \begin{bmatrix} {}^a\hat{a}_1\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_1 & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_3 \mathbf{I}_3 \end{bmatrix} \\ \mathbf{H}_{Da,rpng} & = \begin{bmatrix} {}^a\hat{a}_1 \mathbf{I}_3 & & {}^a\hat{a}_2\mathbf{e}_2 & {}^a\hat{a}_2\mathbf{e}_3 & {}^a\hat{a}_3\mathbf{e}_3 \end{bmatrix} \end{align*}

static Eigen::MatrixXd ov_msckf::Propagator::compute_H_Tg(std::shared_ptr<State> state, const Eigen::Vector3d& a_inI)

compute the Jacobians for Tg

Parameters
state Pointer to state
a_inI Linear acceleration with bias removed

See IMU Reading Linearization for details.

\begin{align*} \mathbf{H}_{Tg} & = \begin{bmatrix} {}^I\hat{a}_1 \mathbf{I}_3 & {}^I\hat{a}_2 \mathbf{I}_3 & {}^I\hat{a}_3 \mathbf{I}_3 \end{bmatrix} \end{align*}

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 (in IMU clock)

void ov_msckf::Propagator::clean_old_imu_measurements(double oldest_time)

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)

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::MatrixXd& F, Eigen::MatrixXd& 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 Propagation page for details on how discrete model was derived. See the Analytical Propagation page for details on how analytic model was derived.

void ov_msckf::Propagator::predict_mean_discrete(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat, const Eigen::Vector3d& a_hat, 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_hat Angular velocity with bias removed
a_hat 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 Discrete-time IMU Propagation 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 wolfram page has the forth order equation defined below. We define function $ f(t,y) $ where y is a function of time t, see IMU Kinematic Equations for the definition of the continuous-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*}

void ov_msckf::Propagator::compute_Xi_sum(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat, const Eigen::Vector3d& a_hat, Eigen::Matrix<double, 3, 18>& Xi_sum) protected

Analytically compute the integration components based on ACI^2.

Parameters
state Pointer to state
dt Time we should integrate over
w_hat Angular velocity with bias removed
a_hat Linear acceleration with bias removed
Xi_sum All the needed integration components, including R_k, Xi_1, Xi_2, Jr, Xi_3, Xi_4 in order

See the Analytical State Mean Integration page and Integration Component Definitions for details. For computing Xi_1, Xi_2, Xi_3 and Xi_4 we have:

\begin{align*} \boldsymbol{\Xi}_1 & = \mathbf{I}_3 \delta t + \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}} \lfloor \hat{\mathbf{k}} \rfloor + \left(\delta t - \frac{\sin (\hat{\omega} \delta t)}{\hat{\omega}}\right) \lfloor \hat{\mathbf{k}} \rfloor^2 \\ \boldsymbol{\Xi}_2 & = \frac{1}{2} \delta t^2 \mathbf{I}_3 + \frac{\hat{\omega} \delta t - \sin (\hat{\omega} \delta t)}{\hat{\omega}^2}\lfloor \hat{\mathbf{k}} \rfloor + \left( \frac{1}{2} \delta t^2 - \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}^2} \right) \lfloor \hat{\mathbf{k}} \rfloor ^2 \\ \boldsymbol{\Xi}_3 &= \frac{1}{2}\delta t^2 \lfloor \hat{\mathbf{a}} \rfloor + \frac{\sin (\hat{\omega} \delta t_i) - \hat{\omega} \delta t }{\hat{\omega}^2} \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor + \frac{\sin (\hat{\omega} \delta t) - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) }{\hat{\omega}^2} \lfloor \hat{\mathbf{k}} \rfloor\lfloor\hat{\mathbf{a}} \rfloor + \left( \frac{1}{2} \delta t^2 - \frac{1 - \cos (\hat{\omega} \delta t)}{\hat{\omega}^2} \right) \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor ^2 + \left( \frac{1}{2} \delta t^2 + \frac{1 - \cos (\hat{\omega} \delta t) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t) }{\hat{\omega}^2} \right) \lfloor \hat{\mathbf{k}} \rfloor ^2 \lfloor\hat{\mathbf{a}} \rfloor + \left( \frac{1}{2} \delta t^2 + \frac{1 - \cos (\hat{\omega} \delta t) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t) }{\hat{\omega}^2} \right) \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor - \frac{ 3 \sin (\hat{\omega} \delta t) - 2 \hat{\omega} \delta t - \hat{\omega} \delta t \cos (\hat{\omega} \delta t) }{\hat{\omega}^2} \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor ^2 \\ \boldsymbol{\Xi}_4 & = \frac{1}{6}\delta t^3 \lfloor\hat{\mathbf{a}} \rfloor + \frac{2(1 - \cos (\hat{\omega} \delta t)) - (\hat{\omega}^2 \delta t^2)}{2 \hat{\omega}^3} \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor + \left( \frac{2(1- \cos(\hat{\omega} \delta t)) - \hat{\omega} \delta t \sin (\hat{\omega} \delta t)}{\hat{\omega}^3} \right) \lfloor \hat{\mathbf{k}} \rfloor\lfloor\hat{\mathbf{a}} \rfloor + \left( \frac{\sin (\hat{\omega} \delta t) - \hat{\omega} \delta t}{\hat{\omega}^3} + \frac{\delta t^3}{6} \right) \lfloor\hat{\mathbf{a}} \rfloor \lfloor \hat{\mathbf{k}} \rfloor^2 + \frac{\hat{\omega} \delta t - 2 \sin(\hat{\omega} \delta t) + \frac{1}{6}(\hat{\omega} \delta t)^3 + \hat{\omega} \delta t \cos(\hat{\omega} \delta t)}{\hat{\omega}^3} \lfloor \hat{\mathbf{k}} \rfloor^2\lfloor\hat{\mathbf{a}} \rfloor + \frac{\hat{\omega} \delta t - 2 \sin(\hat{\omega} \delta t) + \frac{1}{6}(\hat{\omega} \delta t)^3 + \hat{\omega} \delta t \cos(\hat{\omega} \delta t)}{\hat{\omega}^3} \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor + \frac{4 \cos(\hat{\omega} \delta t) - 4 + (\hat{\omega} \delta t)^2 + \hat{\omega} \delta t \sin(\hat{\omega} \delta t) } {\hat{\omega}^3} \hat{\mathbf{k}}^{\top} \hat{\mathbf{a}} \lfloor \hat{\mathbf{k}} \rfloor^2 \end{align*}

void ov_msckf::Propagator::predict_mean_analytic(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat, const Eigen::Vector3d& a_hat, Eigen::Vector4d& new_q, Eigen::Vector3d& new_v, Eigen::Vector3d& new_p, Eigen::Matrix<double, 3, 18>& Xi_sum) protected

Analytically predict IMU mean based on ACI^2.

Parameters
state Pointer to state
dt Time we should integrate over
w_hat Angular velocity with bias removed
a_hat 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
Xi_sum All the needed integration components, including R_k, Xi_1, Xi_2, Jr, Xi_3, Xi_4

See the Analytical State Mean Integration page for details.

\begin{align*} {}^{I_{k+1}}_G\hat{\mathbf{R}} & \simeq \Delta \mathbf{R}_k {}^{I_k}_G\hat{\mathbf{R}} \\ {}^G\hat{\mathbf{p}}_{I_{k+1}} & \simeq {}^{G}\hat{\mathbf{p}}_{I_k} + {}^G\hat{\mathbf{v}}_{I_k}\delta t_k + {}^{I_k}_G\hat{\mathbf{R}}^\top \Delta \hat{\mathbf{p}}_k - \frac{1}{2}{}^G\mathbf{g}\delta t^2_k \\ {}^G\hat{\mathbf{v}}_{I_{k+1}} & \simeq {}^{G}\hat{\mathbf{v}}_{I_k} + {}^{I_k}_G\hat{\mathbf{R}}^\top + \Delta \hat{\mathbf{v}}_k - {}^G\mathbf{g}\delta t_k \end{align*}

void ov_msckf::Propagator::compute_F_and_G_analytic(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat, const Eigen::Vector3d& a_hat, const Eigen::Vector3d& w_uncorrected, const Eigen::Vector3d& a_uncorrected, const Eigen::Vector4d& new_q, const Eigen::Vector3d& new_v, const Eigen::Vector3d& new_p, const Eigen::Matrix<double, 3, 18>& Xi_sum, Eigen::MatrixXd& F, Eigen::MatrixXd& G) protected

Analytically compute state transition matrix F and noise Jacobian G based on ACI^2.

Parameters
state Pointer to state
dt Time we should integrate over
w_hat Angular velocity with bias removed
a_hat Linear acceleration with bias removed
w_uncorrected Angular velocity in acc frame with bias and gravity sensitivity removed
a_uncorrected
new_q The resulting new orientation after integration
new_v The resulting new velocity after integration
new_p The resulting new position after integration
Xi_sum All the needed integration components, including R_k, Xi_1, Xi_2, Jr, Xi_3, Xi_4
F State transition matrix
G Noise Jacobian

This function is for analytical integration of the linearized error-state. This contains our state transition matrix and noise Jacobians. If you have other state variables besides the IMU that evolve you would add them here. See the Model Linearization Derivations page for details on how this was derived.

void ov_msckf::Propagator::compute_F_and_G_discrete(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat, const Eigen::Vector3d& a_hat, const Eigen::Vector3d& w_uncorrected, const Eigen::Vector3d& a_uncorrected, const Eigen::Vector4d& new_q, const Eigen::Vector3d& new_v, const Eigen::Vector3d& new_p, Eigen::MatrixXd& F, Eigen::MatrixXd& G) protected

compute state transition matrix F and noise Jacobian G

Parameters
state Pointer to state
dt Time we should integrate over
w_hat Angular velocity with bias removed
a_hat Linear acceleration with bias removed
w_uncorrected Angular velocity in acc frame with bias and gravity sensitivity removed
a_uncorrected
new_q The resulting new orientation after integration
new_v The resulting new velocity after integration
new_p The resulting new position after integration
F State transition matrix
G Noise Jacobian

This function is for analytical integration or when using a different state representation. This contains our state transition matrix and noise Jacobians. 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.