11 #ifndef MAG_SENSOR_CLASS_H
12 #define MAG_SENSOR_CLASS_H
43 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 MagSensorClass(
const std::string& name, std::shared_ptr<CoreState> core_states)
57 std::cout <<
"Created: [" << this->
name_ <<
"] Sensor" << std::endl;
81 std::shared_ptr<CoreType> latest_core_data)
86 std::string calibration_type;
90 calibration_type =
"Given";
99 calibration_type =
"Auto";
100 std::cout <<
"Magnetometer calibration AUTO init not implemented yet" << std::endl;
105 BufferDataType result(std::make_shared<CoreType>(*latest_core_data.get()),
106 std::make_shared<MagSensorData>(sensor_state));
113 std::cout <<
"Info: Initialized [" <<
name_ <<
"] with [" << calibration_type <<
"] Calibration at t=" << timestamp
118 std::cout <<
"Info: [" <<
name_ <<
"] Calibration(rounded):" << std::endl;
119 std::cout <<
"\tMag Vector []: [" << sensor_state.
state_.mag_.transpose() <<
" ]" << std::endl;
120 std::cout <<
"\tOrientation Mag in IMU [deg]: ["
121 << sensor_state.
state_.q_im_.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * (180 / M_PI) <<
" ]"
129 std::shared_ptr<void> latest_sensor_data,
const Eigen::MatrixXd& prior_cov,
148 mag_meas = mag_meas / mag_meas.norm();
156 Eigen::MatrixXd R_meas_dyn;
163 R_meas_dyn = this->
R_.asDiagonal();
165 const Eigen::Matrix<double, 3, 3> R_meas = R_meas_dyn;
168 const int size_of_sensor_state = prior_sensor_state.
cov_size_;
169 const int size_of_full_error_state = size_of_core_state + size_of_sensor_state;
170 const Eigen::MatrixXd P = prior_cov;
171 assert(P.size() == size_of_full_error_state * size_of_full_error_state);
175 const Eigen::Matrix3d R_wi = prior_core_state.
q_wi_.toRotationMatrix();
176 const Eigen::Vector3d mag_w = prior_sensor_state.
mag_;
177 const Eigen::Matrix3d R_im = prior_sensor_state.
q_im_.toRotationMatrix();
180 const Eigen::Matrix3d Hm_pwi = Eigen::Matrix3d::Zero();
181 const Eigen::Matrix3d Hm_vwi = Eigen::Matrix3d::Zero();
182 const Eigen::Matrix3d Hm_rwi = R_im.transpose() *
Utils::Skew(R_wi.transpose() * mag_w);
183 const Eigen::Matrix3d Hm_bw = Eigen::Matrix3d::Zero();
184 const Eigen::Matrix3d Hm_ba = Eigen::Matrix3d::Zero();
185 const Eigen::Matrix3d Hm_mag = R_im.transpose() * R_wi.transpose();
186 const Eigen::Matrix3d Hm_rim =
Utils::Skew(R_im.transpose() * R_wi.transpose() * mag_w);
190 Eigen::MatrixXd H(3, Hm_pwi.cols() + Hm_vwi.cols() + Hm_rwi.cols() + Hm_bw.cols() + Hm_ba.cols() + Hm_mag.cols() +
192 H << Hm_pwi, Hm_vwi, Hm_rwi, Hm_bw, Hm_ba, Hm_mag, Hm_rim;
196 const Eigen::Vector3d mag_est = R_im.transpose() * R_wi.transpose() * mag_w;
197 residual_ = Eigen::MatrixXd(mag_est.rows(), 1);
203 assert(correction.size() == size_of_full_error_state * 1);
213 assert(P_updated.size() == size_of_full_error_state * size_of_full_error_state);
221 const Eigen::MatrixXd sensor_correction = correction.block(size_of_core_state, 0, size_of_sensor_state, 1);
228 core_data.
state_ = corrected_core_state;
231 std::shared_ptr<MagSensorData> sensor_data(std::make_shared<MagSensorData>());
232 sensor_data->set_cov(P_updated);
233 sensor_data->state_ = corrected_sensor_state;
235 BufferDataType state_entry(std::make_shared<CoreType>(core_data), sensor_data);
246 *new_state_data = state_entry;
257 corrected_sensor_state.
mag_ = prior_sensor_state.
mag_ + correction.block(0, 0, 3, 1);
258 corrected_sensor_state.
q_im_ =
260 return corrected_sensor_state;
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.
Definition: mag_measurement_type.h:21
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d mag_vector_
Raw magnetometer measurement [x y z].
Definition: mag_measurement_type.h:25
Definition: mag_sensor_class.h:35
MagSensorStateType get_state(const std::shared_ptr< void > &sensor_data)
Definition: mag_sensor_class.h:62
void set_initial_calib(std::shared_ptr< void > calibration)
set_initial_calib Sets the calibration of an individual sensor
Definition: mag_sensor_class.h:74
void set_intr_offset(const Eigen::Vector3d &v_offset)
Definition: mag_sensor_class.h:273
Eigen::Matrix3d mag_intr_transform_
Intrinsic cal distortion.
Definition: mag_sensor_class.h:40
MagSensorStateType ApplyCorrection(const MagSensorStateType &prior_sensor_state, const Eigen::MatrixXd &correction)
Definition: mag_sensor_class.h:251
bool normalize_
The measurement will be normalized if True.
Definition: mag_sensor_class.h:37
EIGEN_MAKE_ALIGNED_OPERATOR_NEW MagSensorClass(const std::string &name, std::shared_ptr< CoreState > core_states)
Definition: mag_sensor_class.h:45
void set_intr_transform(const Eigen::Matrix3d &m_transform)
Definition: mag_sensor_class.h:278
virtual ~MagSensorClass()=default
bool CalcUpdate(const Time &, 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: mag_sensor_class.h:128
BufferDataType Initialize(const Time ×tamp, std::shared_ptr< void >, std::shared_ptr< CoreType > latest_core_data)
Initialize the state of an individual sensor.
Definition: mag_sensor_class.h:80
void set_normalize(const bool &value)
Definition: mag_sensor_class.h:263
void set_apply_intrinsic(const bool &value)
Definition: mag_sensor_class.h:268
Eigen::Vector3d mag_intr_offset_
Intrinsic cal offset.
Definition: mag_sensor_class.h:39
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: mag_sensor_class.h:68
bool apply_intrinsic_
The intrinsic calibration will be aplied if True.
Definition: mag_sensor_class.h:38
Definition: mag_sensor_state_type.h:20
Eigen::Quaterniond q_im_
Definition: mag_sensor_state_type.h:25
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d mag_
Definition: mag_sensor_state_type.h:24
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::Matrix3d Skew(const Eigen::Vector3d &v)
skew generate the skew symmetric matrix of v
static Eigen::Quaterniond ApplySmallAngleQuatCorr(const Eigen::Quaterniond &q_prior, const Eigen::Vector3d &correction)
ApplySmallAngleQuatCorr.
Eigen::Matrix< double, CoreStateType::size_error_, 1 > CoreStateVector
Definition: core_state_type.h:135