[WIP] ekf2: initial motion capture (mocap) support with new modular structure

.vscode/settings.json

@@ -127,5 +127,6 @@
         "terminal.integrated.scrollback": 15000,
         "yaml.schemas": {
                 "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
-        }
+        },
+        "ros.distro": "humble"
 }

boards/px4/sitl/default.px4board

@@ -13,6 +13,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+CONFIG_EKF2_MOCAP=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y

msg/EstimatorAidSource1d.msg

@@ -21,4 +21,5 @@ bool fused                   # true if the sample was successfully fused
 # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt
 # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
 # TOPICS estimator_aid_src_fake_hgt
+# TOPICS estimator_aid_src_mocap_z estimator_aid_src_mocap_yaw
 # TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw

msg/EstimatorAidSource2d.msg

@@ -19,4 +19,4 @@ bool innovation_rejected     # true if the observation has been rejected
 bool fused                   # true if the sample was successfully fused
 
 # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
-# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
+# TOPICS estimator_aid_src_aux_vel estimator_aid_src_mocap_xy estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow

src/modules/ekf2/CMakeLists.txt

@@ -99,6 +99,10 @@ if(CONFIG_EKF2_AIRSPEED)
 	list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_MOCAP)
+	list(APPEND EKF_SRCS EKF/mocap.cpp)
+endif()
+
 if(CONFIG_EKF2_AUXVEL)
 	list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp)
 endif()

src/modules/ekf2/EKF/CMakeLists.txt

@@ -64,6 +64,10 @@ if(CONFIG_EKF2_AIRSPEED)
 	list(APPEND EKF_SRCS airspeed_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_MOCAP)
+	list(APPEND EKF_SRCS mocap.cpp)
+endif()
+
 if(CONFIG_EKF2_AUXVEL)
 	list(APPEND EKF_SRCS auxvel_fusion.cpp)
 endif()

src/modules/ekf2/EKF/control.cpp

@@ -111,6 +111,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 
 	controlGpsFusion(imu_delayed);
 
+#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
+	_mocap.update(*this, imu_delayed);
+#endif // CONFIG_EKF2_MOCAP
+
 #if defined(CONFIG_EKF2_AIRSPEED)
 	controlAirDataFusion(imu_delayed);
 #endif // CONFIG_EKF2_AIRSPEED

src/modules/ekf2/EKF/ekf.cpp

@@ -316,3 +316,10 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	// Note fixed coefficients are used to save operations. The exact time constant is not important.
 	_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu_delayed.delta_ang_dt;
 }
+
+void Ekf::updateParameters()
+{
+#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
+	_mocap.updateParameters();
+#endif // CONFIG_EKF2_MOCAP
+}

src/modules/ekf2/EKF/ekf.h

@@ -54,6 +54,10 @@
 #include <uORB/topics/estimator_aid_source2d.h>
 #include <uORB/topics/estimator_aid_source3d.h>
 
+#if defined(CONFIG_EKF2_MOCAP)
+# include "mocap.hpp"
+#endif // CONFIG_EKF2_MOCAP
+
 enum class Likelihood { LOW, MEDIUM, HIGH };
 
 class Ekf final : public EstimatorInterface
@@ -266,6 +270,8 @@ public:
 	// get the wind velocity in m/s
 	const Vector2f &getWindVelocity() const { return _state.wind_vel; };
 
+	auto& getState() const { return _state; }
+
 	// get the wind velocity var
 	Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); }
 
@@ -506,6 +512,10 @@ public:
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 #endif // CONFIG_EKF2_AUXVEL
 
+	void updateParameters();
+
+	friend class Mocap;
+
 private:
 
 	// set the internal states and status to their default value
@@ -1242,6 +1252,10 @@ private:
 		// if any of the innovations are rejected, then the overall innovation is rejected
 		status.innovation_rejected = innovation_rejected;
 	}
+
+#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
+	Mocap _mocap{};
+#endif // CONFIG_EKF2_MOCAP
 };
 
 #endif // !EKF_EKF_H

src/modules/ekf2/EKF/mocap.cpp

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ekf.h"
+
+#include "mocap.hpp"
+
+#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
+
+void Mocap::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
+{
+
+#if defined(MODULE_NAME)
+
+	if (_mocap_sub.updated()) {
+
+		vehicle_odometry_s mocap{};
+		_mocap_sub.copy(&mocap);
+
+		const int64_t time_us = mocap.timestamp_sample - static_cast<int64_t>(_param_ekf2_mocap_delay.get() * 1000);
+
+		MocapSample sample{};
+		sample.time_us = time_us;
+		sample.pos = Vector3f{mocap.position};
+		_mocap_buffer.push(sample);
+	}
+
+#endif // MODULE_NAME
+
+
+	MocapSample mocap_sample;
+
+	if (_mocap_buffer.pop_first_older_than(imu_delayed.time_us, &mocap_sample)) {
+
+		if (!ekf.global_origin_valid()) {
+			return;
+		}
+
+		// position
+		{
+			estimator_aid_source2d_s aid_src{};
+
+			const Vector2f pos_obs_var{
+				math::max(sq(mocap_sample.position_var(0)), sq(_param_ekf2_mocap_noise.get())),
+				math::max(sq(mocap_sample.position_var(1)), sq(_param_ekf2_mocap_noise.get()))
+			};
+
+			ekf.updateHorizontalPositionAidSrcStatus(mocap_sample.time_us,
+					mocap_sample.pos.xy(),                         // observation
+					pos_obs_var,                                   // observation variance
+					math::max(_param_ekf2_mocap_gate.get(), 1.f),  // innovation gate
+					aid_src);
+			aid_src.fusion_enabled = _param_ekf2_mocap_ctrl.get() & 0b001; // bit 0 Horizontal position
+
+			// TODO: check frame
+			if (ekf.control_status_flags().yaw_align) {
+				ekf.fuseHorizontalPosition(aid_src);
+			}
+
+#if defined(MODULE_NAME)
+			aid_src.timestamp = hrt_absolute_time();
+			_estimator_aid_src_mocap_xy_pub.publish(aid_src);
+#endif // MODULE_NAME
+		}
+
+	}
+}
+
+#endif // CONFIG_EKF2_MOCAP

src/modules/ekf2/EKF/mocap.hpp

@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef EKF_MOCAP_HPP
+#define EKF_MOCAP_HPP
+
+#include "common.h"
+#include "RingBuffer.h"
+
+#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
+
+#if defined(MODULE_NAME)
+# include <px4_platform_common/module_params.h>
+# include <uORB/PublicationMulti.hpp>
+# include <uORB/Subscription.hpp>
+# include <uORB/topics/estimator_aid_source1d.h>
+# include <uORB/topics/estimator_aid_source2d.h>
+# include <uORB/topics/vehicle_odometry.h>
+#endif // MODULE_NAME
+
+class Ekf;
+
+class Mocap : public ModuleParams
+{
+public:
+	Mocap() : ModuleParams(nullptr) {}
+	~Mocap() = default;
+
+	void update(Ekf &ekf, const estimator::imuSample &imu_delayed);
+
+	void updateParameters()
+	{
+		updateParams();
+	}
+
+private:
+
+	struct MocapSample {
+		uint64_t    time_us{};     ///< timestamp of the measurement (uSec)
+		Vector3f    pos{};         ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
+		Quatf       quat{};        ///< quaternion defining rotation from body to earth frame
+		Vector3f    position_var{};    ///< XYZ position variances (m**2)
+		Vector3f    orientation_var{}; ///< orientation variance (rad**2)
+		PositionFrame pos_frame = PositionFrame::LOCAL_FRAME_FRD;
+		uint8_t     reset_counter{};
+		int8_t     quality{};     ///< quality indicator between 0 and 100
+	};
+
+	RingBuffer<MocapSample> _mocap_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate
+	uint64_t _time_last_buffer_push{0};
+
+	enum state {
+		stopped,
+		starting,
+		active,
+		stopping,
+		resetting,
+	};
+
+#if defined(MODULE_NAME)
+	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_mocap_xy_pub {ORB_ID(estimator_aid_src_mocap_xy)};
+	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mocap_z_pub{ORB_ID(estimator_aid_src_mocap_z)};
+	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mocap_yaw_pub{ORB_ID(estimator_aid_src_mocap_yaw)};
+
+	uORB::Subscription _mocap_sub{ORB_ID(vehicle_mocap_odometry)};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::EKF2_MOCAP_CTRL>) _param_ekf2_mocap_ctrl,
+		(ParamFloat<px4::params::EKF2_MOCAP_DELAY>) _param_ekf2_mocap_delay,
+		(ParamFloat<px4::params::EKF2_MOCAP_NOISE>) _param_ekf2_mocap_noise,
+		(ParamFloat<px4::params::EKF2_MOCAP_GATE>) _param_ekf2_mocap_gate
+	)
+
+#endif // MODULE_NAME
+};
+
+#endif // CONFIG_EKF2_MOCAP
+
+#endif // !EKF_MOCAP_HPP

src/modules/ekf2/EKF2.cpp

@@ -450,6 +450,8 @@ void EKF2::Run()
 				}
 			}
 		}
+
+		_ekf.updateParameters();
 	}
 
 	if (!_callback_registered) {

src/modules/ekf2/Kconfig

@@ -21,6 +21,13 @@ depends on MODULES_EKF2
 	---help---
 		EKF2 airspeed fusion support.
 
+menuconfig EKF2_MOCAP
+depends on MODULES_EKF2
+	bool "mocap fusion support"
+	default y
+	---help---
+		EKF2 motion capture (mocap) fusion support.
+
 menuconfig EKF2_AUXVEL
 depends on MODULES_EKF2
         bool "aux velocity fusion support"

src/modules/ekf2/ekf2_params.c

@@ -1568,3 +1568,53 @@ PARAM_DEFINE_INT32(EKF2_SYNT_MAG_Z, 0);
  * @decimal 1
  */
 PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f);
+
+/**
+ * Motion capture (mocap) aiding
+ *
+ * Set bits in the following positions to enable:
+ * 0 : Horizontal position fusion
+ * 1 : Vertical position fusion
+ * 2 : Yaw
+ *
+ * @group EKF2
+ * @min 0
+ * @max 15
+ * @bit 0 Horizontal position
+ * @bit 1 Vertical position
+ * @bit 2 Yaw
+ */
+PARAM_DEFINE_INT32(EKF2_MOCAP_CTRL, 1);
+
+/**
+ * Motion capture (mocap) delay relative to IMU measurements
+ *
+ * @group EKF2
+ * @min 0
+ * @max 300
+ * @unit ms
+ * @reboot_required true
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_MOCAP_DELAY, 0);
+
+/**
+ * Measurement noise for motion capture (mocap) observations used to lower bound or replace the uncertainty included in the message
+ *
+ * @group EKF2
+ * @min 0.01
+ * @unit m
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_MOCAP_NOISE, 0.9f);
+
+/**
+ * Gate size for Motion capture (mocap) fusion
+ *
+ * Sets the number of standard deviations used by the innovation consistency test.
+ * @group EKF2
+ * @min 1.0
+ * @unit SD
+ * @decimal 1
+*/
+PARAM_DEFINE_FLOAT(EKF2_MOCAP_GATE, 3.0f);

src/modules/logger/logged_topics.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -198,6 +198,7 @@ void LoggedTopics::add_default_topics()
 	// add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_mocap", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);