Helper which manipulates the State and its covariance. More...

#include <StateHelper.h>

Static Public Member Functions
static void	augment_clone (std::shared_ptr< State > state, Eigen::Matrix< double, 3, 1 > last_w)
	Augment the state with a stochastic copy of the current IMU pose. More...

static std::shared_ptr< ov_type::Type >	clone (std::shared_ptr< State > state, std::shared_ptr< ov_type::Type > variable_to_clone)
	Clones "variable to clone" and places it at end of covariance. More...

static void	EKFPropagation (std::shared_ptr< State > state, const std::vector< std::shared_ptr< ov_type::Type >> &order_NEW, const std::vector< std::shared_ptr< ov_type::Type >> &order_OLD, const Eigen::MatrixXd &Phi, const Eigen::MatrixXd &Q)
	Performs EKF propagation of the state covariance. More...

static void	EKFUpdate (std::shared_ptr< State > state, const std::vector< std::shared_ptr< ov_type::Type >> &H_order, const Eigen::MatrixXd &H, const Eigen::VectorXd &res, const Eigen::MatrixXd &R)
	Performs EKF update of the state (see Linear Measurement Update page) More...

static Eigen::MatrixXd	get_full_covariance (std::shared_ptr< State > state)
	This gets the full covariance matrix. More...

static Eigen::MatrixXd	get_marginal_covariance (std::shared_ptr< State > state, const std::vector< std::shared_ptr< ov_type::Type >> &small_variables)
	For a given set of variables, this will this will calculate a smaller covariance. More...

static bool	initialize (std::shared_ptr< State > state, std::shared_ptr< ov_type::Type > new_variable, const std::vector< std::shared_ptr< ov_type::Type >> &H_order, Eigen::MatrixXd &H_R, Eigen::MatrixXd &H_L, Eigen::MatrixXd &R, Eigen::VectorXd &res, double chi_2_mult)
	Initializes new variable into covariance. More...

static void	initialize_invertible (std::shared_ptr< State > state, std::shared_ptr< ov_type::Type > new_variable, const std::vector< std::shared_ptr< ov_type::Type >> &H_order, const Eigen::MatrixXd &H_R, const Eigen::MatrixXd &H_L, const Eigen::MatrixXd &R, const Eigen::VectorXd &res)
	Initializes new variable into covariance (H_L must be invertible) More...

static void	marginalize (std::shared_ptr< State > state, std::shared_ptr< ov_type::Type > marg)
	Marginalizes a variable, properly modifying the ordering/covariances in the state. More...

static void	marginalize_old_clone (std::shared_ptr< State > state)
	Remove the oldest clone, if we have more then the max clone count!! More...

static void	marginalize_slam (std::shared_ptr< State > state)
	Marginalize bad SLAM features. More...

static void	set_initial_covariance (std::shared_ptr< State > state, const Eigen::MatrixXd &covariance, const std::vector< std::shared_ptr< ov_type::Type >> &order)
	This will set the initial covaraince of the specified state elements. Will also ensure that proper cross-covariances are inserted. More...

Private Member Functions
	StateHelper ()

Detailed Description

Helper which manipulates the State and its covariance.

In general, this class has all the core logic for an Extended Kalman Filter (EKF)-based system. This has all functions that change the covariance along with addition and removing elements from the state. All functions here are static, and thus are self-contained so that in the future multiple states could be tracked and updated. We recommend you look directly at the code for this class for clarity on what exactly we are doing in each and the matching documentation pages.

Definition at line 45 of file StateHelper.h.

Constructor & Destructor Documentation

◆ StateHelper()

ov_msckf::StateHelper::StateHelper ( )

inlineprivate

All function in this class should be static. Thus an instance of this class cannot be created.

Definition at line 237 of file StateHelper.h.

Member Function Documentation

◆ augment_clone()

void StateHelper::augment_clone	(	std::shared_ptr< State >	state,
		Eigen::Matrix< double, 3, 1 >	last_w
	)

static

Augment the state with a stochastic copy of the current IMU pose.

After propagation, normally we augment the state with an new clone that is at the new update timestep. This augmentation clones the IMU pose and adds it to our state's clone map. If we are doing time offset calibration we also make our cloning a function of the time offset. Time offset logic is based on Li and Mourikis [Li2014IJRR].

We can write the current clone at the true imu base clock time as the follow:

$\begin{align*} {}^{I_{t+t_d}}_G\bar{q} &= \begin{bmatrix}\frac{1}{2} {}^{I_{t+\hat{t}_d}}\boldsymbol\omega \tilde{t}_d \\ 1\end{bmatrix}\otimes{}^{I_{t+\hat{t}_d}}_G\bar{q} \\ {}^G\mathbf{p}_{I_{t+t_d}} &= {}^G\mathbf{p}_{I_{t+\hat{t}_d}} + {}^G\mathbf{v}_{I_{t+\hat{t}_d}}\tilde{t}_d \end{align*}$

where we say that we have propagated our state up to the current estimated true imaging time for the current image, ${}^{I_{t+\hat{t}_d}}\boldsymbol\omega$ is the angular velocity at the end of propagation with biases removed. This is off by some smaller error, so to get to the true imaging time in the imu base clock, we can append some small timeoffset error. Thus the Jacobian in respect to our time offset during our cloning procedure is the following:

$\begin{align*} \frac{\partial {}^{I_{t+t_d}}_G\tilde{\boldsymbol\theta}}{\partial \tilde{t}_d} &= {}^{I_{t+\hat{t}_d}}\boldsymbol\omega \\ \frac{\partial {}^G\tilde{\mathbf{p}}_{I_{t+t_d}}}{\partial \tilde{t}_d} &= {}^G\mathbf{v}_{I_{t+\hat{t}_d}} \end{align*}$

Parameters

state	Pointer to state
last_w	The estimated angular velocity at cloning time (used to estimate imu-cam time offset)

Definition at line 579 of file StateHelper.cpp.

◆ clone()

std::shared_ptr< Type > StateHelper::clone	(	std::shared_ptr< State >	state,
		std::shared_ptr< ov_type::Type >	variable_to_clone
	)

static

Clones "variable to clone" and places it at end of covariance.

Parameters

state	Pointer to state
variable_to_clone	Pointer to variable that will be cloned

Definition at line 341 of file StateHelper.cpp.

◆ EKFPropagation()

void StateHelper::EKFPropagation	(	std::shared_ptr< State >	state,
		const std::vector< std::shared_ptr< ov_type::Type >> &	order_NEW,
		const std::vector< std::shared_ptr< ov_type::Type >> &	order_OLD,
		const Eigen::MatrixXd &	Phi,
		const Eigen::MatrixXd &	Q
	)

static

Performs EKF propagation of the state covariance.

The mean of the state should already have been propagated, thus just moves the covariance forward in time. The new states that we are propagating the old covariance into, should be contiguous in memory. The user only needs to specify the sub-variables that this block is a function of.

$\tilde{\mathbf{x}}' = \begin{bmatrix} \boldsymbol\Phi_1 & \boldsymbol\Phi_2 & \boldsymbol\Phi_3 \end{bmatrix} \begin{bmatrix} \tilde{\mathbf{x}}_1 \\ \tilde{\mathbf{x}}_2 \\ \tilde{\mathbf{x}}_3 \end{bmatrix} + \mathbf{n}$

Parameters

state	Pointer to state
order_NEW	Contiguous variables that have evolved according to this state transition
order_OLD	Variable ordering used in the state transition
Phi	State transition matrix (size order_NEW by size order_OLD)
Q	Additive state propagation noise matrix (size order_NEW by size order_NEW)

Definition at line 36 of file StateHelper.cpp.

◆ EKFUpdate()

void StateHelper::EKFUpdate	(	std::shared_ptr< State >	state,
		const std::vector< std::shared_ptr< ov_type::Type >> &	H_order,
		const Eigen::MatrixXd &	H,
		const Eigen::VectorXd &	res,
		const Eigen::MatrixXd &	R
	)

static

Performs EKF update of the state (see Linear Measurement Update page)

Parameters

state	Pointer to state
H_order	Variable ordering used in the compressed Jacobian
H	Condensed Jacobian of updating measurement
res	Residual of updating measurement
R	Updating measurement covariance

Definition at line 116 of file StateHelper.cpp.

◆ get_full_covariance()

Eigen::MatrixXd StateHelper::get_full_covariance ( std::shared_ptr< State > state )

static

This gets the full covariance matrix.

Should only be used during simulation as operations on this covariance will be slow. This will return a copy, so this cannot be used to change the covariance by design. Please use the other interface functions in the StateHelper to progamatically change to covariance.

Parameters

state Pointer to state

Returns: Covariance of current state

Definition at line 256 of file StateHelper.cpp.

◆ get_marginal_covariance()

Eigen::MatrixXd StateHelper::get_marginal_covariance	(	std::shared_ptr< State >	state,
		const std::vector< std::shared_ptr< ov_type::Type >> &	small_variables
	)

static

For a given set of variables, this will this will calculate a smaller covariance.

That only includes the ones specified with all crossterms. Thus the size of the return will be the summed dimension of all the passed variables. Normal use for this is a chi-squared check before update (where you don't need the full covariance).

Parameters

state	Pointer to state
small_variables	Vector of variables whose marginal covariance is desired

Returns: Marginal covariance of the passed variables

Definition at line 226 of file StateHelper.cpp.

◆ initialize()

bool StateHelper::initialize	(	std::shared_ptr< State >	state,
		std::shared_ptr< ov_type::Type >	new_variable,
		const std::vector< std::shared_ptr< ov_type::Type >> &	H_order,
		Eigen::MatrixXd &	H_R,
		Eigen::MatrixXd &	H_L,
		Eigen::MatrixXd &	R,
		Eigen::VectorXd &	res,
		double	chi_2_mult
	)

static

Initializes new variable into covariance.

Uses Givens to separate into updating and initializing systems (therefore system must be fed as isotropic). If you are not isotropic first whiten your system (TODO: we should add a helper function to do this). If your H_L Jacobian is already directly invertable, the just call the initialize_invertible() instead of this function. Please refer to Delayed Feature Initialization page for detailed derivation.

Parameters

state	Pointer to state
new_variable	Pointer to variable to be initialized
H_order	Vector of pointers in order they are contained in the condensed state Jacobian
H_R	Jacobian of initializing measurements wrt variables in H_order
H_L	Jacobian of initializing measurements wrt new variable
R	Covariance of initializing measurements (isotropic)
res	Residual of initializing measurements
chi_2_mult	Value we should multiply the chi2 threshold by (larger means it will be accepted more measurements)

Definition at line 393 of file StateHelper.cpp.

◆ initialize_invertible()

void StateHelper::initialize_invertible	(	std::shared_ptr< State >	state,
		std::shared_ptr< ov_type::Type >	new_variable,
		const std::vector< std::shared_ptr< ov_type::Type >> &	H_order,
		const Eigen::MatrixXd &	H_R,
		const Eigen::MatrixXd &	H_L,
		const Eigen::MatrixXd &	R,
		const Eigen::VectorXd &	res
	)

static

Initializes new variable into covariance (H_L must be invertible)

Please refer to Delayed Feature Initialization page for detailed derivation. This is just the update assuming that H_L is invertable (and thus square) and isotropic noise.

Parameters

state	Pointer to state
new_variable	Pointer to variable to be initialized
H_order	Vector of pointers in order they are contained in the condensed state Jacobian
H_R	Jacobian of initializing measurements wrt variables in H_order
H_L	Jacobian of initializing measurements wrt new variable (needs to be invertible)
R	Covariance of initializing measurements
res	Residual of initializing measurements

Definition at line 484 of file StateHelper.cpp.

◆ marginalize()

void StateHelper::marginalize	(	std::shared_ptr< State >	state,
		std::shared_ptr< ov_type::Type >	marg
	)

static

Marginalizes a variable, properly modifying the ordering/covariances in the state.

This function can support any Type variable out of the box. Right now the marginalization of a sub-variable/type is not supported. For example if you wanted to just marginalize the orientation of a PoseJPL, that isn't supported. We will first remove the rows and columns corresponding to the type (i.e. do the marginalization). After we update all the type ids so that they take into account that the covariance has shrunk in parts of it.

Parameters

state	Pointer to state
marg	Pointer to variable to marginalize

Definition at line 271 of file StateHelper.cpp.

◆ marginalize_old_clone()

void StateHelper::marginalize_old_clone ( std::shared_ptr< State > state )

static

Remove the oldest clone, if we have more then the max clone count!!

This will marginalize the clone from our covariance, and remove it from our state. This is mainly a helper function that we can call after each update. It will marginalize the clone specified by State::margtimestep() which should return a clone timestamp.

Parameters

state Pointer to state

Definition at line 618 of file StateHelper.cpp.

◆ marginalize_slam()

void StateHelper::marginalize_slam ( std::shared_ptr< State > state )

static

Marginalize bad SLAM features.

Parameters

state Pointer to state

Definition at line 631 of file StateHelper.cpp.

◆ set_initial_covariance()

void StateHelper::set_initial_covariance	(	std::shared_ptr< State >	state,
		const Eigen::MatrixXd &	covariance,
		const std::vector< std::shared_ptr< ov_type::Type >> &	order
	)

static

This will set the initial covaraince of the specified state elements. Will also ensure that proper cross-covariances are inserted.

Parameters

state	Pointer to state
covariance	The covariance of the system state
order	Order of the covariance matrix

Definition at line 199 of file StateHelper.cpp.

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

Static Public Member Functions

Private Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ StateHelper()

Member Function Documentation

◆ augment_clone()

◆ clone()

◆ EKFPropagation()

◆ EKFUpdate()

◆ get_full_covariance()

◆ get_marginal_covariance()

◆ initialize()

◆ initialize_invertible()

◆ marginalize()

◆ marginalize_old_clone()

◆ marginalize_slam()

◆ set_initial_covariance()