CalcUpdate Calculates the update for an individual sensor definition.
133 MagMeasurementType* meas =
static_cast<MagMeasurementType*
>(measurement.get());
137 Eigen::Vector3d mag_meas(meas->mag_vector_);
148 mag_meas = mag_meas / mag_meas.norm();
152 MagSensorStateType prior_sensor_state(prior_sensor_data->state_);
156 Eigen::MatrixXd R_meas_dyn;
159 meas->get_meas_noise(&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);
202 const Eigen::MatrixXd correction = ekf.CalculateCorrection(&
chi2_);
203 assert(correction.size() == size_of_full_error_state * 1);
212 Eigen::MatrixXd P_updated = ekf.CalculateCovUpdate();
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);
222 const MagSensorStateType corrected_sensor_state =
ApplyCorrection(prior_sensor_state, sensor_correction);
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;
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 PrintReport(const std::string &name)
PrintReport Print a formated report e.g. if the test did not pass.
static constexpr int size_error_
Definition: core_state_type.h:38
static CoreStateType ApplyCorrection(CoreStateType state_prior, Eigen::Matrix< double, CoreStateType::size_error_, 1 > correction)
ApplyCorrection.
Definition: core_state_type.h:46
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::Vector3d mag_intr_offset_
Intrinsic cal offset.
Definition: mag_sensor_class.h:39
bool apply_intrinsic_
The intrinsic calibration will be aplied if True.
Definition: mag_sensor_class.h:38
bool use_dynamic_meas_noise_
True if dynamic noise values from measurements should be used.
Definition: sensor_abs_class.h:29
Eigen::VectorXd R_
Measurement noise "squared".
Definition: update_sensor_abs_class.h:32
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
Eigen::Matrix< double, CoreStateType::size_error_, 1 > CoreStateVector
Definition: core_state_type.h:135