#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 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 &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. More...

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". 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

◆ 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

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_