13 #ifndef ATTITUDE_SENSOR_CLASS_H
14 #define ATTITUDE_SENSOR_CLASS_H
50 os <<
"ROLL/PITCH/YAW";
66 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
89 std::cout <<
"Warning: [" << this->
name_
90 <<
"] Unexpected type for AttitudeSensorClass.\n"
91 "Assuming roll-pitch-yaw measurement."
100 std::cout <<
"Created: [" << this->
name_ <<
"] Sensor (type: " <<
attitude_type_ <<
")" << std::endl;
124 std::shared_ptr<CoreType> latest_core_data)
129 std::string calibration_type;
133 calibration_type =
"Given";
165 BufferDataType result(std::make_shared<CoreType>(*latest_core_data.get()),
166 std::make_shared<AttitudeSensorData>(sensor_state));
173 std::cout <<
"Info: Initialized [" <<
name_ <<
"] with [" << calibration_type <<
"] Calibration at t=" << timestamp
178 std::cout <<
"Info: [" <<
name_ <<
"] Calibration(rounded):" << std::endl;
179 std::cout <<
"\tOrientation[deg]: ["
180 << sensor_state.
state_.q_aw_.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * (180 / M_PI) <<
" ]"
188 std::shared_ptr<void> latest_sensor_data,
const Eigen::MatrixXd& prior_cov,
194 return CalcUpdateRP(timestamp, measurement, prior_core_state, latest_sensor_data, prior_cov, new_state_data);
196 return CalcUpdateRPY(timestamp, measurement, prior_core_state, latest_sensor_data, prior_cov, new_state_data);
198 std::cout <<
"Error: [" << this->
name_ <<
"] Cannot perform update (unknown type)" << std::endl;
206 const CoreStateType& prior_core_state,
const std::shared_ptr<void>& latest_sensor_data,
221 Eigen::MatrixXd R_meas_dyn;
228 R_meas_dyn = this->
R_.asDiagonal();
230 const Eigen::Matrix<double, 2, 2> R_meas = R_meas_dyn;
233 const int size_of_sensor_state = prior_sensor_state.
cov_size_;
234 const int size_of_full_error_state = size_of_core_state + size_of_sensor_state;
235 const Eigen::MatrixXd P = prior_cov;
236 assert(P.size() == size_of_full_error_state * size_of_full_error_state);
239 typedef Eigen::Matrix<double, 2, 3> Matrix23d_t;
241 I_23 << 1., 0., 0., 0., 1., 0.;
242 const Matrix23d_t Z_23 = Matrix23d_t::Zero();
243 const Eigen::Matrix3d R_wi = prior_core_state.
q_wi_.toRotationMatrix();
244 const Eigen::Matrix3d R_aw = prior_sensor_state.
q_aw_.toRotationMatrix();
245 const Eigen::Matrix3d R_ib = prior_sensor_state.
q_ib_.toRotationMatrix();
248 const Matrix23d_t Hr_pwi = Z_23;
249 const Matrix23d_t Hr_vwi = Z_23;
250 const Matrix23d_t Hr_rwi = R_ib.transpose().block(0, 0, 2, 3);
251 const Matrix23d_t Hr_bw = Z_23;
252 const Matrix23d_t Hr_ba = Z_23;
254 const Matrix23d_t Hr_raw = (R_ib.transpose() * R_wi.transpose()).block(0, 0, 2, 3);
255 const Matrix23d_t Hr_rib = I_23;
259 Eigen::MatrixXd H(2, Hr_pwi.cols() + Hr_vwi.cols() + Hr_rwi.cols() + Hr_bw.cols() + Hr_ba.cols() + Hr_raw.cols() +
261 H << Hr_pwi, Hr_vwi, Hr_rwi, Hr_bw, Hr_ba, Hr_raw, Hr_rib;
266 const Eigen::Vector2d rp_est(rpy_est(0), rpy_est(1));
267 residual_ = Eigen::MatrixXd(rp_est.rows(), 1);
273 assert(correction.size() == size_of_full_error_state * 1);
283 assert(P_updated.size() == size_of_full_error_state * size_of_full_error_state);
291 const Eigen::MatrixXd sensor_correction = correction.block(size_of_core_state, 0, size_of_sensor_state, 1);
298 core_data.
state_ = corrected_core_state;
301 std::shared_ptr<AttitudeSensorData> sensor_data(std::make_shared<AttitudeSensorData>());
302 sensor_data->set_cov(P_updated);
303 sensor_data->state_ = corrected_sensor_state;
305 BufferDataType state_entry(std::make_shared<CoreType>(core_data), sensor_data);
316 *new_state_data = state_entry;
322 const CoreStateType& prior_core_state,
const std::shared_ptr<void>& latest_sensor_data,
336 Eigen::MatrixXd R_meas_dyn;
343 R_meas_dyn = this->
R_.asDiagonal();
345 const Eigen::Matrix<double, 3, 3> R_meas = R_meas_dyn;
348 const int size_of_sensor_state = prior_sensor_state.
cov_size_;
349 const int size_of_full_error_state = size_of_core_state + size_of_sensor_state;
350 const Eigen::MatrixXd P = prior_cov;
351 assert(P.size() == size_of_full_error_state * size_of_full_error_state);
354 const Eigen::Matrix3d I_3 = Eigen::Matrix3d::Identity();
355 const Eigen::Matrix3d Z_3 = Eigen::Matrix3d::Zero();
356 const Eigen::Matrix3d R_wi = prior_core_state.
q_wi_.toRotationMatrix();
358 const Eigen::Matrix3d R_ib = prior_sensor_state.
q_ib_.toRotationMatrix();
361 const Eigen::Matrix3d Hr_pwi = Z_3;
362 const Eigen::Matrix3d Hr_vwi = Z_3;
363 const Eigen::Matrix3d Hr_rwi = R_ib.transpose();
364 const Eigen::Matrix3d Hr_bw = Z_3;
365 const Eigen::Matrix3d Hr_ba = Z_3;
367 const Eigen::Matrix3d Hr_raw = R_ib.transpose() * R_wi.transpose();
368 const Eigen::Matrix3d Hr_rib = I_3;
372 Eigen::MatrixXd H(3, Hr_pwi.cols() + Hr_vwi.cols() + Hr_rwi.cols() + Hr_bw.cols() + Hr_ba.cols() + Hr_raw.cols() +
374 H << Hr_pwi, Hr_vwi, Hr_rwi, Hr_bw, Hr_ba, Hr_raw, Hr_rib;
378 const Eigen::Quaternion<double> q_est =
379 prior_sensor_state.
q_aw_ * prior_core_state.
q_wi_ * prior_sensor_state.
q_ib_;
380 const Eigen::Quaternion<double> res_q = q_est.inverse() * q_meas;
381 residual_ = Eigen::MatrixXd(res_q.vec().rows(), 1);
382 residual_ << (2 * res_q.vec() / res_q.w());
386 const Eigen::MatrixXd correction = ekf.CalculateCorrection(&
chi2_);
387 assert(correction.size() == size_of_full_error_state * 1);
396 Eigen::MatrixXd P_updated = ekf.CalculateCovUpdate();
397 assert(P_updated.size() == size_of_full_error_state * size_of_full_error_state);
404 const Eigen::MatrixXd sensor_correction = correction.block(size_of_core_state, 0, size_of_sensor_state, 1);
411 core_data.
state_ = corrected_core_state;
414 std::shared_ptr<AttitudeSensorData> sensor_data(std::make_shared<AttitudeSensorData>());
415 sensor_data->set_cov(P_updated);
416 sensor_data->state_ = corrected_sensor_state;
418 BufferDataType state_entry(std::make_shared<CoreType>(core_data), sensor_data);
429 *new_state_data = state_entry;
435 const Eigen::MatrixXd& correction)
446 Eigen::Vector3d q_aw_correction, q_ib_correction;
450 q_aw_correction << correction(0), correction(1), 0;
451 q_ib_correction << correction(2), correction(3), 0;
454 q_aw_correction = correction.block(0, 0, 3, 1);
455 q_ib_correction = correction.block(3, 0, 3, 1);
458 std::cout <<
"Error: [" << this->
name_ <<
"] Cannot perform correction (unknown type)" << std::endl;
459 q_aw_correction = Eigen::Vector3d::Zero();
460 q_ib_correction = Eigen::Vector3d::Zero();
467 return corrected_sensor_state;
Definition: attitude_measurement_type.h:26
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Attitude attitude_
Definition: attitude_measurement_type.h:31
Definition: attitude_sensor_class.h:60
AttitudeSensorStateType ApplyCorrection(const AttitudeSensorStateType &prior_sensor_state, const Eigen::MatrixXd &correction)
Definition: attitude_sensor_class.h:434
AttitudeSensorStateType get_state(const std::shared_ptr< void > &sensor_data)
Definition: attitude_sensor_class.h:105
BufferDataType Initialize(const Time ×tamp, std::shared_ptr< void >, std::shared_ptr< CoreType > latest_core_data)
Initialize the state of an individual sensor.
Definition: attitude_sensor_class.h:123
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)
Definition: attitude_sensor_class.h:205
EIGEN_MAKE_ALIGNED_OPERATOR_NEW AttitudeSensorClass(const std::string &name, std::shared_ptr< CoreState > core_states, AttitudeSensorType type=AttitudeSensorType::RPY_TYPE)
Definition: attitude_sensor_class.h:68
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)
Definition: attitude_sensor_class.h:321
void set_initial_calib(std::shared_ptr< void > calibration)
set_initial_calib Sets the calibration of an individual sensor
Definition: attitude_sensor_class.h:117
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...
Definition: attitude_sensor_class.h:111
bool CalcUpdate(const Time ×tamp, 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.
Definition: attitude_sensor_class.h:187
virtual ~AttitudeSensorClass()=default
AttitudeSensorType attitude_type_
Definition: attitude_sensor_class.h:63
Definition: attitude_sensor_state_type.h:22
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Quaternion< double > q_aw_
Definition: attitude_sensor_state_type.h:26
Eigen::Quaternion< double > q_ib_
Definition: attitude_sensor_state_type.h:27
bool has_meas_noise
Definition: measurement_base_class.h:23
bool get_meas_noise(Eigen::MatrixXd *meas_noise)
get the measurement noise associated with the current sensor measurement
Definition: measurement_base_class.h:25
int cov_size_
Definition: base_states.h:25
The BaseSensorData class binds the sensor state and covariance matrix.
Definition: bind_sensor_data.h:29
EIGEN_MAKE_ALIGNED_OPERATOR_NEW T state_
Definition: bind_sensor_data.h:30
Eigen::MatrixXd get_full_cov() const
get_full_cov builds the full covariance matrix
Definition: bind_sensor_data.h:63
Eigen::MatrixXd sensor_cov_
covariance of the sensor states
Definition: bind_sensor_data.h:37
The BufferDataType binds the core and sensor state in form of a shared void pointer.
Definition: buffer_data_type.h:36
bool passed_
Determine if the test is performed or not.
Definition: ekf.h:84
bool do_test_
Upper critival value.
Definition: ekf.h:83
void set_dof(const int &value)
set_dof Set degree of freedom for the X2 distribution
void PrintReport(const std::string &name)
PrintReport Print a formated report e.g. if the test did not pass.
Definition: core_state_type.h:21
static constexpr int size_error_
Definition: core_state_type.h:38
Eigen::Quaternion< double > q_wi_
Definition: core_state_type.h:29
static CoreStateType ApplyCorrection(CoreStateType state_prior, Eigen::Matrix< double, CoreStateType::size_error_, 1 > correction)
ApplyCorrection.
Definition: core_state_type.h:46
Definition: core_type.h:19
CoreStateMatrix cov_
Definition: core_type.h:22
CoreStateType state_
Definition: core_type.h:21
Eigen::MatrixXd CalculateCovUpdate()
CalculateCovUpdate Updating the state covariance after the state update.
Eigen::MatrixXd CalculateCorrection()
Kalman gain.
std::string name_
Name of the individual sensor instance.
Definition: sensor_abs_class.h:23
bool is_initialized_
True if the sensor has been initialized.
Definition: sensor_abs_class.h:24
bool const_ref_to_nav_
True if the reference should not be estimated.
Definition: sensor_abs_class.h:27
bool use_dynamic_meas_noise_
True if dynamic noise values from measurements should be used.
Definition: sensor_abs_class.h:29
Definition: update_sensor_abs_class.h:24
bool initial_calib_provided_
True if an initial calibration was provided.
Definition: update_sensor_abs_class.h:38
std::shared_ptr< CoreState > core_states_
Definition: update_sensor_abs_class.h:42
Eigen::VectorXd R_
Measurement noise "squared".
Definition: update_sensor_abs_class.h:32
std::shared_ptr< void > initial_calib_
Definition: update_sensor_abs_class.h:37
Chi2 chi2_
Definition: update_sensor_abs_class.h:40
Eigen::MatrixXd residual_
Definition: update_sensor_abs_class.h:31
static Eigen::MatrixXd EnforceMatrixSymmetry(const Eigen::Ref< const Eigen::MatrixXd > &mat_in)
EnforceMatrixSymmetry.
static Eigen::Vector3d RPYFromRotMat(const Eigen::Matrix3d &rot_mat)
RPYFromRotMat derives the roll pitch and yaw angle from a rotation matrix (in that order)
static Eigen::Quaterniond ApplySmallAngleQuatCorr(const Eigen::Quaterniond &q_prior, const Eigen::Vector3d &correction)
ApplySmallAngleQuatCorr.
std::ostream & operator<<(std::ostream &os, AttitudeSensorType type)
Definition: attitude_sensor_class.h:42
AttitudeSensorType
Definition: attitude_sensor_class.h:37
@ RPY_TYPE
full orientation, roll, pitch, and yaw
@ RP_TYPE
aviation attitude, roll and pitch only
Eigen::Matrix< double, CoreStateType::size_error_, 1 > CoreStateVector
Definition: core_state_type.h:135
Eigen::Quaterniond quaternion_
Definition: attitude_conversion.h:48
Eigen::Vector2d get_rp()
Definition: attitude_conversion.h:55