CalcUpdate Calculates the update for an individual sensor definition.
122 PoseMeasurementType* meas =
static_cast<PoseMeasurementType*
>(measurement.get());
126 Eigen::Vector3d p_meas = meas->position_;
127 Eigen::Quaternion<double> q_meas = meas->orientation_;
130 PoseSensorStateType prior_sensor_state(prior_sensor_data->state_);
134 Eigen::MatrixXd R_meas_dyn;
137 meas->get_meas_noise(&R_meas_dyn);
141 R_meas_dyn = this->
R_.asDiagonal();
143 const Eigen::Matrix<double, 6, 6> R_meas = R_meas_dyn;
146 const int size_of_sensor_state = prior_sensor_state.cov_size_;
147 const int size_of_full_error_state = size_of_core_state + size_of_sensor_state;
148 const Eigen::MatrixXd P = prior_cov;
149 assert(P.size() == size_of_full_error_state * size_of_full_error_state);
152 const Eigen::Matrix3d I_3 = Eigen::Matrix3d::Identity();
153 const Eigen::Vector3d P_wi = prior_core_state.p_wi_;
154 const Eigen::Matrix3d R_wi = prior_core_state.q_wi_.toRotationMatrix();
155 const Eigen::Vector3d P_ip = prior_sensor_state.p_ip_;
156 const Eigen::Matrix3d R_ip = prior_sensor_state.q_ip_.toRotationMatrix();
159 const Eigen::Matrix3d Hp_pwi = I_3;
160 const Eigen::Matrix3d Hp_vwi = Eigen::Matrix3d::Zero();
161 const Eigen::Matrix3d Hp_rwi = -R_wi *
Utils::Skew(P_ip);
162 const Eigen::Matrix3d Hp_bw = Eigen::Matrix3d::Zero();
163 const Eigen::Matrix3d Hp_ba = Eigen::Matrix3d::Zero();
164 const Eigen::Matrix3d Hp_ip = R_wi;
165 const Eigen::Matrix3d Hp_rip = Eigen::Matrix3d::Zero();
169 Eigen::MatrixXd H_p(3, Hp_pwi.cols() + Hp_vwi.cols() + Hp_rwi.cols() + Hp_bw.cols() + Hp_ba.cols() + Hp_ip.cols() +
171 H_p << Hp_pwi, Hp_vwi, Hp_rwi, Hp_bw, Hp_ba, Hp_ip, Hp_rip;
174 const Eigen::Matrix3d Hr_pwi = Eigen::Matrix3d::Zero();
175 const Eigen::Matrix3d Hr_vwi = Eigen::Matrix3d::Zero();
176 const Eigen::Matrix3d Hr_rwi = R_ip.transpose();
177 const Eigen::Matrix3d Hr_bw = Eigen::Matrix3d::Zero();
178 const Eigen::Matrix3d Hr_ba = Eigen::Matrix3d::Zero();
179 const Eigen::Matrix3d Hr_pip = Eigen::Matrix3d::Zero();
180 const Eigen::Matrix3d Hr_rip = I_3;
184 Eigen::MatrixXd H_r(3, Hr_pwi.cols() + Hr_vwi.cols() + Hr_rwi.cols() + Hr_bw.cols() + Hr_ba.cols() + Hr_pip.cols() +
186 H_r << Hr_pwi, Hr_vwi, Hr_rwi, Hr_bw, Hr_ba, Hr_pip, Hr_rip;
189 Eigen::MatrixXd H(H_p.rows() + H_r.rows(), H_r.cols());
194 const Eigen::Vector3d p_est = P_wi + R_wi * P_ip;
195 const Eigen::Vector3d res_p = p_meas - p_est;
197 const Eigen::Quaternion<double> q_est = prior_core_state.q_wi_ * prior_sensor_state.q_ip_;
198 const Eigen::Quaternion<double> res_q = q_est.inverse() * q_meas;
199 const Eigen::Vector3d res_r = 2 * res_q.vec() / res_q.w();
202 residual_ = Eigen::MatrixXd(res_p.rows() + res_r.rows(), 1);
207 const Eigen::MatrixXd correction = ekf.CalculateCorrection(&
chi2_);
208 assert(correction.size() == size_of_full_error_state * 1);
217 Eigen::MatrixXd P_updated = ekf.CalculateCovUpdate();
218 assert(P_updated.size() == size_of_full_error_state * size_of_full_error_state);
226 const Eigen::MatrixXd sensor_correction = correction.block(size_of_core_state, 0, size_of_sensor_state, 1);
227 const PoseSensorStateType corrected_sensor_state =
ApplyCorrection(prior_sensor_state, sensor_correction);
233 core_data.state_ = corrected_core_state;
236 std::shared_ptr<PoseSensorData> sensor_data(std::make_shared<PoseSensorData>());
237 sensor_data->set_cov(P_updated);
238 sensor_data->state_ = corrected_sensor_state;
240 BufferDataType state_entry(std::make_shared<CoreType>(core_data), sensor_data);
251 *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
PoseSensorStateType ApplyCorrection(const PoseSensorStateType &prior_sensor_state, const Eigen::MatrixXd &correction)
Definition: pose_sensor_class.h:256
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