Function pose_update_2d

uav_simulator/Utils/pose_utils/src/pose_utils.cpp:192–198

}

colvec pose_update_2d(const colvec& X1, const colvec& X2) {
  mat R = yaw_to_R(X1(2));
  colvec X3(3);
  X3.rows(0, 1) = R * X2.rows(0, 1) + X1.rows(0, 1);
  X3(2) = X1(2) + X2(2);
  return X3;
}

colvec pose_inverse_2d(const colvec& X) {
  double c = cos(X(2));