CalcUpdate Calculates the update for an individual sensor definition.
125 BodyvelMeasurementType* meas =
static_cast<BodyvelMeasurementType*
>(measurement.get());
129 Eigen::Vector3d v_meas = meas->velocity_;
132 BodyvelSensorStateType prior_sensor_state(prior_sensor_data->state_);
136 Eigen::MatrixXd R_meas_dyn;
139 meas->get_meas_noise(&R_meas_dyn);
143 R_meas_dyn = this->
R_.asDiagonal();
145 const Eigen::Matrix<double, 3, 3> R_meas = R_meas_dyn;
148 const int size_of_sensor_state = prior_sensor_state.cov_size_;
149 const int size_of_full_error_state = size_of_core_state + size_of_sensor_state;
150 const Eigen::MatrixXd P = prior_cov;
151 assert(P.size() == size_of_full_error_state * size_of_full_error_state);
155 const Eigen::Matrix3d Z_3 = Eigen::Matrix3d::Zero();
157 const Eigen::Vector3d V_wi = prior_core_state.v_wi_;
158 const Eigen::Matrix3d R_wi = prior_core_state.q_wi_.toRotationMatrix();
159 const Eigen::Vector3d P_ib = prior_sensor_state.p_ib_;
160 const Eigen::Matrix3d R_ib = prior_sensor_state.q_ib_.toRotationMatrix();
162 const Eigen::Vector3d w_wi = prior_core_state.w_m_ - prior_core_state.b_w_;
163 const Eigen::Matrix3d w_wi_skew =
Utils::Skew(w_wi);
166 const Eigen::Matrix3d Hv_pwi = Z_3;
167 const Eigen::Matrix3d Hv_vwi = R_ib.transpose() * R_wi.transpose();
168 const Eigen::Matrix3d Hv_rwi = R_ib.transpose() *
Utils::Skew(R_wi.transpose() * V_wi);
169 const Eigen::Matrix3d Hv_bw = R_ib.transpose() *
Utils::Skew(P_ib);
170 const Eigen::Matrix3d Hv_ba = Z_3;
171 const Eigen::Matrix3d Hv_pib = R_ib.transpose() * w_wi_skew;
172 const Eigen::Matrix3d Hv_rib =
173 Utils::Skew(R_ib.transpose() * R_wi.transpose() * V_wi) +
Utils::Skew(R_ib.transpose() * w_wi_skew * P_ib);
177 Eigen::MatrixXd H_v(3, Hv_pwi.cols() + Hv_vwi.cols() + Hv_rwi.cols() + Hv_bw.cols() + Hv_ba.cols() + Hv_pib.cols() +
179 H_v << Hv_pwi, Hv_vwi, Hv_rwi, Hv_bw, Hv_ba, Hv_pib, Hv_rib;
182 Eigen::MatrixXd H(H_v.rows(), H_v.cols());
187 const Eigen::Vector3d v_est = R_ib.transpose() * R_wi.transpose() * V_wi + R_ib.transpose() * w_wi_skew * P_ib;
188 residual_ = Eigen::MatrixXd(v_est.rows(), 1);
193 const Eigen::MatrixXd correction = ekf.CalculateCorrection(&
chi2_);
194 assert(correction.size() == size_of_full_error_state * 1);
203 Eigen::MatrixXd P_updated = ekf.CalculateCovUpdate();
204 assert(P_updated.size() == size_of_full_error_state * size_of_full_error_state);
212 const Eigen::MatrixXd sensor_correction = correction.block(size_of_core_state, 0, size_of_sensor_state, 1);
213 const BodyvelSensorStateType corrected_sensor_state =
ApplyCorrection(prior_sensor_state, sensor_correction);
219 core_data.state_ = corrected_core_state;
222 std::shared_ptr<BodyvelSensorData> sensor_data(std::make_shared<BodyvelSensorData>());
223 sensor_data->set_cov(P_updated);
224 sensor_data->state_ = corrected_sensor_state;
226 BufferDataType state_entry(std::make_shared<CoreType>(core_data), sensor_data);
237 *new_state_data = state_entry;
BodyvelSensorStateType ApplyCorrection(const BodyvelSensorStateType &prior_sensor_state, const Eigen::MatrixXd &correction)
Definition: bodyvel_sensor_class.h:242
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
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