#include </home/runner/work/mars_lib/mars_lib/source/mars/include/mars/sensors/attitude/attitude_sensor_class.h>

Inheritance diagram for mars::AttitudeSensorClass:

Collaboration diagram for mars::AttitudeSensorClass:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	AttitudeSensorClass (const std::string &name, std::shared_ptr< CoreState > core_states, AttitudeSensorType type=AttitudeSensorType::RPY_TYPE)

virtual	~AttitudeSensorClass ()=default

AttitudeSensorStateType	get_state (const std::shared_ptr< void > &sensor_data)

Eigen::MatrixXd	get_covariance (const std::shared_ptr< void > &sensor_data)
	get_covariance Resolves a void pointer to the covariance matrix of the corresponding sensor type Each sensor is responsible to cast its own data type

void	set_initial_calib (std::shared_ptr< void > calibration)
	set_initial_calib Sets the calibration of an individual sensor

BufferDataType	Initialize (const Time &timestamp, std::shared_ptr< void >, std::shared_ptr< CoreType > latest_core_data)
	Initialize the state of an individual sensor.

bool	CalcUpdate (const Time &timestamp, std::shared_ptr< void > measurement, const CoreStateType &prior_core_state, std::shared_ptr< void > latest_sensor_data, const Eigen::MatrixXd &prior_cov, BufferDataType *new_state_data)
	CalcUpdate Calculates the update for an individual sensor definition.

bool	CalcUpdateRP (const Time &, const std::shared_ptr< void > &measurement, const CoreStateType &prior_core_state, const std::shared_ptr< void > &latest_sensor_data, const Eigen::MatrixXd &prior_cov, BufferDataType *new_state_data)

bool	CalcUpdateRPY (const Time &, const std::shared_ptr< void > &measurement, const CoreStateType &prior_core_state, const std::shared_ptr< void > &latest_sensor_data, const Eigen::MatrixXd &prior_cov, BufferDataType *new_state_data)

AttitudeSensorStateType	ApplyCorrection (const AttitudeSensorStateType &prior_sensor_state, const Eigen::MatrixXd &correction)

Public Member Functions inherited from mars::SensorInterface
virtual EIGEN_MAKE_ALIGNED_OPERATOR_NEW	~SensorInterface ()=default

Private Attributes
AttitudeSensorType	attitude_type_ { AttitudeSensorType::RPY_TYPE }

Additional Inherited Members
Public Attributes inherited from mars::UpdateSensorAbsClass
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int	aux_states_

int	aux_error_states_

int	ref_to_nav_

Eigen::MatrixXd	residual_

Eigen::VectorXd	R_
	Measurement noise "squared".

Eigen::MatrixXd	F_

Eigen::MatrixXd	H_

Eigen::MatrixXd	Q_

std::shared_ptr< void >	initial_calib_ { nullptr }

bool	initial_calib_provided_ { false }
	True if an initial calibration was provided.

Chi2	chi2_

std::shared_ptr< CoreState >	core_states_

Public Attributes inherited from mars::SensorAbsClass
int	id_ { -1 }

std::string	name_
	Name of the individual sensor instance.

bool	is_initialized_ { false }
	True if the sensor has been initialized.

bool	do_update_ { true }
	True if updates should be performed with the sensor.

int	type_ { -1 }
	Future feature, holds information such as position or orientation for highlevel decissions.

bool	const_ref_to_nav_ { true }
	True if the reference should not be estimated.

bool	ref_to_nav_given_ { false }
	True if the reference to the navigation frame is given by initial calibration.

bool	use_dynamic_meas_noise_ { false }
	True if dynamic noise values from measurements should be used.

Constructor & Destructor Documentation

◆ AttitudeSensorClass()

EIGEN_MAKE_ALIGNED_OPERATOR_NEW mars::AttitudeSensorClass::AttitudeSensorClass	(	const std::string &	name,
		std::shared_ptr< CoreState >	core_states,
		AttitudeSensorType	type = `AttitudeSensorType::RPY_TYPE`
	)

inline

  {
    name_ = name;
    core_states_ = std::move(core_states);
    const_ref_to_nav_ = false;
    initial_calib_provided_ = false;
 
    // set sensor type
    attitude_type_ = type;
 
    // chi2
    switch (attitude_type_)
    {
      case AttitudeSensorType::RP_TYPE:
        chi2_.set_dof(2);
        break;
      case AttitudeSensorType::RPY_TYPE:
        chi2_.set_dof(3);
        break;
      default:
        std::cout << "Warning: [" << this->name_
                  << "] Unexpected type for AttitudeSensorClass.\n"
                     "Assuming roll-pitch-yaw measurement."
                  << std::endl;
        attitude_type_ = AttitudeSensorType::RPY_TYPE;
        chi2_.set_dof(3);
        break;
    }
 
    // Sensor specific information
    // setup_sensor_properties();
    std::cout << "Created: [" << this->name_ << "] Sensor (type: " << attitude_type_ << ")" << std::endl;
  }

Here is the call graph for this function:

◆ ~AttitudeSensorClass()

virtual mars::AttitudeSensorClass::~AttitudeSensorClass ( )

virtualdefault

Member Function Documentation

◆ get_state()

AttitudeSensorStateType mars::AttitudeSensorClass::get_state ( const std::shared_ptr< void > & sensor_data )

inline

  {
    AttitudeSensorData data = *static_cast<AttitudeSensorData*>(sensor_data.get());
    return data.state_;
  }

◆ get_covariance()

Eigen::MatrixXd mars::AttitudeSensorClass::get_covariance ( const std::shared_ptr< void > & sensor_data )

inlinevirtual

get_covariance Resolves a void pointer to the covariance matrix of the corresponding sensor type Each sensor is responsible to cast its own data type

Parameters

sensor_data

Returns: Covariance matrix contained in the sensor data struct

Implements mars::SensorInterface.

  {
    AttitudeSensorData data = *static_cast<AttitudeSensorData*>(sensor_data.get());
    return data.get_full_cov();
  }

Here is the call graph for this function:

◆ set_initial_calib()

void mars::AttitudeSensorClass::set_initial_calib ( std::shared_ptr< void > calibration )

inlinevirtual

set_initial_calib Sets the calibration of an individual sensor

Parameters

calibration

Implements mars::SensorInterface.

  {
    initial_calib_ = calibration;
    initial_calib_provided_ = true;
  }

◆ Initialize()

BufferDataType mars::AttitudeSensorClass::Initialize	(	const Time &	timestamp,
		std::shared_ptr< void >	measurement,
		std::shared_ptr< CoreType >	latest_core_data
	)

inlinevirtual

Initialize the state of an individual sensor.

Parameters

timestamp	current timestamp
measurement	current sensor measurement
latest_core_data

Returns

Implements mars::SensorInterface.

  {
    // AttitudeMeasurementType measurement = *static_cast<AttitudeMeasurementType*>(sensor_data.get());
 
    AttitudeSensorData sensor_state;
    std::string calibration_type;
 
    if (this->initial_calib_provided_)
    {
      calibration_type = "Given";
 
      AttitudeSensorData calib = *static_cast<AttitudeSensorData*>(initial_calib_.get());
 
      sensor_state.state_ = calib.state_;
      sensor_state.sensor_cov_ = calib.sensor_cov_;
    }
    else
    {
      //      calibration_type = "Auto";
 
      //      Eigen::Vector3d p_wp(measurement.position_);
      //      Eigen::Quaterniond q_wp(measurement.orientation_);
 
      //      Eigen::Vector3d p_wi(latest_core_data->state_.p_wi_);
      //      Eigen::Quaterniond q_wi(latest_core_data->state_.q_wi_);
      //      Eigen::Matrix3d r_wi(q_wi.toRotationMatrix());
 
      //      Eigen::Vector3d p_ip = r_wi.transpose() * (p_wp - p_wi);
      //      Eigen::Quaterniond q_ip = q_wi.conjugate() * q_wp;
 
      //      // Calibration, position / rotation imu-pose
      //      sensor_state.state_.p_ip_ = p_ip;
      //      sensor_state.state_.q_ip_ = q_ip;
 
      //      // The covariance should enclose the initialization with a 3 Sigma bound
      //      Eigen::Matrix<double, 6, 1> std;
      //      std << 1, 1, 1, (35 * M_PI / 180), (35 * M_PI / 180), (35 * M_PI / 180);
      //      sensor_state.sensor_cov_ = std.cwiseProduct(std).asDiagonal();
    }
 
    // Bypass core state for the returned object
    BufferDataType result(std::make_shared<CoreType>(*latest_core_data.get()),
                          std::make_shared<AttitudeSensorData>(sensor_state));
 
    // TODO
    // sensor_data.ref_to_nav = 0; //obj.calc_ref_to_nav(measurement, latest_core_state);
 
    is_initialized_ = true;
 
    std::cout << "Info: Initialized [" << name_ << "] with [" << calibration_type << "] Calibration at t=" << timestamp
              << std::endl;
 
    if (!initial_calib_provided_)
    {
      std::cout << "Info: [" << name_ << "] Calibration(rounded):" << std::endl;
      std::cout << "\tOrientation[deg]: ["
                << sensor_state.state_.q_aw_.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * (180 / M_PI) << " ]"
                << std::endl;
    }
 
    return result;
  }

◆ CalcUpdate()

bool mars::AttitudeSensorClass::CalcUpdate	(	const Time &	timestamp,
		std::shared_ptr< void >	measurement,
		const CoreStateType &	prior_core_state_data,
		std::shared_ptr< void >	latest_sensor_data,
		const Eigen::MatrixXd &	prior_cov,
		BufferDataType *	new_state_data
	)

inlinevirtual

CalcUpdate Calculates the update for an individual sensor definition.

Parameters

timestamp	current timestamp
measurement	current sensor measurement
prior_core_state_data
latest_sensor_data
prior_cov	Prior covariance containing core, sensor and sensor cross covariance
new_state_data	Updated state data

Returns: True if the update was successful, false if the update was rejected

Implements mars::SensorInterface.

  {
    switch (attitude_type_)
    {
      case AttitudeSensorType::RP_TYPE:
        return CalcUpdateRP(timestamp, measurement, prior_core_state, latest_sensor_data, prior_cov, new_state_data);
      case AttitudeSensorType::RPY_TYPE:
        return CalcUpdateRPY(timestamp, measurement, prior_core_state, latest_sensor_data, prior_cov, new_state_data);
      default:
        std::cout << "Error: [" << this->name_ << "] Cannot perform update (unknown type)" << std::endl;
        return false;
    }
 
    return false;
  }

Here is the call graph for this function:

◆ CalcUpdateRP()

bool mars::AttitudeSensorClass::CalcUpdateRP	(	const Time &	,
		const std::shared_ptr< void > &	measurement,
		const CoreStateType &	prior_core_state,
		const std::shared_ptr< void > &	latest_sensor_data,
		const Eigen::MatrixXd &	prior_cov,
		BufferDataType *	new_state_data
	)

inline

  {
    // Cast the sensor measurement and prior state information
    AttitudeMeasurementType* meas = static_cast<AttitudeMeasurementType*>(measurement.get());
    AttitudeSensorData* prior_sensor_data = static_cast<AttitudeSensorData*>(latest_sensor_data.get());
 
    // Decompose sensor measurement
    Eigen::Vector2d rp_meas = meas->attitude_.get_rp();
 
    // Extract sensor state
    AttitudeSensorStateType prior_sensor_state(prior_sensor_data->state_);
 
    // Generate measurement noise matrix and check
    // if noisevalues from the measurement object should be used
    Eigen::MatrixXd R_meas_dyn;
    if (meas->has_meas_noise && use_dynamic_meas_noise_)
    {
      meas->get_meas_noise(&R_meas_dyn);
    }
    else
    {
      R_meas_dyn = this->R_.asDiagonal();
    }
    const Eigen::Matrix<double, 2, 2> R_meas = R_meas_dyn;
 
    const int size_of_core_state = CoreStateType::size_error_;
    const int size_of_sensor_state = prior_sensor_state.cov_size_;
    const int size_of_full_error_state = size_of_core_state + size_of_sensor_state;
    const Eigen::MatrixXd P = prior_cov;
    assert(P.size() == size_of_full_error_state * size_of_full_error_state);
 
    // Calculate the measurement jacobian H
    typedef Eigen::Matrix<double, 2, 3> Matrix23d_t;
    Matrix23d_t I_23;
    I_23 << 1., 0., 0., 0., 1., 0.;
    const Matrix23d_t Z_23 = Matrix23d_t::Zero();
    const Eigen::Matrix3d R_wi = prior_core_state.q_wi_.toRotationMatrix();
    const Eigen::Matrix3d R_aw = prior_sensor_state.q_aw_.toRotationMatrix();
    const Eigen::Matrix3d R_ib = prior_sensor_state.q_ib_.toRotationMatrix();
 
    // Orientation
    const Matrix23d_t Hr_pwi = Z_23;
    const Matrix23d_t Hr_vwi = Z_23;
    const Matrix23d_t Hr_rwi = R_ib.transpose().block(0, 0, 2, 3);
    const Matrix23d_t Hr_bw = Z_23;
    const Matrix23d_t Hr_ba = Z_23;
 
    const Matrix23d_t Hr_raw = (R_ib.transpose() * R_wi.transpose()).block(0, 0, 2, 3);
    const Matrix23d_t Hr_rib = I_23;
 
    // Assemble the jacobian for the orientation (horizontal)
    // H_r = [Hr_pwi Hr_vwi Hr_rwi Hr_bw Hr_ba Hr_mag Hr_rim];
    Eigen::MatrixXd H(2, Hr_pwi.cols() + Hr_vwi.cols() + Hr_rwi.cols() + Hr_bw.cols() + Hr_ba.cols() + Hr_raw.cols() +
                             Hr_rib.cols());
    H << Hr_pwi, Hr_vwi, Hr_rwi, Hr_bw, Hr_ba, Hr_raw, Hr_rib;
 
    // Calculate the residual z = z~ - (estimate)
    // Orientation
    const Eigen::Vector3d rpy_est = mars::Utils::RPYFromRotMat(R_aw * R_wi * R_ib);
    const Eigen::Vector2d rp_est(rpy_est(0), rpy_est(1));
    residual_ = Eigen::MatrixXd(rp_est.rows(), 1);
    residual_ = rp_meas - rp_est;
 
    // Perform EKF calculations
    mars::Ekf ekf(H, R_meas, residual_, P);
    const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_);
    assert(correction.size() == size_of_full_error_state * 1);
 
    // Perform Chi2 test
    if (!chi2_.passed_ && chi2_.do_test_)
    {
      chi2_.PrintReport(name_);
      return false;
    }
 
    Eigen::MatrixXd P_updated = ekf.CalculateCovUpdate();
    assert(P_updated.size() == size_of_full_error_state * size_of_full_error_state);
    P_updated = Utils::EnforceMatrixSymmetry(P_updated);
 
    // Apply Core Correction
    CoreStateVector core_correction = correction.block(0, 0, CoreStateType::size_error_, 1);
    CoreStateType corrected_core_state = CoreStateType::ApplyCorrection(prior_core_state, core_correction);
 
    // Apply Sensor Correction
    const Eigen::MatrixXd sensor_correction = correction.block(size_of_core_state, 0, size_of_sensor_state, 1);
    const AttitudeSensorStateType corrected_sensor_state = ApplyCorrection(prior_sensor_state, sensor_correction);
 
    // Return Results
    // CoreState data
    CoreType core_data;
    core_data.cov_ = P_updated.block(0, 0, CoreStateType::size_error_, CoreStateType::size_error_);
    core_data.state_ = corrected_core_state;
 
    // SensorState data
    std::shared_ptr<AttitudeSensorData> sensor_data(std::make_shared<AttitudeSensorData>());
    sensor_data->set_cov(P_updated);
    sensor_data->state_ = corrected_sensor_state;
 
    BufferDataType state_entry(std::make_shared<CoreType>(core_data), sensor_data);
 
    if (const_ref_to_nav_)
    {
      // corrected_sensor_data.ref_to_nav = prior_ref_to_nav;
    }
    else
    {
      // TODO also estimate ref to nav
    }
 
    *new_state_data = state_entry;
 
    return true;
  }

Here is the call graph for this function:

Here is the caller graph for this function:

◆ CalcUpdateRPY()

bool mars::AttitudeSensorClass::CalcUpdateRPY	(	const Time &	,
		const std::shared_ptr< void > &	measurement,
		const CoreStateType &	prior_core_state,
		const std::shared_ptr< void > &	latest_sensor_data,
		const Eigen::MatrixXd &	prior_cov,
		BufferDataType *	new_state_data
	)

inline

  {
    // Cast the sensor measurement and prior state information
    AttitudeMeasurementType* meas = static_cast<AttitudeMeasurementType*>(measurement.get());
    AttitudeSensorData* prior_sensor_data = static_cast<AttitudeSensorData*>(latest_sensor_data.get());
 
    // Decompose sensor measurement
    Eigen::Quaterniond q_meas = meas->attitude_.quaternion_;
 
    // Extract sensor state
    AttitudeSensorStateType prior_sensor_state(prior_sensor_data->state_);
 
    // Generate measurement noise matrix
    Eigen::MatrixXd R_meas_dyn;
    if (meas->has_meas_noise && use_dynamic_meas_noise_)
    {
      meas->get_meas_noise(&R_meas_dyn);
    }
    else
    {
      R_meas_dyn = this->R_.asDiagonal();
    }
    const Eigen::Matrix<double, 3, 3> R_meas = R_meas_dyn;
 
    const int size_of_core_state = CoreStateType::size_error_;
    const int size_of_sensor_state = prior_sensor_state.cov_size_;
    const int size_of_full_error_state = size_of_core_state + size_of_sensor_state;
    const Eigen::MatrixXd P = prior_cov;
    assert(P.size() == size_of_full_error_state * size_of_full_error_state);
 
    // Calculate the measurement jacobian H
    const Eigen::Matrix3d I_3 = Eigen::Matrix3d::Identity();
    const Eigen::Matrix3d Z_3 = Eigen::Matrix3d::Zero();
    const Eigen::Matrix3d R_wi = prior_core_state.q_wi_.toRotationMatrix();
    // const Eigen::Matrix3d R_aw = prior_sensor_state.q_aw_.toRotationMatrix();
    const Eigen::Matrix3d R_ib = prior_sensor_state.q_ib_.toRotationMatrix();
 
    // Orientation
    const Eigen::Matrix3d Hr_pwi = Z_3;
    const Eigen::Matrix3d Hr_vwi = Z_3;
    const Eigen::Matrix3d Hr_rwi = R_ib.transpose();
    const Eigen::Matrix3d Hr_bw = Z_3;
    const Eigen::Matrix3d Hr_ba = Z_3;
 
    const Eigen::Matrix3d Hr_raw = R_ib.transpose() * R_wi.transpose();
    const Eigen::Matrix3d Hr_rib = I_3;
 
    // Assemble the jacobian for the orientation (horizontal)
    // H_r = [Hr_pwi Hr_vwi Hr_rwi Hr_bw Hr_ba Hr_raw];
    Eigen::MatrixXd H(3, Hr_pwi.cols() + Hr_vwi.cols() + Hr_rwi.cols() + Hr_bw.cols() + Hr_ba.cols() + Hr_raw.cols() +
                             Hr_rib.cols());
    H << Hr_pwi, Hr_vwi, Hr_rwi, Hr_bw, Hr_ba, Hr_raw, Hr_rib;
 
    // Calculate the residual z = z~ - (estimate)
    // Orientation
    const Eigen::Quaternion<double> q_est =
        prior_sensor_state.q_aw_ * prior_core_state.q_wi_ * prior_sensor_state.q_ib_;
    const Eigen::Quaternion<double> res_q = q_est.inverse() * q_meas;
    residual_ = Eigen::MatrixXd(res_q.vec().rows(), 1);
    residual_ << (2 * res_q.vec() / res_q.w());
 
    // Perform EKF calculations
    mars::Ekf ekf(H, R_meas, residual_, P);
    const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_);
    assert(correction.size() == size_of_full_error_state * 1);
 
    // Perform Chi2 test
    if (!chi2_.passed_ && chi2_.do_test_)
    {
      chi2_.PrintReport(name_);
      return false;
    }
 
    Eigen::MatrixXd P_updated = ekf.CalculateCovUpdate();
    assert(P_updated.size() == size_of_full_error_state * size_of_full_error_state);
    P_updated = Utils::EnforceMatrixSymmetry(P_updated);
    // Apply Core Correction
    CoreStateVector core_correction = correction.block(0, 0, CoreStateType::size_error_, 1);
    CoreStateType corrected_core_state = CoreStateType::ApplyCorrection(prior_core_state, core_correction);
 
    // Apply Sensor Correction
    const Eigen::MatrixXd sensor_correction = correction.block(size_of_core_state, 0, size_of_sensor_state, 1);
    const AttitudeSensorStateType corrected_sensor_state = ApplyCorrection(prior_sensor_state, sensor_correction);
 
    // Return Results
    // CoreState data
    CoreType core_data;
    core_data.cov_ = P_updated.block(0, 0, CoreStateType::size_error_, CoreStateType::size_error_);
    core_data.state_ = corrected_core_state;
 
    // SensorState data
    std::shared_ptr<AttitudeSensorData> sensor_data(std::make_shared<AttitudeSensorData>());
    sensor_data->set_cov(P_updated);
    sensor_data->state_ = corrected_sensor_state;
 
    BufferDataType state_entry(std::make_shared<CoreType>(core_data), sensor_data);
 
    if (const_ref_to_nav_)
    {
      // corrected_sensor_data.ref_to_nav = prior_ref_to_nav;
    }
    else
    {
      // TODO also estimate ref to nav
    }
 
    *new_state_data = state_entry;
 
    return true;
  }

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ApplyCorrection()

AttitudeSensorStateType mars::AttitudeSensorClass::ApplyCorrection	(	const AttitudeSensorStateType &	prior_sensor_state,
		const Eigen::MatrixXd &	correction
	)

inline

  {
    // state + error state correction
    // with quaternion from small angle approx -> new state
 
    // q_aw   [0,1,2] 0:2
    // q_ib   [3,4,5] 3:5
 
    AttitudeSensorStateType corrected_sensor_state;
 
    // select correct correction block
    Eigen::Vector3d q_aw_correction, q_ib_correction;
    switch (attitude_type_)
    {
      case AttitudeSensorType::RP_TYPE:
        q_aw_correction << correction(0), correction(1), 0;
        q_ib_correction << correction(2), correction(3), 0;
        break;
      case AttitudeSensorType::RPY_TYPE:
        q_aw_correction = correction.block(0, 0, 3, 1);
        q_ib_correction = correction.block(3, 0, 3, 1);
        break;
      default:
        std::cout << "Error: [" << this->name_ << "] Cannot perform correction (unknown type)" << std::endl;
        q_aw_correction = Eigen::Vector3d::Zero();
        q_ib_correction = Eigen::Vector3d::Zero();
        break;
    }
 
    corrected_sensor_state.q_aw_ = Utils::ApplySmallAngleQuatCorr(prior_sensor_state.q_aw_, q_aw_correction);
    corrected_sensor_state.q_ib_ = Utils::ApplySmallAngleQuatCorr(prior_sensor_state.q_ib_, q_ib_correction);
 
    return corrected_sensor_state;
  }

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ attitude_type_

AttitudeSensorType mars::AttitudeSensorClass::attitude_type_ { AttitudeSensorType::RPY_TYPE }

private

63{ AttitudeSensorType::RPY_TYPE };

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

attitude_sensor_class.h

Public Member Functions

Private Attributes

Additional Inherited Members

Constructor & Destructor Documentation

◆ AttitudeSensorClass()

◆ ~AttitudeSensorClass()

Member Function Documentation

◆ get_state()

◆ get_covariance()

◆ set_initial_calib()

◆ Initialize()

◆ CalcUpdate()

◆ CalcUpdateRP()

◆ CalcUpdateRPY()

◆ ApplyCorrection()

Member Data Documentation

◆ attitude_type_