mars::EmptySensorClass Class Reference

#include </home/runner/work/mars_lib/mars_lib/source/mars/include/mars/sensors/empty/empty_sensor_class.h>

Inheritance diagram for mars::EmptySensorClass:

Collaboration diagram for mars::EmptySensorClass:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	EmptySensorClass (const std::string &name, std::shared_ptr< CoreState > core_states)
virtual	~EmptySensorClass ()=default
EmptySensorStateType	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 More...
void	set_initial_calib (std::shared_ptr< void > calibration)
	set_initial_calib Sets the calibration of an individual sensor More...
BufferDataType	Initialize (const Time &timestamp, std::shared_ptr< void >, std::shared_ptr< CoreType > latest_core_data)
	Initialize the state of an individual sensor. More...
bool	CalcUpdate (const Time &, std::shared_ptr< void >, 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. More...
EmptySensorStateType	ApplyCorrection (const EmptySensorStateType &prior_sensor_state, const Eigen::MatrixXd &correction)
Public Member Functions inherited from mars::SensorInterface
virtual EIGEN_MAKE_ALIGNED_OPERATOR_NEW	~SensorInterface ()=default

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". More...
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. More...
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. More...
bool	is_initialized_ { false }
	True if the sensor has been initialized. More...
bool	do_update_ { true }
	True if updates should be performed with the sensor. More...
int	type_ { -1 }
	Future feature, holds information such as position or orientation for highlevel decissions. More...
bool	const_ref_to_nav_ { true }
	True if the reference should not be estimated. More...
bool	ref_to_nav_given_ { false }
	True if the reference to the navigation frame is given by initial calibration. More...
bool	use_dynamic_meas_noise_ { false }
	True if dynamic noise values from measurements should be used. More...

Constructor & Destructor Documentation

EmptySensorClass()

EIGEN_MAKE_ALIGNED_OPERATOR_NEW mars::EmptySensorClass::EmptySensorClass ( const std::string &  name, std::shared_ptr< CoreState >  core_states  )

inline

  {
    name_ = name;
    core_states_ = std::move(core_states);
    const_ref_to_nav_ = false;
    initial_calib_provided_ = false;
 
    std::cout << "Created: [" << this->name_ << "] Sensor" << std::endl;
  }

~EmptySensorClass()

virtual mars::EmptySensorClass::~EmptySensorClass ( )

virtualdefault

Member Function Documentation

get_state()

EmptySensorStateType mars::EmptySensorClass::get_state ( const std::shared_ptr< void > &  sensor_data )

inline

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

get_covariance()

Eigen::MatrixXd mars::EmptySensorClass::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.

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

Here is the call graph for this function:

set_initial_calib()

void mars::EmptySensorClass::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::EmptySensorClass::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.

  {
    // EmptyMeasurementType measurement = *static_cast<EmptyMeasurementType*>(sensor_data.get());
 
    EmptySensorData sensor_state;
    std::string calibration_type;
 
    if (this->initial_calib_provided_)
    {
      calibration_type = "Given";
 
      EmptySensorData calib = *static_cast<EmptySensorData*>(initial_calib_.get());
 
      sensor_state.state_ = calib.state_;
      sensor_state.sensor_cov_ = calib.sensor_cov_;
    }
    else
    {
      std::cout << "Empty calibration AUTO init not implemented yet" << std::endl;
      exit(EXIT_FAILURE);
    }
 
    // Bypass core state for the returned object
    BufferDataType result(std::make_shared<CoreType>(*latest_core_data.get()),
                          std::make_shared<EmptySensorData>(sensor_state));
 
    is_initialized_ = true;
 
    std::cout << "Info: Initialized [" << name_ << "] with [" << calibration_type << "] Calibration at t=" << timestamp
              << std::endl;
 
    std::cout << "Info: [" << name_ << "] Calibration(rounded):" << std::endl;
    std::cout << "\tPosition[m]: [" << sensor_state.state_.value_.transpose() << " ]" << std::endl;
 
    return result;
  }

CalcUpdate()

bool mars::EmptySensorClass::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.

  {
    // Cast the sensor measurement and prior state information
    // EmptyMeasurementType* meas = static_cast<EmptyMeasurementType*>(measurement.get());
    EmptySensorData* prior_sensor_data = static_cast<EmptySensorData*>(latest_sensor_data.get());
 
    // Extract sensor state
    EmptySensorStateType prior_sensor_state(prior_sensor_data->state_);
 
    // Return Results
    // CoreState data
    CoreType core_data;
    core_data.cov_ = prior_cov.block(0, 0, CoreStateType::size_error_, CoreStateType::size_error_);
    core_data.state_ = prior_core_state;
 
    // SensorState data
    std::shared_ptr<EmptySensorData> sensor_data(std::make_shared<EmptySensorData>());
    sensor_data->set_cov(prior_cov);
    sensor_data->state_ = prior_sensor_state;
 
    BufferDataType state_entry(std::make_shared<CoreType>(core_data), sensor_data);
 
    *new_state_data = state_entry;
 
    return true;
  }

ApplyCorrection()

EmptySensorStateType mars::EmptySensorClass::ApplyCorrection ( const EmptySensorStateType &  prior_sensor_state, const Eigen::MatrixXd &  correction  )

inline

  {
    // state + error state correction
    // with quaternion from small angle approx -> new state
 
    EmptySensorStateType corrected_sensor_state;
    corrected_sensor_state.value_ = prior_sensor_state.value_ + correction.block(0, 0, 3, 1);
    return corrected_sensor_state;
  }

---

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

• empty_sensor_class.h