#include </home/runner/work/mars_lib/mars_lib/source/mars/include/mars/core_state.h>

Collaboration diagram for mars::CoreState:

Public Member Functions
	CoreState ()
	CoreState Default constructor. More...

void	set_fixed_acc_bias (const bool &value)
	set_fixed_acc_bias disable or enable the estimation of the accelerometer bias More...

void	set_fixed_gyro_bias (const bool &value)
	set_fixed_gyro_bias disable or enable the estimation of the gyro bias More...

void	set_propagation_sensor (std::shared_ptr< SensorAbsClass > propagation_sensor)
	set_propagation_sensor Stores a reference to the propagation sensor More...

void	set_noise_std (const Eigen::Vector3d &n_w, const Eigen::Vector3d &n_bw, const Eigen::Vector3d &n_a, const Eigen::Vector3d &n_ba)
	set_noise_std Sets the noise and bias of the propagation sensor More...

CoreStateType	InitializeState (const Eigen::Vector3d &ang_vel, const Eigen::Vector3d &lin_acc, const Eigen::Vector3d &p_wi, const Eigen::Vector3d &v_wi, const Eigen::Quaterniond &q_wi, const Eigen::Vector3d &b_w, const Eigen::Vector3d &b_a)
	InitializeState Initializes the core state. More...

void	set_initial_covariance (const Eigen::Vector3d &p, const Eigen::Vector3d &v, const Eigen::Vector3d &q, const Eigen::Vector3d &bw, const Eigen::Vector3d &ba)
	set_initial_covariance used to set the initial covariance of the core states More...

CoreStateMatrix	InitializeCovariance ()
	InitializeCovariance Returnes the initialized core covariance. More...

CoreStateType	PropagateState (const CoreStateType &prior_state, const IMUMeasurementType &measurement, const double &dt)
	PropagateState Performs the state propagation. More...

CoreType	PredictProcessCovariance (const CoreType &prior_core_state, const IMUMeasurementType &system_input, const double &dt)
	PredictProcessCovariance Predicted core state covariance and generate the state transition matrix. More...

Static Public Member Functions
static CoreStateMatrix	GenerateFdTaylor ()
	GenerateFdTaylor Generates the state-transition matrix with cut-off Taylor series. More...

static CoreStateMatrix	GenerateFdClosedForm ()
	GenerateFdClosedForm Generates the state-transition matrix in closed-form. More...

static CoreStateMatrix	GenerateFdSmallAngleApprox (const Eigen::Quaterniond &q_wi, const Eigen::Vector3d &a_est, const Eigen::Vector3d &w_est, const double &dt)
	GenerateFdSmallAngleApprox Generates the state-transition matrix with small angle approximation. More...

static CoreStateMatrix	CalcQSmallAngleApprox (const double &dt, const Eigen::Quaterniond &q_wi, const Eigen::Vector3d &a_m, const Eigen::Vector3d &n_a, const Eigen::Vector3d &b_a, const Eigen::Vector3d &n_ba, const Eigen::Vector3d &w_m, const Eigen::Vector3d &n_w, const Eigen::Vector3d &b_w, const Eigen::Vector3d &n_bw)

Public Attributes
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CoreStateType	state
	Core State elements. More...

Eigen::Vector3d	n_a_ { Eigen::Vector3d::Zero() }
	noise for linear acceleration measurement More...

Eigen::Vector3d	n_ba_ { Eigen::Vector3d::Zero() }
	random walk for linear acceleration bias More...

Eigen::Vector3d	n_w_ { Eigen::Vector3d::Zero() }
	noise for angular velocity measurement More...

Eigen::Vector3d	n_bw_ { Eigen::Vector3d::Zero() }
	random walk for angular velocity bias More...

const Eigen::Vector3d	g_ { 0, 0, 9.81 }
	defined gravity More...

CoreStateMatrix	initial_covariance_

std::shared_ptr< SensorAbsClass >	propagation_sensor_ { nullptr }
	Reference to the propagation sensor. More...

bool	test_state_transition_ { false }
	If true, the class performs tests on the state-transition properties. More...

bool	verbose_ { false }
	increased output of information More...

Private Attributes
bool	fixed_acc_bias_ { false }
	bias are not estimated if fixed_bias = true More...

bool	fixed_gyro_bias_ { false }
	bias are not estimated if fixed_bias = true More...

Constructor & Destructor Documentation

◆ CoreState()

mars::CoreState::CoreState ( )

CoreState Default constructor.

Member Function Documentation

◆ set_fixed_acc_bias()

void mars::CoreState::set_fixed_acc_bias ( const bool & value )

set_fixed_acc_bias disable or enable the estimation of the accelerometer bias

Parameters

value

◆ set_fixed_gyro_bias()

void mars::CoreState::set_fixed_gyro_bias ( const bool & value )

set_fixed_gyro_bias disable or enable the estimation of the gyro bias

Parameters

value

◆ set_propagation_sensor()

void mars::CoreState::set_propagation_sensor ( std::shared_ptr< SensorAbsClass > propagation_sensor )

set_propagation_sensor Stores a reference to the propagation sensor

Parameters

propagation_sensor

◆ set_noise_std()

void mars::CoreState::set_noise_std	(	const Eigen::Vector3d &	n_w,
		const Eigen::Vector3d &	n_bw,
		const Eigen::Vector3d &	n_a,
		const Eigen::Vector3d &	n_ba
	)

set_noise_std Sets the noise and bias of the propagation sensor

Parameters

n_w
n_bw
n_a
n_ba

◆ InitializeState()

CoreStateType mars::CoreState::InitializeState	(	const Eigen::Vector3d &	ang_vel,
		const Eigen::Vector3d &	lin_acc,
		const Eigen::Vector3d &	p_wi,
		const Eigen::Vector3d &	v_wi,
		const Eigen::Quaterniond &	q_wi,
		const Eigen::Vector3d &	b_w,
		const Eigen::Vector3d &	b_a
	)

InitializeState Initializes the core state.

Parameters

ang_vel
lin_acc
p_wi
v_wi
q_wi
b_w
b_a

Returns

◆ set_initial_covariance()

void mars::CoreState::set_initial_covariance	(	const Eigen::Vector3d &	p,
		const Eigen::Vector3d &	v,
		const Eigen::Vector3d &	q,
		const Eigen::Vector3d &	bw,
		const Eigen::Vector3d &	ba
	)

set_initial_covariance used to set the initial covariance of the core states

Returns

◆ InitializeCovariance()

CoreStateMatrix mars::CoreState::InitializeCovariance ( )

InitializeCovariance Returnes the initialized core covariance.

Returns

◆ PropagateState()

CoreStateType mars::CoreState::PropagateState	(	const CoreStateType &	prior_state,
		const IMUMeasurementType &	measurement,
		const double &	dt
	)

PropagateState Performs the state propagation.

Parameters

prior_state	Prior state for the propagation
measurement	System input
dt	propagation timespan

Returns: Propagated core state

Note: Previous state is x_t-1 , Current state is x_t ew and ea are the estimates of the inputs because the bias are removed and the error state definition; first order Quaternion integration Sola, J. (2017). Quaternion kinematics for the error-state Kalman filter. arXiv preprint arXiv:1711.02508.

◆ PredictProcessCovariance()

CoreType mars::CoreState::PredictProcessCovariance	(	const CoreType &	prior_core_state,
		const IMUMeasurementType &	system_input,
		const double &	dt
	)

PredictProcessCovariance Predicted core state covariance and generate the state transition matrix.

Parameters

prior_core_state
system_input	Measurement for the system input
dt	propagation timespan

Returns: Core state covariance and state transition matrix

Note: The Covariance is generated for the error state definition.

◆ GenerateFdTaylor()

static CoreStateMatrix mars::CoreState::GenerateFdTaylor ( )

static

GenerateFdTaylor Generates the state-transition matrix with cut-off Taylor series.

Returns: state-transition matrix

◆ GenerateFdClosedForm()

static CoreStateMatrix mars::CoreState::GenerateFdClosedForm ( )

static

GenerateFdClosedForm Generates the state-transition matrix in closed-form.

Note: Solar - Quaternion Kinematics Section B.3

Returns: state-transition matrix

◆ GenerateFdSmallAngleApprox()

static CoreStateMatrix mars::CoreState::GenerateFdSmallAngleApprox	(	const Eigen::Quaterniond &	q_wi,
		const Eigen::Vector3d &	a_est,
		const Eigen::Vector3d &	w_est,
		const double &	dt
	)

static

GenerateFdSmallAngleApprox Generates the state-transition matrix with small angle approximation.

Parameters

q_wi	orientation of imu in world
a_est	the estimate of the acceleration (a_m - b_a)
w_est	the estimate of the angular velocity (w_m - b_w)
dt	time step

Note: Eq.(3.35) in S. Weiss, "Vision Based Navigation for Micro Helicopters (PhD Thesis - Weiss 2012)," Thesis.

Returns: state-transition matrix

◆ CalcQSmallAngleApprox()

static CoreStateMatrix mars::CoreState::CalcQSmallAngleApprox	(	const double &	dt,
		const Eigen::Quaterniond &	q_wi,
		const Eigen::Vector3d &	a_m,
		const Eigen::Vector3d &	n_a,
		const Eigen::Vector3d &	b_a,
		const Eigen::Vector3d &	n_ba,
		const Eigen::Vector3d &	w_m,
		const Eigen::Vector3d &	n_w,
		const Eigen::Vector3d &	b_w,
		const Eigen::Vector3d &	n_bw
	)

static

Member Data Documentation

◆ fixed_acc_bias_

bool mars::CoreState::fixed_acc_bias_ { false }

private

bias are not estimated if fixed_bias = true

◆ fixed_gyro_bias_

bool mars::CoreState::fixed_gyro_bias_ { false }

private

bias are not estimated if fixed_bias = true

◆ state

EIGEN_MAKE_ALIGNED_OPERATOR_NEW CoreStateType mars::CoreState::state

Core State elements.

◆ n_a_

Eigen::Vector3d mars::CoreState::n_a_ { Eigen::Vector3d::Zero() }

noise for linear acceleration measurement

◆ n_ba_

Eigen::Vector3d mars::CoreState::n_ba_ { Eigen::Vector3d::Zero() }

random walk for linear acceleration bias

◆ n_w_

Eigen::Vector3d mars::CoreState::n_w_ { Eigen::Vector3d::Zero() }

noise for angular velocity measurement

◆ n_bw_

Eigen::Vector3d mars::CoreState::n_bw_ { Eigen::Vector3d::Zero() }

random walk for angular velocity bias

◆ g_

const Eigen::Vector3d mars::CoreState::g_ { 0, 0, 9.81 }

defined gravity

◆ initial_covariance_

CoreStateMatrix mars::CoreState::initial_covariance_

◆ propagation_sensor_

std::shared_ptr<SensorAbsClass> mars::CoreState::propagation_sensor_ { nullptr }

Reference to the propagation sensor.

◆ test_state_transition_

bool mars::CoreState::test_state_transition_ { false }

If true, the class performs tests on the state-transition properties.

◆ verbose_

bool mars::CoreState::verbose_ { false }

increased output of information

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

core_state.h

Public Member Functions

Static Public Member Functions

Public Attributes

Private Attributes

Constructor & Destructor Documentation

◆ CoreState()

Member Function Documentation

◆ set_fixed_acc_bias()

◆ set_fixed_gyro_bias()

◆ set_propagation_sensor()

◆ set_noise_std()

◆ InitializeState()

◆ set_initial_covariance()

◆ InitializeCovariance()

◆ PropagateState()

◆ PredictProcessCovariance()

◆ GenerateFdTaylor()

◆ GenerateFdClosedForm()

◆ GenerateFdSmallAngleApprox()

◆ CalcQSmallAngleApprox()

Member Data Documentation

◆ fixed_acc_bias_

◆ fixed_gyro_bias_

◆ state

◆ n_a_

◆ n_ba_

◆ n_w_

◆ n_bw_

◆ g_

◆ initial_covariance_

◆ propagation_sensor_

◆ test_state_transition_

◆ verbose_