Function yaw_to_R

uav_simulator/Utils/pose_utils/src/pose_utils.cpp:41–50

}

mat yaw_to_R(double yaw) {
  mat R = zeros<mat>(2, 2);
  double c = cos(yaw);
  double s = sin(yaw);
  R(0, 0) = c;
  R(1, 0) = s;
  R(0, 1) = -s;
  R(1, 1) = c;
  return R;
}

colvec R_to_ypr(const mat& R) {
  colvec n = R.col(0);