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

Collaboration diagram for mars::CoreStateType:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	CoreStateType ()=default

std::string	to_csv_string (const double &timestamp) const
	to_csv_string export state to single csv string

Static Public Member Functions
static CoreStateType	ApplyCorrection (CoreStateType state_prior, Eigen::Matrix< double, CoreStateType::size_error_, 1 > correction)
	ApplyCorrection.

static std::string	get_csv_state_header_string ()

Public Attributes
Eigen::Vector3d	p_wi_ { Eigen::Vector3d::Zero() }

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

Eigen::Quaternion< double >	q_wi_ { Eigen::Quaternion<double>::Identity() }

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

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

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

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

Static Public Attributes
static constexpr int	size_true_ = 16

static constexpr int	size_error_ = 15

Friends
std::ostream &	operator<< (std::ostream &out, const CoreStateType &data)

Constructor & Destructor Documentation

◆ CoreStateType()

EIGEN_MAKE_ALIGNED_OPERATOR_NEW mars::CoreStateType::CoreStateType ( )

default

Member Function Documentation

◆ ApplyCorrection()

static CoreStateType mars::CoreStateType::ApplyCorrection	(	CoreStateType	state_prior,
		Eigen::Matrix< double, CoreStateType::size_error_, 1 >	correction
	)

inlinestatic

ApplyCorrection.

Parameters

state_prior
correction	order [p_wi(0:2), v_wi(3:5), q_wi(6:8), b_w(9:11), b_a(12:14)]

Returns: Corrected state

  {
    // APPLY_CORRECTION Applies the given correction to the provided state_prior
    // state + error state correction
    // with quaternion from small-angle approx -> new state
 
    CoreStateType corrected_state;
 
    corrected_state.p_wi_ = state_prior.p_wi_ + correction.block(0, 0, 3, 1);
    corrected_state.v_wi_ = state_prior.v_wi_ + correction.block(3, 0, 3, 1);
 
    // Attention: due to small-angle to quaternion conversion,
    // the index for the corrected state does not map 1:1 (7:9 to 7:10)
    // this is important for the mapping idx after the quaternion.
    corrected_state.q_wi_ = Utils::ApplySmallAngleQuatCorr(state_prior.q_wi_, correction.block(6, 0, 3, 1));
 
    //      %if obj.fixed_bias
    //      %    correction(10:12) = zeros(3,1);
    //      %    correction(13:15) = zeros(3,1);
    //      %end
 
    corrected_state.b_w_ = state_prior.b_w_ + correction.block(9, 0, 3, 1);
    corrected_state.b_a_ = state_prior.b_a_ + correction.block(12, 0, 3, 1);
 
    // Pass through IMU measurements
    corrected_state.a_m_ = state_prior.a_m_;
    corrected_state.w_m_ = state_prior.w_m_;
 
    return corrected_state;
  }

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_csv_state_header_string()

static std::string mars::CoreStateType::get_csv_state_header_string ( )

inlinestatic

  {
    std::stringstream os;
    os << "t, ";
    os << "w_m_x, w_m_y, w_m_z, ";
    os << "a_m_x, a_m_y, a_m_z, ";
    os << "p_wi_x, p_wi_y, p_wi_z, ";
    os << "v_wi_x, v_wi_y, v_wi_z, ";
    os << "q_wi_w, q_wi_x, q_wi_y, q_wi_z, ";
    os << "b_w_x, b_w_y, b_w_z, ";
    os << "b_a_x, b_a_y, b_a_z";
 
    return os.str();
  }

◆ to_csv_string()

std::string mars::CoreStateType::to_csv_string ( const double & timestamp ) const

inline

to_csv_string export state to single csv string

Parameters

timestamp

Returns: string format [p_wi v_wi q_wi b_w b_a]

  {
    std::stringstream os;
    os.precision(17);
    os << timestamp;
 
    os << ", " << w_m_(0) << ", " << w_m_(1) << ", " << w_m_(2);
    os << ", " << a_m_(0) << ", " << a_m_(1) << ", " << a_m_(2);
    os << ", " << p_wi_(0) << ", " << p_wi_(1) << ", " << p_wi_(2);
    os << ", " << v_wi_(0) << ", " << v_wi_(1) << ", " << v_wi_(2);
 
    Eigen::Vector4d q_wi = q_wi_.coeffs();  // x y z w
    os << ", " << q_wi(3) << ", " << q_wi(0) << ", " << q_wi(1) << ", " << q_wi(2);
 
    os << ", " << b_w_(0) << ", " << b_w_(1) << ", " << b_w_(2);
    os << ", " << b_a_(0) << ", " << b_a_(1) << ", " << b_a_(2);
 
    return os.str();
  }

Friends And Related Symbol Documentation

◆ operator<<

std::ostream & operator<<	(	std::ostream &	out,
		const CoreStateType &	data
	)

friend

  {
    out.precision(10);
    out << std::fixed;
    out << "p_wi:\t[ " << data.p_wi_.transpose() << " ]" << std::endl
        << "v_wi:\t[ " << data.v_wi_.transpose() << " ]" << std::endl
        << "q_wi:\t[ " << data.q_wi_.w() << " " << data.q_wi_.vec().transpose() << " ]" << std::endl
        << "b_w:\t[ " << data.b_w_.transpose() << " ]" << std::endl
        << "b_a:\t[ " << data.b_a_.transpose() << " ]" << std::endl
        << "w_m:\t[ " << data.w_m_.transpose() << " ]" << std::endl
        << "a_m:\t[ " << data.a_m_.transpose() << " ]" << std::endl;
 
    return out;
  }

Member Data Documentation

◆ p_wi_

Eigen::Vector3d mars::CoreStateType::p_wi_ { Eigen::Vector3d::Zero() }

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

◆ v_wi_

Eigen::Vector3d mars::CoreStateType::v_wi_ { Eigen::Vector3d::Zero() }

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

◆ q_wi_

Eigen::Quaternion<double> mars::CoreStateType::q_wi_ { Eigen::Quaternion<double>::Identity() }

29{ Eigen::Quaternion<double>::Identity() };

◆ b_w_

Eigen::Vector3d mars::CoreStateType::b_w_ { Eigen::Vector3d::Zero() }

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

◆ b_a_

Eigen::Vector3d mars::CoreStateType::b_a_ { Eigen::Vector3d::Zero() }

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

◆ w_m_

Eigen::Vector3d mars::CoreStateType::w_m_ { Eigen::Vector3d::Zero() }

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

◆ a_m_

Eigen::Vector3d mars::CoreStateType::a_m_ { Eigen::Vector3d::Zero() }

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

◆ size_true_

constexpr int mars::CoreStateType::size_true_ = 16

staticconstexpr

◆ size_error_

constexpr int mars::CoreStateType::size_error_ = 15