diff --git a/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow.post b/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow.post
index 868f402651e55d5a900833419a5620f30141f0dc..3bdadbc7cb27a8f9dbb571fe1ffafed386fccb25 100644
--- a/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow.post
+++ b/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow.post
@@ -1,3 +1,3 @@
 
 # shellcheck disable=SC2154
-mavlink stream -r 10 -s VISION_POSITION_ESTIMATE -u $udp_gcs_port_local
+mavlink stream -r 50 -s DISTANCE_SENSOR -u $udp_gcs_port_local
diff --git a/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision.post b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision.post
index 868f402651e55d5a900833419a5620f30141f0dc..80cf3d5fa967345f4d640e033bfd6cfc95aae135 100644
--- a/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision.post
+++ b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision.post
@@ -1,3 +1,3 @@
 
 # shellcheck disable=SC2154
-mavlink stream -r 10 -s VISION_POSITION_ESTIMATE -u $udp_gcs_port_local
+mavlink stream -r 30 -s ODOMETRY -u $udp_gcs_port_local
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index f9b26814a3b8c63aa79c8fc82fe2487d87a49d37..002fb2a2855b6d987b001144c855e36956f7dea8 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -283,6 +283,7 @@ private:
 
 	uORB::Publication<vehicle_local_position_s> _vehicle_local_position_pub;
 	uORB::Publication<vehicle_global_position_s> _vehicle_global_position_pub;
+	uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub;
 
 	Ekf _ekf;
 
@@ -509,6 +510,7 @@ Ekf2::Ekf2():
 	_perf_ekf_update(perf_alloc_once(PC_ELAPSED, "EKF2 update")),
 	_vehicle_local_position_pub(ORB_ID(vehicle_local_position)),
 	_vehicle_global_position_pub(ORB_ID(vehicle_global_position)),
+	_vehicle_odometry_pub(ORB_ID(vehicle_odometry)),
 	_params(_ekf.getParamHandle()),
 	_obs_dt_min_ms(_params->sensor_interval_min_ms),
 	_mag_delay_ms(_params->mag_delay_ms),
@@ -1297,7 +1299,13 @@ void Ekf2::run()
 				// generate vehicle local position data
 				vehicle_local_position_s &lpos = _vehicle_local_position_pub.get();
 
+				// generate vehicle odometry data
+				vehicle_odometry_s &odom = _vehicle_odometry_pub.get();
+
 				lpos.timestamp = now;
+				odom.timestamp = lpos.timestamp;
+
+				odom.local_frame = odom.LOCAL_FRAME_NED;
 
 				// Position of body origin in local NED frame
 				float position[3];
@@ -1308,6 +1316,11 @@ void Ekf2::run()
 				lpos.y = (_ekf.local_position_is_valid()) ? position[1] : 0.0f;
 				lpos.z = position[2];
 
+				// Vehicle odometry position
+				odom.x = lpos.x;
+				odom.y = lpos.y;
+				odom.z = lpos.z;
+
 				// Velocity of body origin in local NED frame (m/s)
 				float velocity[3];
 				_ekf.get_velocity(velocity);
@@ -1315,6 +1328,11 @@ void Ekf2::run()
 				lpos.vy = velocity[1];
 				lpos.vz = velocity[2];
 
+				// Vehicle odometry linear velocity
+				odom.vx = lpos.vx;
+				odom.vy = lpos.vy;
+				odom.vz = lpos.vz;
+
 				// vertical position time derivative (m/s)
 				_ekf.get_pos_d_deriv(&lpos.z_deriv);
 
@@ -1352,6 +1370,16 @@ void Ekf2::run()
 
 				lpos.yaw = matrix::Eulerf(q).psi();
 
+				// Vehicle odometry quaternion
+				q.copyTo(odom.q);
+
+				// Vehicle odometry angular rates
+				float gyro_bias[3];
+				_ekf.get_gyro_bias(gyro_bias);
+				odom.rollspeed = sensors.gyro_rad[0] - gyro_bias[0];
+				odom.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1];
+				odom.yawspeed = sensors.gyro_rad[2] - gyro_bias[2];
+
 				lpos.dist_bottom_valid = _ekf.get_terrain_valid();
 
 				float terrain_vpos;
@@ -1394,9 +1422,43 @@ void Ekf2::run()
 					lpos.hagl_max = INFINITY;
 				}
 
+				// Get covariances to vehicle odometry
+				float covariances[24];
+				_ekf.get_covariances(covariances);
+
+				// get the covariance matrix size
+				const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
+				const size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
+
+				// initially set pose covariances to 0
+				for (size_t i = 0; i < POS_URT_SIZE; i++) {
+					odom.pose_covariance[i] = 0.0;
+				}
+
+				// set the position variances
+				odom.pose_covariance[odom.COVARIANCE_MATRIX_X_VARIANCE] = covariances[7];
+				odom.pose_covariance[odom.COVARIANCE_MATRIX_Y_VARIANCE] = covariances[8];
+				odom.pose_covariance[odom.COVARIANCE_MATRIX_Z_VARIANCE] = covariances[9];
+
+				// TODO: implement propagation from quaternion covariance to Euler angle covariance
+				// by employing the covariance law
+
+				// initially set velocity covariances to 0
+				for (size_t i = 0; i < VEL_URT_SIZE; i++) {
+					odom.velocity_covariance[i] = 0.0;
+				}
+
+				// set the linear velocity variances
+				odom.velocity_covariance[odom.COVARIANCE_MATRIX_VX_VARIANCE] = covariances[4];
+				odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5];
+				odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
+
 				// publish vehicle local position data
 				_vehicle_local_position_pub.update();
 
+				// publish vehicle odometry data
+				_vehicle_odometry_pub.update();
+
 				if (_ekf.global_position_is_valid() && !_preflt_fail) {
 					// generate and publish global position data
 					vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get();
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 14ff7a05d647b2e1f1cc0545160c548161d8f6f8..5bb474468ab7de583a8a331e856a406aed6cc849 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -51,6 +51,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// publications
 	_pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()),
 	_pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()),
+	_pub_odom(ORB_ID(vehicle_odometry), -1, &getPublications()),
 	_pub_est_status(ORB_ID(estimator_status), -1, &getPublications()),
 	_pub_innov(ORB_ID(ekf2_innovations), -1, &getPublications()),
 
@@ -509,6 +510,7 @@ void BlockLocalPositionEstimator::update()
 	if (_altOriginInitialized) {
 		// update all publications if possible
 		publishLocalPos();
+		publishOdom();
 		publishEstimatorStatus();
 		_pub_innov.get().timestamp = _timeStamp;
 		_pub_innov.update();
@@ -637,6 +639,82 @@ void BlockLocalPositionEstimator::publishLocalPos()
 	}
 }
 
+void BlockLocalPositionEstimator::publishOdom()
+{
+	const Vector<float, n_x> &xLP = _xLowPass.getState();
+
+	// publish vehicle odometry
+	if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
+	    PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
+	    && PX4_ISFINITE(_x(X_vz))) {
+		_pub_odom.get().timestamp = _timeStamp;
+		_pub_odom.get().local_frame = _pub_odom.get().LOCAL_FRAME_NED;
+
+		// position
+		_pub_odom.get().x = xLP(X_x);	// north
+		_pub_odom.get().y = xLP(X_y);	// east
+
+		if (_fusion.get() & FUSE_PUB_AGL_Z) {
+			_pub_odom.get().z = -_aglLowPass.getState();	// agl
+
+		} else {
+			_pub_odom.get().z = xLP(X_z);	// down
+		}
+
+		// orientation
+		matrix::Quatf q = matrix::Quatf(_sub_att.get().q);
+		q.copyTo(_pub_odom.get().q);
+
+		// linear velocity
+		_pub_odom.get().vx = xLP(X_vx);		// vel north
+		_pub_odom.get().vy = xLP(X_vy);		// vel east
+		_pub_odom.get().vz = xLP(X_vz);		// vel down
+
+		// angular velocity
+		_pub_odom.get().rollspeed = _sub_att.get().rollspeed;	// roll rate
+		_pub_odom.get().pitchspeed = _sub_att.get().pitchspeed;	// pitch rate
+		_pub_odom.get().yawspeed = _sub_att.get().yawspeed;	// yaw rate
+
+		// get the covariance matrix size
+		const size_t POS_URT_SIZE = sizeof(_pub_odom.get().pose_covariance) / sizeof(_pub_odom.get().pose_covariance[0]);
+		const size_t VEL_URT_SIZE = sizeof(_pub_odom.get().velocity_covariance) / sizeof(
+						    _pub_odom.get().velocity_covariance[0]);
+
+		// initially set pose covariances to 0
+		for (size_t i = 0; i < POS_URT_SIZE; i++) {
+			_pub_odom.get().pose_covariance[i] = 0.0;
+		}
+
+		// set the position variances
+		_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_X_VARIANCE] = _P(X_vx, X_vx);
+		_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Y_VARIANCE] = _P(X_vy, X_vy);
+		_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Z_VARIANCE] = _P(X_vz, X_vz);
+
+		// unknown orientation covariances
+		// TODO: add orientation covariance to vehicle_attitude
+		_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_ROLL_VARIANCE] = NAN;
+		_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_PITCH_VARIANCE] = NAN;
+		_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_YAW_VARIANCE] = NAN;
+
+		// initially set velocity covariances to 0
+		for (size_t i = 0; i < VEL_URT_SIZE; i++) {
+			_pub_odom.get().velocity_covariance[i] = 0.0;
+		}
+
+		// set the linear velocity variances
+		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VX_VARIANCE] = _P(X_vx, X_vx);
+		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VY_VARIANCE] = _P(X_vy, X_vy);
+		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VZ_VARIANCE] = _P(X_vz, X_vz);
+
+		// unknown angular velocity covariances
+		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_ROLLRATE_VARIANCE] = NAN;
+		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_PITCHRATE_VARIANCE] = NAN;
+		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_YAWRATE_VARIANCE] = NAN;
+
+		_pub_odom.update();
+	}
+}
+
 void BlockLocalPositionEstimator::publishEstimatorStatus()
 {
 	_pub_est_status.get().timestamp = _timeStamp;
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index 002147d7a9656d6380ce02c19cf1adccd105c653..478e8a570bd394c3ff6ed7d57d495c911405177e 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -239,6 +239,7 @@ private:
 	// publications
 	void publishLocalPos();
 	void publishGlobalPos();
+	void publishOdom();
 	void publishEstimatorStatus();
 
 	// attributes
@@ -267,6 +268,7 @@ private:
 	// publications
 	uORB::Publication<vehicle_local_position_s> _pub_lpos;
 	uORB::Publication<vehicle_global_position_s> _pub_gpos;
+	uORB::Publication<vehicle_odometry_s> _pub_odom;
 	uORB::Publication<estimator_status_s> _pub_est_status;
 	uORB::Publication<ekf2_innovations_s> _pub_innov;
 
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 60679bc25af32e30e24a7ba1400dca62a4af4f50..b626cf8e2701bebbbff04f7c74a094c2123648c5 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1775,7 +1775,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
 		configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
 		configure_stream_local("VFR_HUD", 4.0f);
-		configure_stream_local("VISION_POSITION_ESTIMATE", 1.0f);
+		configure_stream_local("ODOMETRY", 3.0f);
 		configure_stream_local("WIND_COV", 1.0f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 5.f);
 		break;
@@ -1818,7 +1818,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
 		configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
 		configure_stream_local("VFR_HUD", 10.0f);
-		configure_stream_local("VISION_POSITION_ESTIMATE", 10.0f);
+		configure_stream_local("ODOMETRY", 30.0f);
 		configure_stream_local("WIND_COV", 10.0f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 5.f);
 		break;
@@ -1884,7 +1884,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("TIMESYNC", 10.0f);
 		configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
 		configure_stream_local("VFR_HUD", 20.0f);
-		configure_stream_local("VISION_POSITION_ESTIMATE", 10.0f);
+		configure_stream_local("ODOMETRY", 30.0f);
 		configure_stream_local("WIND_COV", 10.0f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 5.f);
 		break;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 1c248e6f283e08f681b9cbc1331280d96c6ffbbe..b9cdfcd1565840bcf476c6f86ecdd8d1baa15cfd 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -2258,23 +2258,22 @@ protected:
 	}
 };
 
-//TODO: remove this -> add ODOMETRY loopback only
-class MavlinkStreamVisionPositionEstimate : public MavlinkStream
+class MavlinkStreamOdometry : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamVisionPositionEstimate::get_name_static();
+		return MavlinkStreamOdometry::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "VISION_POSITION_ESTIMATE";
+		return "ODOMETRY";
 	}
 
 	static uint16_t get_id_static()
 	{
-		return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE;
+		return MAVLINK_MSG_ID_ODOMETRY;
 	}
 
 	uint16_t get_id()
@@ -2284,50 +2283,109 @@ public:
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamVisionPositionEstimate(mavlink);
+		return new MavlinkStreamOdometry(mavlink);
 	}
 
 	unsigned get_size()
 	{
-		return (_odom_time > 0) ? MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+		return (_odom_time > 0) ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
-private:
 
+private:
 	MavlinkOrbSubscription *_odom_sub;
 	uint64_t _odom_time;
 
+	MavlinkOrbSubscription *_vodom_sub;
+	uint64_t _vodom_time;
+
 	/* do not allow top copying this class */
-	MavlinkStreamVisionPositionEstimate(MavlinkStreamVisionPositionEstimate &) = delete;
-	MavlinkStreamVisionPositionEstimate &operator = (const MavlinkStreamVisionPositionEstimate &) = delete;
+	MavlinkStreamOdometry(MavlinkStreamOdometry &) = delete;
+	MavlinkStreamOdometry &operator = (const MavlinkStreamOdometry &) = delete;
 
 protected:
-	explicit MavlinkStreamVisionPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_odom_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_visual_odometry))),
-		_odom_time(0)
+	explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_odom_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_odometry))),
+		_odom_time(0),
+		_vodom_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_visual_odometry))),
+		_vodom_time(0)
 	{}
 
 	bool send(const hrt_abstime t)
 	{
-		vehicle_odometry_s vodom;
+		vehicle_odometry_s odom;
+		// check if it is to send visual odometry loopback or not
+		bool odom_updated = false;
 
-		if (_odom_sub->update(&_odom_time, &vodom)) {
-			mavlink_vision_position_estimate_t vmsg = {};
-			vmsg.usec = vodom.timestamp;
-			vmsg.x = vodom.x;
-			vmsg.y = vodom.y;
-			vmsg.z = vodom.z;
+		mavlink_odometry_t msg = {};
 
-			matrix::Eulerf euler = matrix::Quatf(vodom.q);
-			vmsg.roll = euler.phi();
-			vmsg.pitch = euler.theta();
-			vmsg.yaw = euler.psi();
+		if (_send_odom_loopback.get()) {
+			odom_updated = _vodom_sub->update(&_vodom_time, &odom);
+			// frame matches the external vision system
+			msg.frame_id = MAV_FRAME_VISION_NED;
 
-			mavlink_msg_vision_position_estimate_send_struct(_mavlink->get_channel(), &vmsg);
+		} else {
+			odom_updated = _odom_sub->update(&_odom_time, &odom);
+			// frame matches the PX4 local NED frame
+			msg.frame_id = MAV_FRAME_ESTIM_NED;
+		}
+
+		if (odom_updated) {
+			msg.time_usec = odom.timestamp;
+			msg.child_frame_id = MAV_FRAME_BODY_NED;
+
+			// Current position
+			msg.x = odom.x;
+			msg.y = odom.y;
+			msg.z = odom.z;
+
+			// Current orientation
+			msg.q[0] = odom.q[0];
+			msg.q[1] = odom.q[1];
+			msg.q[2] = odom.q[2];
+			msg.q[3] = odom.q[3];
+
+			// Local NED to body-NED Dcm matrix
+			matrix::Dcmf Rlb(matrix::Quatf(odom.q));
+
+			// Rotate linear and angular velocity from local NED to body-NED frame
+			matrix::Vector3f linvel_body(Rlb * matrix::Vector3f(odom.vx, odom.vy, odom.vz));
+
+			// Current linear velocity
+			msg.vx = linvel_body(0);
+			msg.vy = linvel_body(1);
+			msg.vz = linvel_body(2);
+
+			// Current body rates
+			msg.rollspeed = odom.rollspeed;
+			msg.pitchspeed = odom.pitchspeed;
+			msg.yawspeed = odom.yawspeed;
+
+			// get the covariance matrix size
+			const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
+			const size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
+			static_assert(POS_URT_SIZE == (sizeof(msg.pose_covariance) / sizeof(msg.pose_covariance[0])),
+				      "Odometry Pose Covariance matrix URT array size mismatch");
+			static_assert(VEL_URT_SIZE == (sizeof(msg.twist_covariance) / sizeof(msg.twist_covariance[0])),
+				      "Odometry Velocity Covariance matrix URT array size mismatch");
+
+			// copy pose covariances
+			for (size_t i = 0; i < POS_URT_SIZE; i++) {
+				msg.pose_covariance[i] = odom.pose_covariance[i];
+			}
+
+			// copy velocity covariances
+			//TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame
+			for (size_t i = 0; i < VEL_URT_SIZE; i++) {
+				msg.twist_covariance[i] = odom.velocity_covariance[i];
+			}
+
+			mavlink_msg_odometry_send_struct(_mavlink->get_channel(), &msg);
 
 			return true;
 		}
 
 		return false;
+
 	}
 };
 
@@ -2402,88 +2460,6 @@ protected:
 	}
 };
 
-
-class MavlinkStreamLocalPositionNEDCOV : public MavlinkStream
-{
-public:
-	const char *get_name() const
-	{
-		return MavlinkStreamLocalPositionNEDCOV::get_name_static();
-	}
-
-	static const char *get_name_static()
-	{
-		return "LOCAL_POSITION_NED_COV";
-	}
-
-	static uint16_t get_id_static()
-	{
-		return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV;
-	}
-
-	uint16_t get_id()
-	{
-		return get_id_static();
-	}
-
-	static MavlinkStream *new_instance(Mavlink *mavlink)
-	{
-		return new MavlinkStreamLocalPositionNEDCOV(mavlink);
-	}
-
-	unsigned get_size()
-	{
-		return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-	}
-
-private:
-	MavlinkOrbSubscription *_est_sub;
-	uint64_t _est_time;
-
-	/* do not allow top copying this class */
-	MavlinkStreamLocalPositionNEDCOV(MavlinkStreamLocalPositionNEDCOV &) = delete;
-	MavlinkStreamLocalPositionNEDCOV &operator = (const MavlinkStreamLocalPositionNEDCOV &) = delete;
-
-protected:
-	explicit MavlinkStreamLocalPositionNEDCOV(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_est_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))),
-		_est_time(0)
-	{}
-
-	bool send(const hrt_abstime t)
-	{
-		struct estimator_status_s est = {};
-
-		if (_est_sub->update(&_est_time, &est)) {
-			mavlink_local_position_ned_cov_t msg = {};
-
-			msg.time_usec = est.timestamp;
-			msg.x = est.states[0];
-			msg.y = est.states[1];
-			msg.z = est.states[2];
-			msg.vx = est.states[3];
-			msg.vy = est.states[4];
-			msg.vz = est.states[5];
-			msg.ax = est.states[6];
-			msg.ay = est.states[7];
-			msg.az = est.states[8];
-
-			for (int i = 0; i < 9; i++) {
-				msg.covariance[i] = est.covariances[i];
-			}
-
-			msg.covariance[10] = est.health_flags;
-			msg.covariance[11] = est.timeout_flags;
-
-			mavlink_msg_local_position_ned_cov_send_struct(_mavlink->get_channel(), &msg);
-
-			return true;
-		}
-
-		return false;
-	}
-};
-
 class MavlinkStreamEstimatorStatus : public MavlinkStream
 {
 public:
@@ -4857,8 +4833,7 @@ static const StreamListItem streams_list[] = {
 	StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static, &MavlinkStreamTimesync::get_id_static),
 	StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static, &MavlinkStreamGlobalPositionInt::get_id_static),
 	StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static, &MavlinkStreamLocalPositionNED::get_id_static),
-	StreamListItem(&MavlinkStreamVisionPositionEstimate::new_instance, &MavlinkStreamVisionPositionEstimate::get_name_static, &MavlinkStreamVisionPositionEstimate::get_id_static),
-	StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static, &MavlinkStreamLocalPositionNEDCOV::get_id_static),
+	StreamListItem(&MavlinkStreamOdometry::new_instance, &MavlinkStreamOdometry::get_name_static, &MavlinkStreamOdometry::get_id_static),
 	StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static, &MavlinkStreamEstimatorStatus::get_id_static),
 	StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static, &MavlinkStreamAttPosMocap::get_id_static),
 	StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static, &MavlinkStreamHomePosition::get_id_static),
diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c
index 310ed4e164200cfa26eb0795929ec850befa90b7..2968712e33c5c5ecf7a58f193d6476669a722a54 100644
--- a/src/modules/mavlink/mavlink_params.c
+++ b/src/modules/mavlink/mavlink_params.c
@@ -164,3 +164,14 @@ PARAM_DEFINE_INT32(MAV_HASH_CHK_EN, 1);
  * @group MAVLink
  */
 PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1);
+
+/**
+ * Activate ODOMETRY loopback.
+ *
+ * If set, it gets the data from 'vehicle_visual_odometry' instead of 'vehicle_odometry'
+ * serving as a loopback of the received ODOMETRY messages on the Mavlink receiver.
+ *
+ * @boolean
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_ODOM_LP, 0);
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index af7c9fb3fdec972c514e2b84d19fe49b15ae649c..f3d6c2b2ccb48799bf7b635b4534353bc2012832 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -45,6 +45,7 @@
 #include "mavlink_main.h"
 
 MavlinkStream::MavlinkStream(Mavlink *mavlink) :
+	ModuleParams(nullptr),
 	_mavlink(mavlink)
 {
 	_last_sent = hrt_absolute_time();
@@ -57,6 +58,7 @@ int
 MavlinkStream::update(const hrt_abstime &t)
 {
 	update_data();
+	ModuleParams::updateParams();
 
 	// If the message has never been sent before we want
 	// to send it immediately and can return right away
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 6c00f8f606fd865ec12fa5bcf48fcdd99a3881cd..e982c61e97cd65bc489023d5493f00dcc7adc88e 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -42,10 +42,11 @@
 #define MAVLINK_STREAM_H_
 
 #include <drivers/drv_hrt.h>
+#include <px4_module_params.h>
 
 class Mavlink;
 
-class MavlinkStream
+class MavlinkStream : public ModuleParams
 {
 
 public:
@@ -126,6 +127,10 @@ protected:
 	 */
 	virtual void update_data() { }
 
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::MAV_ODOM_LP>) _send_odom_loopback
+	)
+
 private:
 	hrt_abstime _last_sent{0};
 	bool _first_message_sent{false};