Function pose_update

uav_simulator/Utils/pose_utils/src/pose_utils.cpp:161–171

// General Pose Update ----------

colvec pose_update(const colvec& X1, const colvec& X2) {
  mat R1 = ypr_to_R(X1.rows(3, 5));
  mat R2 = ypr_to_R(X2.rows(3, 5));
  mat R3 = R1 * R2;

  colvec X3xyz = X1.rows(0, 2) + R1 * X2.rows(0, 2);
  colvec X3ypr = R_to_ypr(R3);

  colvec X3 = join_cols(X3xyz, X3ypr);
  return X3;
}

colvec pose_inverse(const colvec& X) {
  mat R = ypr_to_R(X.rows(3, 5));