mars::CoreState Class Reference

#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.
void	set_fixed_acc_bias (const bool &value)
	set_fixed_acc_bias disable or enable the estimation of the accelerometer bias
void	set_fixed_gyro_bias (const bool &value)
	set_fixed_gyro_bias disable or enable the estimation of the gyro bias
void	set_propagation_sensor (std::shared_ptr< SensorAbsClass > propagation_sensor)
	set_propagation_sensor Stores a reference to the propagation sensor
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
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.
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
CoreStateMatrix	InitializeCovariance ()
	InitializeCovariance Returnes the initialized core covariance.
CoreStateType	PropagateState (const CoreStateType &prior_state, const IMUMeasurementType &measurement, const double &dt)
	PropagateState Performs the state propagation.
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.

Static Public Member Functions
static CoreStateMatrix	GenerateFdTaylor ()
	GenerateFdTaylor Generates the state-transition matrix with cut-off Taylor series.
static CoreStateMatrix	GenerateFdClosedForm ()
	GenerateFdClosedForm Generates the state-transition matrix in closed-form.
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.
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.
Eigen::Vector3d	n_a_ { Eigen::Vector3d::Zero() }
	noise for linear acceleration measurement
Eigen::Vector3d	n_ba_ { Eigen::Vector3d::Zero() }
	random walk for linear acceleration bias
Eigen::Vector3d	n_w_ { Eigen::Vector3d::Zero() }
	noise for angular velocity measurement
Eigen::Vector3d	n_bw_ { Eigen::Vector3d::Zero() }
	random walk for angular velocity bias
const Eigen::Vector3d	g_ { 0, 0, 9.81 }
	defined gravity
CoreStateMatrix	initial_covariance_
std::shared_ptr< SensorAbsClass >	propagation_sensor_ { nullptr }
	Reference to the propagation sensor.
bool	test_state_transition_ { false }
	If true, the class performs tests on the state-transition properties.
bool	verbose_ { false }
	increased output of information

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

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

{ false };   

fixed_gyro_bias_

bool mars::CoreState::fixed_gyro_bias_ { false }

private

bias are not estimated if fixed_bias = true

{ false };  

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

{ Eigen::Vector3d::Zero() };   

n_ba_

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

random walk for linear acceleration bias

{ Eigen::Vector3d::Zero() };  

n_w_

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

noise for angular velocity measurement

{ Eigen::Vector3d::Zero() };   

n_bw_

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

random walk for angular velocity bias

{ Eigen::Vector3d::Zero() };  

g_

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

defined gravity

{ 0, 0, 9.81 };  

initial_covariance_

CoreStateMatrix mars::CoreState::initial_covariance_

propagation_sensor_

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

Reference to the propagation sensor.

{ nullptr };  

test_state_transition_

bool mars::CoreState::test_state_transition_ { false }

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

{ false };  

verbose_

bool mars::CoreState::verbose_ { false }

increased output of information

{ false };                

---

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

• core_state.h