Performs the state covariance and mean propagation using imu measurements. More...

#include <Propagator.h>

Public Member Functions
void	clean_old_imu_measurements (double oldest_time)
	This will remove any IMU measurements that are older then the given measurement time. More...

bool	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. More...

void	feed_imu (const ov_core::ImuData &message, double oldest_time=-1)
	Stores incoming inertial readings. More...

void	invalidate_cache ()
	Will invalidate the cache used for fast propagation. More...

void	propagate_and_clone (std::shared_ptr< State > state, double timestamp)
	Propagate state up to given timestamp and then clone. More...

	Propagator (NoiseManager noises, double gravity_mag)
	Default constructor. More...

Static Public Member Functions
static Eigen::MatrixXd	compute_H_Da (std::shared_ptr< State > state, const Eigen::Vector3d &a_uncorrected)
	compute the Jacobians for Da More...

static Eigen::MatrixXd	compute_H_Dw (std::shared_ptr< State > state, const Eigen::Vector3d &w_uncorrected)
	compute the Jacobians for Dw More...

static Eigen::MatrixXd	compute_H_Tg (std::shared_ptr< State > state, const Eigen::Vector3d &a_inI)
	compute the Jacobians for Tg More...

static ov_core::ImuData	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. More...

static std::vector< ov_core::ImuData >	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. More...

Protected Member Functions
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. More...

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 More...

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. More...

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. More...

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. More...

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. More...

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. More...

Protected Attributes
Eigen::Vector3d	_gravity
	Gravity vector. More...

NoiseManager	_noises
	Container for the noise values. More...

std::atomic< bool >	cache_imu_valid

Eigen::MatrixXd	cache_state_covariance

Eigen::MatrixXd	cache_state_est

double	cache_state_time

double	cache_t_off

bool	have_last_prop_time_offset = false

std::vector< ov_core::ImuData >	imu_data
	Our history of IMU messages (time, angular, linear) More...

std::mutex	imu_data_mtx

double	last_prop_time_offset = 0.0

Detailed Description

Performs the state covariance and mean propagation using imu measurements.

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.

Definition at line 44 of file Propagator.h.

Constructor & Destructor Documentation

◆ Propagator()

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

inline

Default constructor.

Parameters

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

Definition at line 51 of file Propagator.h.

Member Function Documentation

◆ clean_old_imu_measurements()

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

inline

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)

Definition at line 80 of file Propagator.h.

◆ compute_F_and_G_analytic()

void 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.

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.

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
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

Definition at line 683 of file Propagator.cpp.

◆ compute_F_and_G_discrete()

void 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

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.

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
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

Definition at line 830 of file Propagator.cpp.

◆ compute_H_Da()

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

static

compute the Jacobians for Da

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*}$

Parameters

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

Definition at line 984 of file Propagator.cpp.

◆ compute_H_Dw()

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

static

compute the Jacobians for Dw

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*}$

Parameters

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

Definition at line 964 of file Propagator.cpp.

◆ compute_H_Tg()

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

static

compute the Jacobians for Tg

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*}$

Parameters

state	Pointer to state
a_inI	Linear acceleration with bias removed

Definition at line 1004 of file Propagator.cpp.

◆ compute_Xi_sum()

void 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.

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*}$

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

Definition at line 588 of file Propagator.cpp.

◆ fast_state_propagate()

bool 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.

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.

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

Definition at line 140 of file Propagator.cpp.

◆ feed_imu()

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

inline

Stores incoming inertial readings.

Parameters

message	Contains our timestamp and inertial information
oldest_time	Time that we can discard measurements before (in IMU clock)

Definition at line 65 of file Propagator.h.

◆ interpolate_data()

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

inlinestatic

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

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.

Parameters

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

Definition at line 154 of file Propagator.h.

◆ invalidate_cache()

void ov_msckf::Propagator::invalidate_cache ( )

inline

Will invalidate the cache used for fast propagation.

Definition at line 96 of file Propagator.h.

◆ predict_and_compute()

void 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.

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.

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

Definition at line 395 of file Propagator.cpp.

◆ predict_mean_analytic()

void 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.

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*}$

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

Definition at line 667 of file Propagator.cpp.

◆ predict_mean_discrete()

void 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.

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*}$

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

Definition at line 482 of file Propagator.cpp.

◆ predict_mean_rk4()

void 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.

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*}$

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

Definition at line 507 of file Propagator.cpp.

◆ propagate_and_clone()

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

Propagate state up to given timestamp and then clone.

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.

Parameters

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

Definition at line 33 of file Propagator.cpp.

◆ select_imu_readings()

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

static

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

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.

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)

Definition at line 269 of file Propagator.cpp.

Member Data Documentation

◆ _gravity

Eigen::Vector3d ov_msckf::Propagator::_gravity

protected

Gravity vector.

Definition at line 442 of file Propagator.h.

◆ _noises

NoiseManager ov_msckf::Propagator::_noises

protected

Container for the noise values.

Definition at line 435 of file Propagator.h.

◆ cache_imu_valid

std::atomic<bool> ov_msckf::Propagator::cache_imu_valid

protected

Definition at line 449 of file Propagator.h.

◆ cache_state_covariance

Eigen::MatrixXd ov_msckf::Propagator::cache_state_covariance

protected

Definition at line 452 of file Propagator.h.

◆ cache_state_est

Eigen::MatrixXd ov_msckf::Propagator::cache_state_est

protected

Definition at line 451 of file Propagator.h.

◆ cache_state_time

double ov_msckf::Propagator::cache_state_time

protected

Definition at line 450 of file Propagator.h.

◆ cache_t_off

double ov_msckf::Propagator::cache_t_off

protected

Definition at line 453 of file Propagator.h.

◆ have_last_prop_time_offset

bool ov_msckf::Propagator::have_last_prop_time_offset = false

protected

Definition at line 446 of file Propagator.h.

◆ imu_data

std::vector<ov_core::ImuData> ov_msckf::Propagator::imu_data

protected

Our history of IMU messages (time, angular, linear)

Definition at line 438 of file Propagator.h.

◆ imu_data_mtx

std::mutex ov_msckf::Propagator::imu_data_mtx

protected

Definition at line 439 of file Propagator.h.

◆ last_prop_time_offset

double ov_msckf::Propagator::last_prop_time_offset = 0.0

protected

Definition at line 445 of file Propagator.h.

The documentation for this class was generated from the following files:

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ Propagator()

Member Function Documentation

◆ clean_old_imu_measurements()

◆ compute_F_and_G_analytic()

◆ compute_F_and_G_discrete()

◆ compute_H_Da()

◆ compute_H_Dw()

◆ compute_H_Tg()

◆ compute_Xi_sum()

◆ fast_state_propagate()

◆ feed_imu()

◆ interpolate_data()

◆ invalidate_cache()

◆ predict_and_compute()

◆ predict_mean_analytic()

◆ predict_mean_discrete()

◆ predict_mean_rk4()

◆ propagate_and_clone()

◆ select_imu_readings()

Member Data Documentation

◆ _gravity

◆ _noises

◆ cache_imu_valid

◆ cache_state_covariance

◆ cache_state_est

◆ cache_state_time

◆ cache_t_off

◆ have_last_prop_time_offset

◆ imu_data

◆ imu_data_mtx

◆ last_prop_time_offset