diff --git a/msg/att_pos_mocap.msg b/msg/att_pos_mocap.msg
new file mode 100644
index 0000000000..52bc04b5aa
--- /dev/null
+++ b/msg/att_pos_mocap.msg
@@ -0,0 +1,10 @@
+uint32 id			# ID of the estimator, commonly the component ID of the incoming message
+
+uint64 timestamp_boot		# time of this estimate, in microseconds since system start
+uint64 timestamp_computer	# timestamp provided by the companion computer, in us
+
+float32[4] q			# Estimated attitude as quaternion
+
+float32 x			# X position in meters in NED earth-fixed frame
+float32 y			# Y position in meters in NED earth-fixed frame
+float32 z			# Z position in meters in NED earth-fixed frame (negative altitude)
diff --git a/msg/vehicle_vicon_position.msg b/msg/vehicle_vicon_position.msg
deleted file mode 100644
index 1626d85383..0000000000
--- a/msg/vehicle_vicon_position.msg
+++ /dev/null
@@ -1,10 +0,0 @@
-uint64 timestamp		# time of this estimate, in microseconds since system start
-bool valid			# true if position satisfies validity criteria of estimator
-
-float32 x			# X position in meters in NED earth-fixed frame
-float32 y			# Y position in meters in NED earth-fixed frame
-float32 z			# Z position in meters in NED earth-fixed frame (negative altitude)
-float32 roll
-float32 pitch
-float32 yaw
-float32[4] q			# Attitude as quaternion
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index afd8706dbf..a320ffe518 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -63,6 +63,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vision_position_estimate.h>
+#include <uORB/topics/att_pos_mocap.h>
 #include <drivers/drv_hrt.h>
 
 #include <lib/mathlib/mathlib.h>
@@ -261,6 +262,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	/* subscribe to vision estimate */
 	int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
 
+	/* subscribe to mocap data */
+	int mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
+
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
@@ -291,6 +295,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	R_decl.identity();
 
 	struct vision_position_estimate_s vision {};
+	struct att_pos_mocap_s mocap {};
 
 	/* register the perf counter */
 	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
@@ -445,11 +450,30 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					bool vision_updated = false;
 					orb_check(vision_sub, &vision_updated);
 
+					bool mocap_updated = false;
+					orb_check(mocap_sub, &mocap_updated);
+
 					if (vision_updated) {
 						orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision);
 					}
 
-					if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) {
+					if (mocap_updated) {
+						orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap);
+					}
+
+					if (mocap.timestamp_boot > 0 && (hrt_elapsed_time(&mocap.timestamp_boot) < 500000)) {
+
+						math::Quaternion q(mocap.q);
+						math::Matrix<3, 3> Rmoc = q.to_dcm();
+
+						math::Vector<3> v(1.0f, 0.0f, 0.4f);
+
+						math::Vector<3> vn = Rmoc.transposed() * v; //Rmoc is Rwr (robot respect to world) while v is respect to world. Hence Rmoc must be transposed having (Rwr)' * Vw
+											    // Rrw * Vw = vn. This way we have consistency
+						z_k[6] = vn(0);
+						z_k[7] = vn(1);
+						z_k[8] = vn(2);
+					}else if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) {
 
 						math::Quaternion q(vision.q);
 						math::Matrix<3, 3> Rvis = q.to_dcm();
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 1b2689e6bb..1c157c6e84 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -54,7 +54,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/att_pos_mocap.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
@@ -1187,64 +1187,65 @@ protected:
 };
 
 
-class MavlinkStreamViconPositionEstimate : public MavlinkStream
+class MavlinkStreamAttPosMocap : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamViconPositionEstimate::get_name_static();
+		return MavlinkStreamAttPosMocap::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "VICON_POSITION_ESTIMATE";
+		return "ATT_POS_MOCAP";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
+		return MAVLINK_MSG_ID_ATT_POS_MOCAP;
 	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamViconPositionEstimate(mavlink);
+		return new MavlinkStreamAttPosMocap(mavlink);
 	}
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *_pos_sub;
-	uint64_t _pos_time;
+	MavlinkOrbSubscription *_mocap_sub;
+	uint64_t _mocap_time;
 
 	/* do not allow top copying this class */
-	MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &);
-	MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &);
+	MavlinkStreamAttPosMocap(MavlinkStreamAttPosMocap &);
+	MavlinkStreamAttPosMocap& operator = (const MavlinkStreamAttPosMocap &);
 
 protected:
-	explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))),
-		_pos_time(0)
+	explicit MavlinkStreamAttPosMocap(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_mocap_sub(_mavlink->add_orb_subscription(ORB_ID(att_pos_mocap))),
+		_mocap_time(0)
 	{}
 
 	void send(const hrt_abstime t)
 	{
-		struct vehicle_vicon_position_s pos;
+		struct att_pos_mocap_s mocap;
 
-		if (_pos_sub->update(&_pos_time, &pos)) {
-			mavlink_vicon_position_estimate_t msg;
+		if (_mocap_sub->update(&_mocap_time, &mocap)) {
+			mavlink_att_pos_mocap_t msg;
 
-			msg.usec = pos.timestamp;
-			msg.x = pos.x;
-			msg.y = pos.y;
-			msg.z = pos.z;
-			msg.roll = pos.roll;
-			msg.pitch = pos.pitch;
-			msg.yaw = pos.yaw;
+			msg.time_usec = mocap.timestamp_boot;
+			msg.q[0] = mocap.q[0];
+			msg.q[1] = mocap.q[1];
+			msg.q[2] = mocap.q[2];
+			msg.q[3] = mocap.q[3];
+			msg.x = mocap.x;
+			msg.y = mocap.y;
+			msg.z = mocap.z;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg);
+			_mavlink->send_message(MAVLINK_MSG_ID_ATT_POS_MOCAP, &msg);
 		}
 	}
 };
@@ -2277,7 +2278,7 @@ const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static),
 	new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
 	new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
-	new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
+	new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static),
 	new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static),
 	new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
 	new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 4e3da6c7c0..575321c4d0 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -118,7 +118,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_rates_sp_pub(nullptr),
 	_force_sp_pub(nullptr),
 	_pos_sp_triplet_pub(nullptr),
-	_vicon_position_pub(nullptr),
+	_att_pos_mocap_pub(nullptr),
 	_vision_position_pub(nullptr),
 	_telemetry_status_pub(nullptr),
 	_rc_pub(nullptr),
@@ -169,8 +169,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_set_mode(msg);
 		break;
 
-	case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
-		handle_message_vicon_position_estimate(msg);
+	case MAVLINK_MSG_ID_ATT_POS_MOCAP:
+		handle_message_att_pos_mocap(msg);
 		break;
 
 	case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
@@ -556,27 +556,34 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
 }
 
 void
-MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
+MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
 {
-	mavlink_vicon_position_estimate_t pos;
-	mavlink_msg_vicon_position_estimate_decode(msg, &pos);
+	mavlink_att_pos_mocap_t mocap;
+	mavlink_msg_att_pos_mocap_decode(msg, &mocap);
 
-	struct vehicle_vicon_position_s vicon_position;
-	memset(&vicon_position, 0, sizeof(vicon_position));
+	struct att_pos_mocap_s att_pos_mocap;
+	memset(&att_pos_mocap, 0, sizeof(att_pos_mocap));
 
-	vicon_position.timestamp = hrt_absolute_time();
-	vicon_position.x = pos.x;
-	vicon_position.y = pos.y;
-	vicon_position.z = pos.z;
-	vicon_position.roll = pos.roll;
-	vicon_position.pitch = pos.pitch;
-	vicon_position.yaw = pos.yaw;
+	// Use the component ID to identify the mocap system
+	att_pos_mocap.id = msg->compid;
 
-	if (_vicon_position_pub == nullptr) {
-		_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
+	att_pos_mocap.timestamp_boot = hrt_absolute_time(); // Monotonic time
+	att_pos_mocap.timestamp_computer = sync_stamp(mocap.time_usec); // Synced time
+
+	att_pos_mocap.q[0] = mocap.q[0];
+	att_pos_mocap.q[1] = mocap.q[1];
+	att_pos_mocap.q[2] = mocap.q[2];
+	att_pos_mocap.q[3] = mocap.q[3];
+
+	att_pos_mocap.x = mocap.x;
+	att_pos_mocap.y = mocap.y;
+	att_pos_mocap.z = mocap.z;
+
+	if (_att_pos_mocap_pub == nullptr) {
+		_att_pos_mocap_pub = orb_advertise(ORB_ID(att_pos_mocap), &att_pos_mocap);
 
 	} else {
-		orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position);
+		orb_publish(ORB_ID(att_pos_mocap), _att_pos_mocap_pub, &att_pos_mocap);
 	}
 }
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 8fffad4c3e..2709a10915 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -58,7 +58,7 @@
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/att_pos_mocap.h>
 #include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
@@ -119,7 +119,7 @@ private:
 	void handle_message_optical_flow_rad(mavlink_message_t *msg);
 	void handle_message_hil_optical_flow(mavlink_message_t *msg);
 	void handle_message_set_mode(mavlink_message_t *msg);
-	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
+	void handle_message_att_pos_mocap(mavlink_message_t *msg);
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
 	void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
@@ -174,7 +174,7 @@ private:
 	orb_advert_t _rates_sp_pub;
 	orb_advert_t _force_sp_pub;
 	orb_advert_t _pos_sp_triplet_pub;
-	orb_advert_t _vicon_position_pub;
+	orb_advert_t _att_pos_mocap_pub;
 	orb_advert_t _vision_position_pub;
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk
index 45c8762996..56aa3fad04 100644
--- a/src/modules/position_estimator_inav/module.mk
+++ b/src/modules/position_estimator_inav/module.mk
@@ -42,5 +42,5 @@ SRCS		 	= position_estimator_inav_main.c \
 
 MODULE_STACKSIZE = 1200
 
-EXTRACFLAGS = -Wframe-larger-than=3500
+EXTRACFLAGS = -Wframe-larger-than=3800
 
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 8654a7cb11..eaad4e3156 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -36,6 +36,7 @@
  * Model-identification based position estimator for multirotors
  *
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Nuno Marques <n.marques21@hotmail.com>
  */
 
 #include <unistd.h>
@@ -64,6 +65,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vision_position_estimate.h>
+#include <uORB/topics/att_pos_mocap.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/optical_flow.h>
 #include <mavlink/mavlink_log.h>
@@ -87,6 +89,7 @@ static int position_estimator_inav_task; /**< Handle of deamon task / thread */
 static bool verbose_mode = false;
 
 static const hrt_abstime vision_topic_timeout = 500000;	// Vision topic timeout = 0.5s
+static const hrt_abstime mocap_topic_timeout = 500000;	// Mocap topic timeout = 0.5s
 static const hrt_abstime gps_topic_timeout = 500000;		// GPS topic timeout = 0.5s
 static const hrt_abstime flow_topic_timeout = 1000000;	// optical flow topic timeout = 1s
 static const hrt_abstime sonar_timeout = 150000;	// sonar timeout = 150ms
@@ -241,6 +244,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float eph_vision = 0.2f;
 	float epv_vision = 0.2f;
 
+	float eph_mocap = 0.05f;
+	float epv_mocap = 0.05f;
+
 	float x_est_prev[2], y_est_prev[2], z_est_prev[2];
 	memset(x_est_prev, 0, sizeof(x_est_prev));
 	memset(y_est_prev, 0, sizeof(y_est_prev));
@@ -267,6 +273,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	uint16_t gps_updates = 0;
 	uint16_t attitude_updates = 0;
 	uint16_t flow_updates = 0;
+	uint16_t vision_updates = 0;
+	uint16_t mocap_updates = 0;
 
 	hrt_abstime updates_counter_start = hrt_absolute_time();
 	hrt_abstime pub_last = hrt_absolute_time();
@@ -291,6 +299,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		{ 0.0f, 0.0f },		// D (pos, vel)
 	};
 
+	float corr_mocap[3][1] = {
+		{ 0.0f },			// N (pos)
+		{ 0.0f },			// E (pos)
+		{ 0.0f },			// D (pos)
+	};
+
 	float corr_sonar = 0.0f;
 	float corr_sonar_filtered = 0.0f;
 
@@ -306,7 +320,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	bool sonar_valid = false;		// sonar is valid
 	bool flow_valid = false;		// flow is valid
 	bool flow_accurate = false;		// flow should be accurate (this flag not updated if flow_valid == false)
-	bool vision_valid = false;
+	bool vision_valid = false;		// vision is valid
+	bool mocap_valid = false;		// mocap is valid
 
 	/* declare and safely initialize all structs */
 	struct actuator_controls_s actuator;
@@ -327,6 +342,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	memset(&flow, 0, sizeof(flow));
 	struct vision_position_estimate_s vision;
 	memset(&vision, 0, sizeof(vision));
+	struct att_pos_mocap_s mocap;
+	memset(&mocap, 0, sizeof(mocap));
 	struct vehicle_global_position_s global_pos;
 	memset(&global_pos, 0, sizeof(global_pos));
 
@@ -339,6 +356,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
 	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
+	int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
 	int home_position_sub = orb_subscribe(ORB_ID(home_position));
 
 	/* advertise */
@@ -699,9 +717,36 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						corr_vision[2][1] = 0.0f - z_est[1];
 					}
 
+					vision_updates++;
 				}
 			}
 
+			/* vehicle mocap position */
+			orb_check(att_pos_mocap_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap);
+
+				/* reset position estimate on first mocap update */
+				if (!mocap_valid) {
+					x_est[0] = mocap.x;
+					y_est[0] = mocap.y;
+					z_est[0] = mocap.z;
+
+					mocap_valid = true;
+
+					warnx("MOCAP data valid");
+					mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid");
+				}
+
+				/* calculate correction for position */
+				corr_mocap[0][0] = mocap.x - x_est[0];
+				corr_mocap[1][0] = mocap.y - y_est[0];
+				corr_mocap[2][0] = mocap.z - z_est[0];
+
+				mocap_updates++;
+			}
+
 			/* vehicle GPS position */
 			orb_check(vehicle_gps_position_sub, &updated);
 
@@ -832,6 +877,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			mavlink_log_info(mavlink_fd, "[inav] VISION timeout");
 		}
 
+		/* check for timeout on mocap topic */
+		if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) {
+			mocap_valid = false;
+			warnx("MOCAP timeout");
+			mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout");
+		}
+
 		/* check for sonar measurement timeout */
 		if (sonar_valid && (t > (sonar_time + sonar_timeout))) {
 			corr_sonar = 0.0f;
@@ -856,10 +908,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		/* use VISION if it's valid and has a valid weight parameter */
 		bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;
 		bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
+		/* use MOCAP if it's valid and has a valid weight parameter */
+		bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W;
 		/* use flow if it's valid and (accurate or no GPS available) */
 		bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
 
-		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy;
+		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
 
 		bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
 
@@ -883,6 +937,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		float w_xy_vision_v = params.w_xy_vision_v;
 		float w_z_vision_p = params.w_z_vision_p;
 
+		float w_mocap_p = params.w_mocap_p;
+
 		/* reduce GPS weight if optical flow is good */
 		if (use_flow && flow_accurate) {
 			w_xy_gps_p *= params.w_gps_flow;
@@ -940,6 +996,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p;
 		}
 
+		/* accelerometer bias correction for MOCAP (use buffered rotation matrix) */
+		accel_bias_corr[0] = 0.0f;
+		accel_bias_corr[1] = 0.0f;
+		accel_bias_corr[2] = 0.0f;
+
+		if (use_mocap) {
+			accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p;
+			accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p;
+			accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p;
+		}
+
 		/* transform error vector from NED frame to body frame */
 		for (int i = 0; i < 3; i++) {
 			float c = 0.0f;
@@ -1001,11 +1068,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p);
 		}
 
+		if (use_mocap) {
+			epv = fminf(epv, epv_mocap);
+			inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p);
+		}
+
 		if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
-			write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+			write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);										
 			memcpy(z_est, z_est_prev, sizeof(z_est));
 			memset(corr_gps, 0, sizeof(corr_gps));
 			memset(corr_vision, 0, sizeof(corr_vision));
+			memset(corr_mocap, 0, sizeof(corr_mocap));
 			corr_baro = 0;
 
 		} else {
@@ -1055,12 +1128,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				}
 			}
 
+			if (use_mocap) {
+				eph = fminf(eph, eph_mocap);
+
+				inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p);
+				inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p);
+			}
+
 			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
 				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
 				memcpy(x_est, x_est_prev, sizeof(x_est));
 				memcpy(y_est, y_est_prev, sizeof(y_est));
 				memset(corr_gps, 0, sizeof(corr_gps));
 				memset(corr_vision, 0, sizeof(corr_vision));
+				memset(corr_mocap, 0, sizeof(corr_mocap));
 				memset(corr_flow, 0, sizeof(corr_flow));
 
 			} else {
@@ -1078,18 +1159,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (t > updates_counter_start + updates_counter_len) {
 				float updates_dt = (t - updates_counter_start) * 0.000001f;
 				warnx(
-					"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
+					"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s",
 					(double)(accel_updates / updates_dt),
 					(double)(baro_updates / updates_dt),
 					(double)(gps_updates / updates_dt),
 					(double)(attitude_updates / updates_dt),
-					(double)(flow_updates / updates_dt));
+					(double)(flow_updates / updates_dt),
+					(double)(vision_updates / updates_dt),
+					(double)(mocap_updates / updates_dt));
 				updates_counter_start = t;
 				accel_updates = 0;
 				baro_updates = 0;
 				gps_updates = 0;
 				attitude_updates = 0;
 				flow_updates = 0;
+				vision_updates = 0;
+				mocap_updates = 0;
 			}
 		}
 
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index 382e9e46d7..a9ddafc0de 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -140,6 +140,18 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f);
  */
 PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
 
+/**
+ * Weight for mocap system
+ *
+ * Weight (cutoff frequency) for mocap position measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
+
+ PARAM_DEFINE_FLOAT(INAV_W_MOC_P, 10.0f);
+
 /**
  * XY axis weight for optical flow
  *
@@ -312,6 +324,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
 	h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
 	h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P");
 	h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V");
+	h->w_mocap_p = param_find("INAV_W_MOC_P");
 	h->w_xy_flow = param_find("INAV_W_XY_FLOW");
 	h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
 	h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
@@ -339,6 +352,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 	param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
 	param_get(h->w_xy_vision_p, &(p->w_xy_vision_p));
 	param_get(h->w_xy_vision_v, &(p->w_xy_vision_v));
+	param_get(h->w_mocap_p, &(p->w_mocap_p));
 	param_get(h->w_xy_flow, &(p->w_xy_flow));
 	param_get(h->w_xy_res_v, &(p->w_xy_res_v));
 	param_get(h->w_gps_flow, &(p->w_gps_flow));
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index 51bbda412a..d6cb9ed410 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -51,6 +51,7 @@ struct position_estimator_inav_params {
 	float w_xy_gps_v;
 	float w_xy_vision_p;
 	float w_xy_vision_v;
+	float w_mocap_p;
 	float w_xy_flow;
 	float w_xy_res_v;
 	float w_gps_flow;
@@ -76,6 +77,7 @@ struct position_estimator_inav_param_handles {
 	param_t w_xy_gps_v;
 	param_t w_xy_vision_p;
 	param_t w_xy_vision_v;
+	param_t w_mocap_p;
 	param_t w_xy_flow;
 	param_t w_xy_res_v;
 	param_t w_gps_flow;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index acc4771968..ef50fe7e2d 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -80,7 +80,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/satellite_info.h>
-#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/att_pos_mocap.h>
 #include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -1071,7 +1071,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_local_position_setpoint_s local_pos_sp;
 		struct vehicle_global_position_s global_pos;
 		struct position_setpoint_triplet_s triplet;
-		struct vehicle_vicon_position_s vicon_pos;
+		struct att_pos_mocap_s att_pos_mocap;
 		struct vision_position_estimate_s vision_pos;
 		struct optical_flow_s flow;
 		struct rc_channels_s rc;
@@ -1127,7 +1127,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_EST0_s log_EST0;
 			struct log_EST1_s log_EST1;
 			struct log_PWR_s log_PWR;
-			struct log_VICN_s log_VICN;
+			struct log_MOCP_s log_MOCP;
 			struct log_VISN_s log_VISN;
 			struct log_GS0A_s log_GS0A;
 			struct log_GS0B_s log_GS0B;
@@ -1162,7 +1162,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int triplet_sub;
 		int gps_pos_sub;
 		int sat_info_sub;
-		int vicon_pos_sub;
+		int att_pos_mocap_sub;
 		int vision_pos_sub;
 		int flow_sub;
 		int rc_sub;
@@ -1197,7 +1197,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.local_pos_sp_sub = -1;
 	subs.global_pos_sub = -1;
 	subs.triplet_sub = -1;
-	subs.vicon_pos_sub = -1;
+	subs.att_pos_mocap_sub = -1;
 	subs.vision_pos_sub = -1;
 	subs.flow_sub = -1;
 	subs.rc_sub = -1;
@@ -1681,16 +1681,17 @@ int sdlog2_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/* --- VICON POSITION --- */
-		if (copy_if_updated(ORB_ID(vehicle_vicon_position), &subs.vicon_pos_sub, &buf.vicon_pos)) {
-			log_msg.msg_type = LOG_VICN_MSG;
-			log_msg.body.log_VICN.x = buf.vicon_pos.x;
-			log_msg.body.log_VICN.y = buf.vicon_pos.y;
-			log_msg.body.log_VICN.z = buf.vicon_pos.z;
-			log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch;
-			log_msg.body.log_VICN.roll = buf.vicon_pos.roll;
-			log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
-			LOGBUFFER_WRITE_AND_COUNT(VICN);
+		/* --- MOCAP ATTITUDE AND POSITION --- */
+		if (copy_if_updated(ORB_ID(att_pos_mocap), &subs.att_pos_mocap_sub, &buf.att_pos_mocap)) {
+			log_msg.msg_type = LOG_MOCP_MSG;
+			log_msg.body.log_MOCP.qw = buf.att_pos_mocap.q[0];
+			log_msg.body.log_MOCP.qx = buf.att_pos_mocap.q[1];
+			log_msg.body.log_MOCP.qy = buf.att_pos_mocap.q[2];
+			log_msg.body.log_MOCP.qz = buf.att_pos_mocap.q[3];
+			log_msg.body.log_MOCP.x = buf.att_pos_mocap.x;
+			log_msg.body.log_MOCP.y = buf.att_pos_mocap.y;
+			log_msg.body.log_MOCP.z = buf.att_pos_mocap.z;
+			LOGBUFFER_WRITE_AND_COUNT(MOCP);
 		}
 
 		/* --- VISION POSITION --- */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 9cf37683ae..f665eed2a2 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -321,15 +321,16 @@ struct log_PWR_s {
 	uint8_t high_power_rail_overcurrent;
 };
 
-/* --- VICN - VICON POSITION --- */
-#define LOG_VICN_MSG 25
-struct log_VICN_s {
+/* --- MOCP - MOCAP ATTITUDE AND POSITION --- */
+#define LOG_MOCP_MSG 25
+struct log_MOCP_s {
+	float qw;
+	float qx;
+	float qy;
+	float qz;
 	float x;
 	float y;
 	float z;
-	float roll;
-	float pitch;
-	float yaw;
 };
 
 /* --- GS0A - GPS SNR #0, SAT GROUP A --- */
@@ -427,10 +428,10 @@ struct log_VISN_s {
 	float vx;
 	float vy;
 	float vz;
+	float qw;
 	float qx;
 	float qy;
 	float qz;
-	float qw;
 };
 
 /* --- ENCODERS - ENCODER DATA --- */
@@ -523,8 +524,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(EST0, "ffffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"),
 	LOG_FORMAT(EST1, "ffffffffffffffff",	"s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
 	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
-	LOG_FORMAT(VICN, "ffffff",		"X,Y,Z,Roll,Pitch,Yaw"),
-	LOG_FORMAT(VISN, "ffffffffff",		"X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"),
+	LOG_FORMAT(MOCP, "fffffff",		"QuatW,QuatX,QuatY,QuatZ,X,Y,Z"),
+	LOG_FORMAT(VISN, "ffffffffff",		"X,Y,Z,VX,VY,VZ,QuatW,QuatX,QuatY,QuatZ"),
 	LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 9f980eebea..de628b3f86 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -108,8 +108,8 @@ ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
 #include "topics/vehicle_local_position.h"
 ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s);
 
-#include "topics/vehicle_vicon_position.h"
-ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
+#include "topics/att_pos_mocap.h"
+ORB_DEFINE(att_pos_mocap, struct att_pos_mocap_s);
 
 #include "topics/vehicle_rates_setpoint.h"
 ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);