diff --git a/mavros/include/mavros/mavros_uas.h b/mavros/include/mavros/mavros_uas.h
index edfa87811..76c3fb676 100644
--- a/mavros/include/mavros/mavros_uas.h
+++ b/mavros/include/mavros/mavros_uas.h
@@ -297,35 +297,20 @@ class UAS {
 	 */
 	static tf::Vector3 sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation);
 
-	/**
-	 * @brief Function to convert position values from ENU to NED frames and vice-versa
-	 */
-	static tf::Vector3 convert_position(float _x, float _y, float _z);
-
-	/**
-	 * @brief Function to convert velocity values from ENU to NED frames and vice-versa
-	 */
-	static tf::Vector3 convert_velocity(float _vx, float _vy, float _vz);
-
-	/**
-	 * @brief Function to convert acceleration values from ENU to NED frames and vice-versa
-	 */
-	static tf::Vector3 convert_accel(float _ax, float _ay, float _az);
-
 	/**
 	 * @brief Function to convert general XYZ values from ENU to NED frames and vice-versa
 	 */
-	static tf::Vector3 convert_general_xyz(float _x, float _y, float _z);
+	static tf::Vector3 transform_frame_general_xyz(float _x, float _y, float _z);
 
 	/**
 	 * @brief Function to convert attitude quaternion values from ENU to NED frames and vice-versa
 	 */
-	static tf::Quaternion convert_attitude_q(tf::Quaternion qo);
+	static tf::Quaternion transform_frame_attitude_q(tf::Quaternion qo);
 
 	/**
 	 * @brief Function to convert attitude euler angles values from ENU to NED frames and vice-versa
 	 */
-	static tf::Vector3 convert_attitude_rpy(float _roll, float _pitch, float _yaw);
+	static tf::Vector3 transform_frame_attitude_rpy(float _roll, float _pitch, float _yaw);
 
 	/**
 	 * @brief Function to convert full 6D pose covariance matrix values from ENU to NED frames and vice-versa
@@ -349,7 +334,31 @@ class UAS {
 	 *
 	 * 			C' = R * C * R^T
 	 */
-	static std::array<float, 36> convert_covariance_pose6x6(std::array<float, 36> _covariance);
+	static std::array<float, 36> transform_frame_covariance_pose6x6(std::array<float, 36> _covariance);
+
+	/**
+	 * @brief Function to convert position or attitude RPY covariance matrix values from ENU to NED frames and vice-versa
+	 * @details Matrix formats
+	 *
+	 * Pos_Cov_matrix =	|var_x  cov_xy cov_xz |
+	 * 			|cov_yx var_y  cov_yz |
+	 * 			|cov_zx cov_zy var_z  |
+	 *
+	 * Att_Cov_matrix =	|var_Z  cov_ZY cov_ZX |
+	 * 			|cov_YZ var_Y  cov_YX |
+	 * 			|cov_XZ cov_XY var_X  |
+	 *
+	 * Note that for ROS<->ENU frame transformations, the rotation matrix is the same for position and attitude.
+	 *
+	 * Rot_matrix = | 1	 0	 0 |
+	 * 		| 0	-1 	 0 |
+	 * 		| 0	 0	-1 |
+	 *
+	 * Compute Covariance matrix in another frame:
+	 *
+	 * 			C' = R * C * R^T
+	 */
+	static std::array<float, 9> transform_frame_covariance_general3x3(std::array<float, 9> _covariance);
 
 private:
 	std::recursive_mutex mutex;
diff --git a/mavros/src/lib/uas_frame_conversions.cpp b/mavros/src/lib/uas_frame_conversions.cpp
index fa54f1b8b..1239a5a90 100644
--- a/mavros/src/lib/uas_frame_conversions.cpp
+++ b/mavros/src/lib/uas_frame_conversions.cpp
@@ -21,49 +21,28 @@ const double PI = boost::math::constants::pi<double>();
 
 using namespace mavros;
 
-static tf::Vector3 convert_position(float _x, float _y, float _z){
+static tf::Vector3 transform_frame_general_xyz(float _x, float _y, float _z){
 	float x =  _x;
 	float y = -_y;
 	float z = -_z;
 	return tf::Vector3(x, y, z);
 };
 
-static tf::Vector3 convert_velocity(float _vx, float _vy, float _vz){
-	float vx =  _vx;
-	float vy = -_vy;
-	float vz = -_vz;
-	return tf::Vector3(vx, vy, vz);
-};
-
-static tf::Vector3 convert_accel(float _ax, float _ay, float _az){
-	float ax =  _ax;
-	float ay = -_ay;
-	float az = -_az;
-	return tf::Vector3(ax, ay, az);
-};
-
-static tf::Vector3 convert_general_xyz(float _x, float _y, float _z){
-	float x =  _x;
-	float y = -_y;
-	float z = -_z;
-	return tf::Vector3(x, y, z);
-};
-
-static tf::Quaternion convert_attitude_q(tf::Quaternion qo){
+static tf::Quaternion transform_frame_attitude_q(tf::Quaternion qo){
 	double roll = PI, pitch = 0.0 , yaw = 0.0f; 
 	tf::Quaternion qr = tf::createQuaternionFromRPY(roll, pitch, yaw);
 	tf::Quaternion qt = qo * qr; 
 	return qt;
 };
 
-static tf::Vector3 convert_attitude_rpy(float _roll, float _pitch, float _yaw){
+static tf::Vector3 transform_frame_attitude_rpy(float _roll, float _pitch, float _yaw){
 	float roll = _roll + PI;
 	float pitch = _pitch;
 	float yaw = _yaw;
 	return tf::Vector3(roll, pitch, yaw);
 };
 
-static std::array<float, 36> convert_covariance_pose6x6(std::array<float, 36> _covariance){
+static std::array<float, 36> transform_frame_covariance_pose6x6(std::array<float, 36> _covariance){
 
 	std::array<float, 36> rotation = {1 ,0,0,0,0,0,
     					  0,-1,0,0,0,0,
@@ -82,3 +61,20 @@ static std::array<float, 36> convert_covariance_pose6x6(std::array<float, 36> _c
 
 	return covariance;
 };
+
+static std::array<float, 9> transform_frame_covariance_general3x3(std::array<float, 9> _covariance){
+
+	std::array<float, 9> rotation = {1 ,0,0,
+    					 0,-1,0,
+    					 0,0,-1};
+
+   	std::array<float, 9> covariance;							
+	std::array<float, 9> temp;		// temporary matrix = R * C
+
+	// The rotation matrix in this case is a diagonal matrix so R = R^T
+	
+	std::transform(rotation.begin()+1, rotation.end(), _covariance.begin()+1, temp.begin(), std::multiplies<float>());
+	std::transform(temp.begin()+1, temp.end(), rotation.begin()+1, covariance.begin(), std::multiplies<float>());
+
+	return covariance;
+};
diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp
index 4b8dca3ac..c62b0f99a 100644
--- a/mavros/src/plugins/global_position.cpp
+++ b/mavros/src/plugins/global_position.cpp
@@ -148,7 +148,7 @@ class GlobalPositionPlugin : public MavRosPlugin {
 		double northing, easting;
 		std::string zone;
 
-		/** Adapted from gps_umd ROS package @a http://wiki.ros.org/gps_umd
+		/** @note Adapted from gps_umd ROS package @a http://wiki.ros.org/gps_umd
 		 *  Author: Ken Tossell <ken AT tossell DOT net>
 		 */
 		UTM::LLtoUTM(gp_fix->latitude, gp_fix->longitude, northing, easting, zone);
@@ -189,7 +189,11 @@ class GlobalPositionPlugin : public MavRosPlugin {
 
 		if (send_tf) {
 			tf::Transform transform;
-			auto position = UAS::convert_position(pose_cov->pose.pose.position.x,
+
+			/**
+	 	 	 * @note NED->ENU frame conversion
+	 	 	 */
+			auto position = UAS::transform_frame_general_xyz(pose_cov->pose.pose.position.x,
 						pose_cov->pose.pose.position.y,
 						pose_cov->pose.pose.position.z);
 
diff --git a/mavros/src/plugins/imu_pub.cpp b/mavros/src/plugins/imu_pub.cpp
index 4fa639cfa..91aaa6fe9 100644
--- a/mavros/src/plugins/imu_pub.cpp
+++ b/mavros/src/plugins/imu_pub.cpp
@@ -178,12 +178,14 @@ class IMUPubPlugin : public MavRosPlugin {
 
 		auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
 
-		// NED -> ENU (body-fixed)
-		tf::Vector3 attitude = UAS::convert_attitude_rpy(att.roll, att.pitch, att.yaw);
+		/**
+	 	 * @note NED->ENU frame conversion
+	 	 */
+		tf::Vector3 attitude = UAS::transform_frame_attitude_rpy(att.roll, att.pitch, att.yaw);
 		tf::Quaternion orientation = tf::createQuaternionFromRPY(
 				attitude.x(), attitude.y(), attitude.z());
 
-		tf::Vector3 attitude_s = UAS::convert_attitude_rpy(att.rollspeed, att.pitchspeed, att.yawspeed);
+		tf::Vector3 attitude_s = UAS::transform_frame_attitude_rpy(att.rollspeed, att.pitchspeed, att.yawspeed);
 		fill_imu_msg_attitude(imu_msg, orientation,
 				attitude_s.x(), attitude_s.y(), attitude_s.z());
 
@@ -207,11 +209,13 @@ class IMUPubPlugin : public MavRosPlugin {
 
 		auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
 
-		// PX4 NED -> ROS ENU (body-fixed)
+		/**
+	 	 * @note NED->ENU frame conversion
+	 	 */
 		tf::Quaternion orientation(att_q.q1, att_q.q2, att_q.q3, att_q.q4);
-		tf::Quaternion qo = UAS::convert_attitude_q(orientation);
+		tf::Quaternion qo = UAS::transform_frame_attitude_q(orientation);
 
-		tf::Vector3 attitude_s = UAS::convert_attitude_rpy(att_q.rollspeed, att_q.pitchspeed, att_q.yawspeed);
+		tf::Vector3 attitude_s = UAS::transform_frame_attitude_rpy(att_q.rollspeed, att_q.pitchspeed, att_q.yawspeed);
 		fill_imu_msg_attitude(imu_msg, orientation,
 				attitude_s.x(), attitude_s.y(), attitude_s.z());
 
@@ -240,8 +244,11 @@ class IMUPubPlugin : public MavRosPlugin {
 		if (imu_hr.fields_updated & ((7 << 3) | (7 << 0))) {
 			auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
 
-			auto hr_imu_gyro = UAS::convert_general_xyz(imu_hr.xgyro, imu_hr.ygyro, imu_hr.zgyro);
-			auto hr_imu_acc = UAS::convert_general_xyz(imu_hr.xacc, imu_hr.yacc, imu_hr.zacc);
+			/**
+	 		 * @note NED->ENU frame conversion
+	 		 */
+			auto hr_imu_gyro = UAS::transform_frame_general_xyz(imu_hr.xgyro, imu_hr.ygyro, imu_hr.zgyro);
+			auto hr_imu_acc = UAS::transform_frame_general_xyz(imu_hr.xacc, imu_hr.yacc, imu_hr.zacc);
 
 			fill_imu_msg_raw(imu_msg,
 					hr_imu_gyro.x(), hr_imu_gyro.y(), hr_imu_gyro.z(),
@@ -254,8 +261,10 @@ class IMUPubPlugin : public MavRosPlugin {
 		if (imu_hr.fields_updated & (7 << 6)) {
 			auto magn_msg = boost::make_shared<sensor_msgs::MagneticField>();
 
-			// Convert from local NED plane to ENU
-			auto mag_field = UAS::convert_general_xyz(imu_hr.xmag, imu_hr.ymag, imu_hr.zmag);
+			/**
+	 		 * @note NED->ENU frame conversion
+	 		 */
+			auto mag_field = UAS::transform_frame_general_xyz(imu_hr.xmag, imu_hr.ymag, imu_hr.zmag);
 
 			magn_msg->magnetic_field.x = mag_field.x() * MILLIT_TO_TESLA;
 			magn_msg->magnetic_field.y = mag_field.y() * MILLIT_TO_TESLA;
@@ -296,9 +305,15 @@ class IMUPubPlugin : public MavRosPlugin {
 		header.stamp = uas->synchronise_stamp(imu_raw.time_usec);
 		header.frame_id = frame_id;
 
-		/* NOTE: APM send SCALED_IMU data as RAW_IMU */
-		auto raw_imu_gyro = UAS::convert_general_xyz(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro);
-		auto raw_imu_acc = UAS::convert_general_xyz(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc);
+		/**
+		 * @note APM send SCALED_IMU data as RAW_IMU
+		 */
+
+		/**
+	 	 * @note NED->ENU frame conversion
+	 	 */
+		auto raw_imu_gyro = UAS::transform_frame_general_xyz(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro);
+		auto raw_imu_acc = UAS::transform_frame_general_xyz(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc);
 
 		fill_imu_msg_raw(imu_msg,
 				raw_imu_gyro.x() * MILLIRS_TO_RADSEC,
@@ -321,9 +336,11 @@ class IMUPubPlugin : public MavRosPlugin {
 		/* -*- magnetic vector -*- */
 		auto magn_msg = boost::make_shared<sensor_msgs::MagneticField>();
 
-		// Convert from local NED plane to ENU
-		auto mag_field = UAS::convert_general_xyz(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag);
-		
+		/**
+	 	 * @note NED->ENU frame conversion
+	 	 */
+		auto mag_field = UAS::transform_frame_general_xyz(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag);
+
 		magn_msg->magnetic_field.x = mag_field.x() * MILLIT_TO_TESLA;
 		magn_msg->magnetic_field.y = mag_field.y() * MILLIT_TO_TESLA;
 		magn_msg->magnetic_field.z = mag_field.z() * MILLIT_TO_TESLA;
@@ -348,8 +365,11 @@ class IMUPubPlugin : public MavRosPlugin {
 		std_msgs::Header header;
 		header.stamp = uas->synchronise_stamp(imu_raw.time_boot_ms);
 
-		auto raw_imu_gyro = UAS::convert_general_xyz(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro);
-		auto raw_imu_acc = UAS::convert_general_xyz(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc);
+		/**
+	 	 * @note NED->ENU frame conversion
+	 	 */
+		auto raw_imu_gyro = UAS::transform_frame_general_xyz(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro);
+		auto raw_imu_acc = UAS::transform_frame_general_xyz(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc);
 
 		fill_imu_msg_raw(imu_msg,
 				raw_imu_gyro.x() * MILLIRS_TO_RADSEC,
@@ -365,8 +385,10 @@ class IMUPubPlugin : public MavRosPlugin {
 		/* -*- magnetic vector -*- */
 		auto magn_msg = boost::make_shared<sensor_msgs::MagneticField>();
 
-		// Convert from local NED plane to ENU
-		auto mag_field = UAS::convert_general_xyz(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag);
+		/**
+	 	 * @note NED->ENU frame conversion
+	 	 */
+		auto mag_field = UAS::transform_frame_general_xyz(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag);
 
 		magn_msg->magnetic_field.x = mag_field.x() * MILLIT_TO_TESLA;
 		magn_msg->magnetic_field.y = mag_field.y() * MILLIT_TO_TESLA;
diff --git a/mavros/src/plugins/local_position.cpp b/mavros/src/plugins/local_position.cpp
index caceb6708..08f828925 100644
--- a/mavros/src/plugins/local_position.cpp
+++ b/mavros/src/plugins/local_position.cpp
@@ -24,7 +24,7 @@
 namespace mavplugin {
 /**
  * @brief Local position plugin.
- * Publish local position to TF and PositionStamped,
+ * Publish local position to TF and PositionStamped
  */
 class LocalPositionPlugin : public MavRosPlugin {
 public:
@@ -73,8 +73,11 @@ class LocalPositionPlugin : public MavRosPlugin {
 				pos_ned.vx, pos_ned.vy, pos_ned.vz);
 
 		tf::Transform transform;
-		// ENU->NED
-		auto position = UAS::convert_position(pos_ned.x, pos_ned.y, pos_ned.z);
+
+		/**
+	 	 * @note NED->ENU frame conversion
+	 	 */
+		auto position = UAS::transform_frame_general_xyz(pos_ned.x, pos_ned.y, pos_ned.z);
 
 		transform.setOrigin(position);
 		transform.setRotation(uas->get_attitude_orientation());
diff --git a/mavros/src/plugins/safety_area.cpp b/mavros/src/plugins/safety_area.cpp
index ba8cbb0f7..e1cd96afd 100644
--- a/mavros/src/plugins/safety_area.cpp
+++ b/mavros/src/plugins/safety_area.cpp
@@ -99,7 +99,7 @@ class SafetyAreaPlugin : public MavRosPlugin {
 	/* -*- mid-level helpers -*- */
 
 	/**
-	 * Send a safety zone (volume), which is defined by two corners of a cube,
+	 * @brief Send a safety zone (volume), which is defined by two corners of a cube,
 	 * to the FCU.
 	 *
 	 * @note ENU frame.
@@ -110,9 +110,11 @@ class SafetyAreaPlugin : public MavRosPlugin {
 				p1x, p1y, p1z,
 				p2x, p2y, p2z);
 
-		// ENU->NED
-		auto p1 = UAS::convert_position(p1x, p1y, p1z);
-		auto p2 = UAS::convert_position(p2x, p2y, p2z);
+		/**
+	 	 * @note ENU->NED frame conversion
+	 	 */
+		auto p1 = UAS::transform_frame_general_xyz(p1x, p1y, p1z);
+		auto p2 = UAS::transform_frame_general_xyz(p2x, p2y, p2z);
 
 		safety_set_allowed_area(
 				MAV_FRAME_LOCAL_NED, // TODO: use enum from lib
diff --git a/mavros/src/plugins/setpoint_accel.cpp b/mavros/src/plugins/setpoint_accel.cpp
index 72faecb01..fac6637f4 100644
--- a/mavros/src/plugins/setpoint_accel.cpp
+++ b/mavros/src/plugins/setpoint_accel.cpp
@@ -62,9 +62,9 @@ class SetpointAccelerationPlugin : public MavRosPlugin,
 	/* -*- mid-level helpers -*- */
 
 	/**
-	 * Send acceleration/force to FCU acceleration controller
+	 * @brief Send acceleration/force to FCU acceleration controller.
 	 *
-	 * Note: send only AFX AFY AFZ. ENU frame.
+	 * @warning Send only AFX AFY AFZ. ENU frame.
 	 */
 	void send_setpoint_acceleration(const ros::Time &stamp, float afx, float afy, float afz) {
 		/* Documentation start from bit 1 instead 0.
@@ -75,8 +75,10 @@ class SetpointAccelerationPlugin : public MavRosPlugin,
 		if (send_force)
 			ignore_all_except_a_xyz |= (1 << 9);
 
-		// ENU->NED
-		auto accel = UAS::convert_accel(afx, afy, afz);
+		/**
+	 	 * @note ENU->NED frame conversion
+	 	 */
+		auto accel = UAS::transform_frame_general_xyz(afx, afy, afz);
 
 		set_position_target_local_ned(stamp.toNSec() / 1000000,
 				MAV_FRAME_LOCAL_NED,	// TODO: use enum on lib
diff --git a/mavros/src/plugins/setpoint_attitude.cpp b/mavros/src/plugins/setpoint_attitude.cpp
index 6763bcb75..a6d271d63 100644
--- a/mavros/src/plugins/setpoint_attitude.cpp
+++ b/mavros/src/plugins/setpoint_attitude.cpp
@@ -117,19 +117,23 @@ class SetpointAttitudePlugin : public MavRosPlugin,
 	/* -*- mid-level helpers -*- */
 
 	/**
-	 * Send attitude setpoint to FCU attitude controller
+	 * @brief Send attitude setpoint to FCU attitude controller
 	 *
-	 * ENU frame.
+	 * @note ENU frame.
 	 */
 	void send_attitude_transform(const tf::Transform &transform, const ros::Time &stamp) {
-		// Thrust + RPY, also bits noumbering started from 1 in docs
+		/**
+		 * @note Thrust + RPY, also bits numbering started from 1 in docs
+		 */ 
 		const uint8_t ignore_all_except_q = (1 << 6) | (7 << 0);
 		float q[4];
 
 		tf::Quaternion tf_q = transform.getRotation();
-		
-		// ENU->NED
-		auto qt = UAS::convert_attitude_q(tf_q);
+
+		/**
+	 	 * @note ENU->NED frame conversion
+	 	 */
+		auto qt = UAS::transform_frame_attitude_q(tf_q);
 
 		q[0] = qt.w();
 		q[1] = qt.x();
@@ -144,17 +148,21 @@ class SetpointAttitudePlugin : public MavRosPlugin,
 	}
 
 	/**
-	 * Send angular velocity setpoint to FCU attitude controller
+	 * @brief Send angular velocity setpoint to FCU attitude controller
 	 *
-	 * ENU frame.
+	 * @note ENU frame.
 	 */
 	void send_attitude_ang_velocity(const ros::Time &stamp, const float vx, const float vy, const float vz) {
-		// Q + Thrust, also bits noumbering started from 1 in docs
+		/*
+		 * @note Q + Thrust, also bits noumbering started from 1 in docs
+		 */
 		const uint8_t ignore_all_except_rpy = (1 << 7) | (1 << 6);
 		float q[4] = { 1.0, 0.0, 0.0, 0.0 };
 
-		// ENU->NED
-		auto vel = UAS::convert_velocity(vx, vy, vz);
+		/**
+	 	 * @note ENU->NED frame conversion
+	 	 */
+		auto vel = UAS::transform_frame_general_xyz(vx, vy, vz);
 
 		set_attitude_target(stamp.toNSec() / 1000000,
 				ignore_all_except_rpy,
@@ -164,7 +172,7 @@ class SetpointAttitudePlugin : public MavRosPlugin,
 	}
 
 	/**
-	 * Send throttle to FCU attitude controller
+	 * @brief Send throttle to FCU attitude controller
 	 */
 	void send_attitude_throttle(const float throttle) {
 		// Q + RPY
@@ -216,7 +224,9 @@ class SetpointAttitudePlugin : public MavRosPlugin,
 	void throttle_cb(const std_msgs::Float64::ConstPtr &req) {
 		float throttle_normalized = req->data;
 
-		// note: && are lazy, is_normalized() should be called only if reverse_throttle are true.
+		/**
+		 * @note && are lazy, is_normalized() should be called only if reverse_throttle are true.
+		 */
 		if (reverse_throttle && !is_normalized(throttle_normalized, -1.0, 1.0))
 			return;
 		else if (!is_normalized(throttle_normalized, 0.0, 1.0))
diff --git a/mavros/src/plugins/setpoint_position.cpp b/mavros/src/plugins/setpoint_position.cpp
index a963deb26..1d7858d34 100644
--- a/mavros/src/plugins/setpoint_position.cpp
+++ b/mavros/src/plugins/setpoint_position.cpp
@@ -77,23 +77,24 @@ class SetpointPositionPlugin : public MavRosPlugin,
 	/* -*- mid-level helpers -*- */
 
 	/**
-	 * Send transform to FCU position controller
+	 * @brief Send transform to FCU position controller.
 	 *
-	 * Note: send only XYZ, Yaw
+	 * @warning Send only XYZ, Yaw. ENU frame.
 	 */
 	void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) {
 		// ENU frame
 		tf::Vector3 origin = transform.getOrigin();
 		tf::Quaternion q = transform.getRotation();
 
-		/* Documentation start from bit 1 instead 0,
-		 * Ignore velocity and accel vectors, yaw rate
+		/**
+		 * @note Documentation start from bit 1 instead 0;
+		 * Ignore velocity and accel vectors, yaw rate.
 		 */
 		uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);
 
 		if (uas->is_px4()) {
 			/**
-			 * Current PX4 has bug: it cuts throttle if there no velocity sp
+			 * @bug Current PX4 has a bug: it cuts out throttle if there's no velocity SP
 			 * Issue #273.
 			 *
 			 * @todo Revesit this quirk later. Should be fixed in firmware.
@@ -101,9 +102,11 @@ class SetpointPositionPlugin : public MavRosPlugin,
 			ignore_all_except_xyz_y = (1 << 11) | (7 << 6);
 		}
 
-		// ENU->NED
-		auto position = UAS::convert_position(origin.x(), origin.y(), origin.z());
-		auto qt = UAS::convert_attitude_q(q);
+		/**
+	 	 * @note ENU->NED frame conversion
+	 	 */
+		auto position = UAS::transform_frame_general_xyz(origin.x(), origin.y(), origin.z());
+		auto qt = UAS::transform_frame_attitude_q(q);
 
 		set_position_target_local_ned(stamp.toNSec() / 1000000,
 				MAV_FRAME_LOCAL_NED,
diff --git a/mavros/src/plugins/setpoint_velocity.cpp b/mavros/src/plugins/setpoint_velocity.cpp
index 3fce3f615..23a575435 100644
--- a/mavros/src/plugins/setpoint_velocity.cpp
+++ b/mavros/src/plugins/setpoint_velocity.cpp
@@ -57,19 +57,22 @@ class SetpointVelocityPlugin : public MavRosPlugin,
 	/* -*- mid-level helpers -*- */
 
 	/**
-	 * Send velocity to FCU velocity controller
+	 * @brief Send velocity to FCU velocity controller
 	 *
-	 * Note: send only VX VY VZ. ENU frame.
+	 * @warning Send only VX VY VZ. ENU frame.
 	 */
 	void send_setpoint_velocity(const ros::Time &stamp, float vx, float vy, float vz, float yaw_rate) {
-		/* Documentation start from bit 1 instead 0,
-		 * Ignore position and accel vectors, yaw
+		/**
+		 * @note Documentation start from bit 1 instead 0;
+		 * Ignore position and accel vectors, yaw.
 		 */
 		uint16_t ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0);
 
-		// ENU->NED
-		auto vel = UAS::convert_velocity(vx, vy, vz);
-		auto yr = UAS::convert_velocity(0.0, 0.0, yaw_rate);
+		/**
+	 	 * @note ENU->NED frame conversion
+	 	 */
+		auto vel = UAS::transform_frame_general_xyz(vx, vy, vz);
+		auto yr = UAS::transform_frame_general_xyz(0.0, 0.0, yaw_rate);
 
 		set_position_target_local_ned(stamp.toNSec() / 1000000,
 				MAV_FRAME_LOCAL_NED,
diff --git a/mavros_extras/src/plugins/mocap_pose_estimate.cpp b/mavros_extras/src/plugins/mocap_pose_estimate.cpp
index 99ade8e2d..6fe729bb0 100644
--- a/mavros_extras/src/plugins/mocap_pose_estimate.cpp
+++ b/mavros_extras/src/plugins/mocap_pose_estimate.cpp
@@ -26,7 +26,7 @@ namespace mavplugin {
 /**
  * @brief MocapPoseEstimate plugin
  *
- * Sends mountion capture data to FCU.
+ * Sends motion capture data to FCU.
  */
 class MocapPoseEstimatePlugin : public MavRosPlugin
 {
@@ -43,9 +43,15 @@ class MocapPoseEstimatePlugin : public MavRosPlugin
 
 		uas = &uas_;
 
-		mp_nh.param("use_tf", use_tf, false);		// Vicon
-		mp_nh.param("use_pose", use_pose, true);	// Optitrack
+		/**
+	 	 * @note For VICON ROS package, subscribe to TransformStamped topic
+	 	 */
+		mp_nh.param("use_tf", use_tf, false);
 
+		/**
+	 	 * @note For Optitrack ROS package, subscribe to PoseStamped topic
+	 	 */
+		mp_nh.param("use_pose", use_pose, true);
 
 		if (use_tf && !use_pose) {
 			mocap_tf_sub = mp_nh.subscribe("tf", 1, &MocapPoseEstimatePlugin::mocap_tf_cb, this);
@@ -90,17 +96,20 @@ class MocapPoseEstimatePlugin : public MavRosPlugin
 	{
 		float q[4];
 
-		// ENU->NED
+		/**
+	 	 * @note ENU->NED frame conversion
+	 	 */
 		tf::Quaternion qo;
 		quaternionMsgToTF(pose->pose.orientation,qo);
-		auto qt = UAS::convert_attitude_q(qo);
+		auto qt = UAS::transform_frame_attitude_q(qo);
 
 		q[0] = qt.w();
 		q[1] = qt.x();
 		q[2] = qt.y();
 		q[3] = qt.z();
 
-		auto position = UAS::convert_position(pose->pose.position.x,
+		auto position = UAS::transform_frame_general_xyz(
+					pose->pose.position.x,
 					pose->pose.position.y,
 					pose->pose.position.z);
 
@@ -115,23 +124,22 @@ class MocapPoseEstimatePlugin : public MavRosPlugin
 	void mocap_tf_cb(const geometry_msgs::TransformStamped::ConstPtr &trans)
 	{
 		float q[4];
-		q[0] = trans->transform.rotation.y;
-		q[1] = trans->transform.rotation.x;
-		q[2] = trans->transform.rotation.z;
-		q[3] = trans->transform.rotation.w;
-
-		// ENU->NED
+		
+		/**
+	 	 * @note ENU->NED frame conversion
+	 	 */
 		tf::Transform tf;
 		transformMsgToTF(trans->transform,tf);
 		tf::Quaternion qo = tf.getRotation();
-		auto qt = UAS::convert_attitude_q(qo);
+		auto qt = UAS::transform_frame_attitude_q(qo);
 
 		q[0] = qt.w();
 		q[1] = qt.x();
 		q[2] = qt.y();
 		q[3] = qt.z();
 
-		auto position = UAS::convert_position(trans->transform.translation.x,
+		auto position = UAS::transform_frame_general_xyz(
+					trans->transform.translation.x,
 					trans->transform.translation.y,
 					trans->transform.translation.z);
 
diff --git a/mavros_extras/src/plugins/px4flow.cpp b/mavros_extras/src/plugins/px4flow.cpp
index 566f4b2bf..8a6ebb9a2 100644
--- a/mavros_extras/src/plugins/px4flow.cpp
+++ b/mavros_extras/src/plugins/px4flow.cpp
@@ -41,7 +41,9 @@ class PX4FlowPlugin : public MavRosPlugin {
 
 		flow_nh.param<std::string>("frame_id", frame_id, "px4flow");
 
-		//Default rangefinder is Maxbotix HRLV-EZ4
+		/**
+		 * @note Default rangefinder is Maxbotix HRLV-EZ4
+		 */
 		flow_nh.param("ranger_fov", ranger_fov, 0.0);	// TODO
 		flow_nh.param("ranger_min_range", ranger_min_range, 0.3);
 		flow_nh.param("ranger_max_range", ranger_max_range, 5.0);
@@ -80,8 +82,10 @@ class PX4FlowPlugin : public MavRosPlugin {
 		header.stamp = ros::Time::now();
 		header.frame_id = frame_id;
 
-		/* Raw message with axes mapped to ROS conventions and temp in degrees celsius
-		 * The optical flow camera is essentially an angular sensor, so conversion is like
+		/**
+		 * @note Raw message with axes mapped to ROS conventions and temp in degrees celsius.
+		 *
+		 * @note The optical flow camera is essentially an angular sensor, so conversion is like
 		 * gyroscope. (body-fixed NED -> ENU)
 		 */
 
@@ -91,14 +95,24 @@ class PX4FlowPlugin : public MavRosPlugin {
 
 		flow_rad_msg->integration_time_us = flow_rad.integration_time_us;
 
-		// NED->ENU
-		auto position = UAS::convert_position(flow_rad.integrated_x, flow_rad.integrated_y, 0.0);
+		/**
+	 	 * @note ENU->NED frame conversion
+	 	 */
+		auto position = UAS::transform_frame_general_xyz(
+						flow_rad.integrated_x,
+						flow_rad.integrated_y,
+						0.0);
 
 		flow_rad_msg->integrated_x = position.x();
 		flow_rad_msg->integrated_y = position.y();
 
-		// NED->ENU
-		auto flow_gyro = UAS::convert_general_xyz(flow_rad.integrated_xgyro, flow_rad.integrated_ygyro, flow_rad.integrated_zgyro);
+		/**
+	 	 * @note ENU->NED frame conversion
+	 	 */
+		auto flow_gyro = UAS::transform_frame_general_xyz(
+						flow_rad.integrated_xgyro,
+						flow_rad.integrated_ygyro,
+						flow_rad.integrated_zgyro);
 
 		flow_rad_msg->integrated_xgyro = flow_gyro.x();
 		flow_rad_msg->integrated_ygyro = flow_gyro.y();	
@@ -122,6 +136,14 @@ class PX4FlowPlugin : public MavRosPlugin {
 		temp_pub.publish(temp_msg);
 
 		// Rangefinder
+		/**
+		 * @todo: use distance_sensor plugin only to publish this data
+		 * (which receives DISTANCE_SENSOR msg with multiple rangefinder
+		 * sensors data)
+		 *
+		 * @todo: suggest modification on MAVLink OPTICAL_FLOW_RAD msg
+		 * which removes sonar data fields from it
+		 */
 		auto range_msg = boost::make_shared<sensor_msgs::Range>();
 
 		range_msg->header = header;
diff --git a/mavros_extras/src/plugins/vision_pose_estimate.cpp b/mavros_extras/src/plugins/vision_pose_estimate.cpp
index 62c645021..9f927ae19 100644
--- a/mavros_extras/src/plugins/vision_pose_estimate.cpp
+++ b/mavros_extras/src/plugins/vision_pose_estimate.cpp
@@ -27,7 +27,7 @@ namespace mavplugin {
  * @brief Vision pose estimate plugin
  *
  * Send pose estimation from various vision estimators
- * to FCU position controller.
+ * to FCU position and attitude estimators.
  *
  */
 class VisionPoseEstimatePlugin : public MavRosPlugin,
@@ -103,17 +103,20 @@ class VisionPoseEstimatePlugin : public MavRosPlugin,
 	/* -*- mid-level helpers -*- */
 
 	/**
-	 * Send vision estimate transform to FCU position controller
+	 * @brief Send vision estimate transform to FCU position controller
 	 */
 	void send_vision_transform(const tf::Transform &transform, const ros::Time &stamp) {
-		// origin and RPY in ENU frame
+		/**
+		 * @note origin and RPY in ENU frame
+		 */
 		tf::Vector3 origin = transform.getOrigin();
 		double roll, pitch, yaw;
 		tf::Matrix3x3 orientation(transform.getBasis());
 		orientation.getRPY(roll, pitch, yaw);
 
-		/* Issue #60.
-		 * Note: this now affects pose callbacks too, but i think its not big deal.
+		/**
+		 * @warning Issue #60.
+		 * This now affects pose callbacks too.
 		 */
 		if (last_transform_stamp == stamp) {
 			ROS_DEBUG_THROTTLE_NAMED(10, "vision_pose", "Vision: Same transform as last one, dropped.");
@@ -121,9 +124,11 @@ class VisionPoseEstimatePlugin : public MavRosPlugin,
 		}
 		last_transform_stamp = stamp;
 
-		// ENU->NED
-		auto position = UAS::convert_position(origin.x(), origin.y(), origin.z());
-		tf::Vector3 rpy = UAS::convert_attitude_rpy(roll, pitch, yaw);
+		/**
+	 	 * @note ENU->NED frame conversion
+	 	 */
+		auto position = UAS::transform_frame_general_xyz(origin.x(), origin.y(), origin.z());
+		tf::Vector3 rpy = UAS::transform_frame_attitude_rpy(roll, pitch, yaw);
 		
 		vision_position_estimate(stamp.toNSec() / 1000,
 				position.x(), position.y(), position.z(),
diff --git a/mavros_extras/src/plugins/vision_speed_estimate.cpp b/mavros_extras/src/plugins/vision_speed_estimate.cpp
index 6c663ad9e..3241333f6 100644
--- a/mavros_extras/src/plugins/vision_speed_estimate.cpp
+++ b/mavros_extras/src/plugins/vision_speed_estimate.cpp
@@ -25,8 +25,8 @@ namespace mavplugin {
 /**
  * @brief Vision speed estimate plugin
  *
- * Send speed estimation from various vision estimators
- * to FCU position controller.
+ * Send velocity estimation from various vision estimators
+ * to FCU position and attitude estimators.
  *
  */
 class VisionSpeedEstimatePlugin : public MavRosPlugin {
@@ -70,14 +70,21 @@ class VisionSpeedEstimatePlugin : public MavRosPlugin {
 		UAS_FCU(uas)->send_message(&msg);
 	}
 
+	/**
+	 * @todo Suggest modification on PX4 firmware to MAVLINK VISION_SPEED_ESTIMATE
+	 * msg name, which should be called instead VISION_VELOCITY_ESTIMATE
+	 */
+
 	/* -*- mid-level helpers -*- */
 
 	/**
-	 * Send vision speed estimate to FCU velocity controller
+	 * @brief Send vision speed estimate to FCU velocity controller
 	 */
 	void send_vision_speed(float vx, float vy, float vz, const ros::Time &stamp) {
-		// ENU->NED
-		auto vel = UAS::convert_velocity(vx, vy, vz);
+		/**
+	 	 * @note ENU->NED frame conversion
+	 	 */
+		auto vel = UAS::transform_frame_general_xyz(vx, vy, vz);
 
 		vision_speed_estimate(stamp.toNSec() / 1000,
 				vel.x(), vel.y(), vel.z());