CalcUpdate Calculates the update for an individual sensor definition.
196 VisionMeasurementType* meas =
static_cast<VisionMeasurementType*
>(measurement.get());
200 Eigen::Vector3d p_meas = meas->position_;
201 Eigen::Quaternion<double> q_meas = meas->orientation_;
204 VisionSensorStateType prior_sensor_state(prior_sensor_data->state_);
208 Eigen::MatrixXd R_meas_dyn;
211 meas->get_meas_noise(&R_meas_dyn);
215 R_meas_dyn = this->
R_.asDiagonal();
217 const Eigen::Matrix<double, 6, 6> R_meas = R_meas_dyn;
220 const int size_of_sensor_state = prior_sensor_state.cov_size_;
221 const int size_of_full_error_state = size_of_core_state + size_of_sensor_state;
222 const Eigen::MatrixXd P = prior_cov;
223 assert(P.size() == size_of_full_error_state * size_of_full_error_state);
226 const Eigen::Matrix3d I_3 = Eigen::Matrix3d::Identity();
227 const Eigen::Vector3d P_wi = prior_core_state.p_wi_;
228 const Eigen::Matrix3d R_wi = prior_core_state.q_wi_.toRotationMatrix();
229 const Eigen::Vector3d P_vw = prior_sensor_state.p_vw_;
230 const Eigen::Matrix3d R_vw = prior_sensor_state.q_vw_.toRotationMatrix();
231 const Eigen::Vector3d P_ic = prior_sensor_state.p_ic_;
232 const Eigen::Matrix3d R_ic = prior_sensor_state.q_ic_.toRotationMatrix();
233 const double L = prior_sensor_state.lambda_;
236 const Eigen::Matrix3d Hp_pwi = L * R_vw;
237 const Eigen::Matrix3d Hp_vwi = Eigen::Matrix3d::Zero();
238 const Eigen::Matrix3d Hp_rwi = -L * R_vw * R_wi *
Utils::Skew(P_ic);
239 const Eigen::Matrix3d Hp_bw = Eigen::Matrix3d::Zero();
240 const Eigen::Matrix3d Hp_ba = Eigen::Matrix3d::Zero();
242 const Eigen::Matrix3d Hp_pvw = I_3 * L;
243 const Eigen::Matrix3d Hp_rvw = -L * R_vw *
Utils::Skew(P_wi + R_wi * P_ic);
244 const Eigen::Matrix3d Hp_pic = L * R_vw * R_wi;
245 const Eigen::Matrix3d Hp_ric = Eigen::Matrix3d::Zero();
246 Eigen::Vector3d Hp_lambda;
249 Hp_lambda = P_vw + R_vw * (P_wi + R_wi * P_ic);
253 Hp_lambda = Eigen::Vector3d::Zero();
257 Eigen::MatrixXd H_p(3, Hp_pwi.cols() + Hp_vwi.cols() + Hp_rwi.cols() + Hp_bw.cols() + Hp_ba.cols() + Hp_pvw.cols() +
258 Hp_rvw.cols() + Hp_pic.cols() + Hp_ric.cols() + Hp_lambda.cols());
260 H_p << Hp_pwi, Hp_vwi, Hp_rwi, Hp_bw, Hp_ba, Hp_pvw, Hp_rvw, Hp_pic, Hp_ric, Hp_lambda;
263 const Eigen::Matrix3d Hr_pwi = Eigen::Matrix3d::Zero();
264 const Eigen::Matrix3d Hr_vwi = Eigen::Matrix3d::Zero();
265 const Eigen::Matrix3d Hr_rwi = R_ic.transpose();
266 const Eigen::Matrix3d Hr_bw = Eigen::Matrix3d::Zero();
267 const Eigen::Matrix3d Hr_ba = Eigen::Matrix3d::Zero();
269 const Eigen::Matrix3d Hr_pvw = Eigen::Matrix3d::Zero();
270 const Eigen::Matrix3d Hr_rvw = R_ic.transpose() * R_wi.transpose();
271 const Eigen::Matrix3d Hr_pic = Eigen::Matrix3d::Zero();
272 const Eigen::Matrix3d Hr_ric = I_3;
273 const Eigen::Vector3d Hr_lambda = Eigen::Vector3d::Zero();
277 Eigen::MatrixXd H_r(3, Hr_pwi.cols() + Hr_vwi.cols() + Hr_rwi.cols() + Hr_bw.cols() + Hr_ba.cols() + Hr_pvw.cols() +
278 Hr_rvw.cols() + Hr_pic.cols() + Hr_ric.cols() + Hr_lambda.cols());
279 H_r << Hr_pwi, Hr_vwi, Hr_rwi, Hr_bw, Hr_ba, Hr_pvw, Hr_rvw, Hr_pic, Hr_ric, Hr_lambda;
282 Eigen::MatrixXd H(H_p.rows() + H_r.rows(), H_r.cols());
287 const Eigen::Vector3d p_est = (P_vw + R_vw * (P_wi + R_wi * P_ic)) * L;
288 const Eigen::Vector3d res_p = p_meas - p_est;
291 const Eigen::Quaternion<double> q_est =
292 prior_sensor_state.q_vw_ * prior_core_state.q_wi_ * prior_sensor_state.q_ic_;
293 const Eigen::Quaternion<double> res_q = q_est.inverse() * q_meas;
294 const Eigen::Vector3d res_r = 2 * res_q.vec() / res_q.w();
297 residual_ = Eigen::MatrixXd(res_p.rows() + res_r.rows(), 1);
302 const Eigen::MatrixXd correction = ekf.CalculateCorrection(&
chi2_);
303 assert(correction.size() == size_of_full_error_state * 1);
312 Eigen::MatrixXd P_updated = ekf.CalculateCovUpdate();
313 assert(P_updated.size() == size_of_full_error_state * size_of_full_error_state);
321 const Eigen::MatrixXd sensor_correction = correction.block(size_of_core_state, 0, size_of_sensor_state, 1);
322 const VisionSensorStateType corrected_sensor_state =
ApplyCorrection(prior_sensor_state, sensor_correction);
328 core_data.state_ = corrected_core_state;
331 std::shared_ptr<VisionSensorData> sensor_data(std::make_shared<VisionSensorData>());
332 sensor_data->set_cov(P_updated);
333 sensor_data->state_ = corrected_sensor_state;
335 BufferDataType state_entry(std::make_shared<CoreType>(core_data), sensor_data);
346 *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
bool do_update_
True if updates should be performed with the sensor.
Definition: sensor_abs_class.h:25
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
VisionSensorStateType ApplyCorrection(const VisionSensorStateType &prior_sensor_state, const Eigen::MatrixXd &correction)
Definition: vision_sensor_class.h:351
Eigen::Matrix< double, CoreStateType::size_error_, 1 > CoreStateVector
Definition: core_state_type.h:135