Add module vehicle_model_estimator

This module estimates the vehicle mass and inertia matrix.

It uses a least mean square algorithm between
delayed, low pass filtered, and time differentiated setpoints
(thrust and torque, respectively),
and low pass filtered and time differentiated accelerations
(linear and angular, respectively).

msg/CMakeLists.txt

@@ -145,6 +145,7 @@ set(msg_files
 	vehicle_local_position.msg
 	vehicle_local_position_setpoint.msg
 	vehicle_magnetometer.msg
+	vehicle_model.msg
 	vehicle_odometry.msg
 	vehicle_rates_setpoint.msg
 	vehicle_roi.msg

msg/vehicle_model.msg

@@ -0,0 +1,6 @@
+# Vehicle model, mass and inertia matrix
+
+uint64 timestamp		# time since system start (microseconds)
+
+float32 mass			# vehicle mass
+float32[9] inertia		# vehicle inertia matrix

src/modules/vehicle_model_estimator/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2019 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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE modules__vehicle_model_estimator
+	MAIN vehicle_model_estimator
+	COMPILE_FLAGS
+	SRCS
+		VehicleModelEstimator.cpp
+		VehicleModelEstimator.hpp
+	DEPENDS
+		mathlib
+		px4_work_queue
+		ecl_EKF
+	)

src/modules/vehicle_model_estimator/VehicleModelEstimator.cpp

@@ -0,0 +1,519 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2019 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file VehicleModelEstimator.cpp
+ *
+ * Vehicle model estimator.
+ *
+ * To estimate the vehicle inertia, the estimator low-pass filters and
+ * computes the time derivative of the vehicle angular acceleration
+ * and of the vehicle torque setpoint, then applies a least-mean-square.
+ * Similarly, to estimate the vehicle mass, the estimator low-pass
+ * filters and computes the time derivative of the vehicle acceleration
+ * and of the vehicle thrust setpoint, then applies a least-mean-square.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include "VehicleModelEstimator.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <matrix/matrix/math.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+
+VehicleModelEstimator::VehicleModelEstimator() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	parameters_updated();
+	_I_est_inv = inv(_I);
+	_mass_est_inv = 1.0f / _mass;
+}
+
+VehicleModelEstimator::~VehicleModelEstimator()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+VehicleModelEstimator::init()
+{
+	if (!_vehicle_angular_acceleration_sub.registerCallback()) {
+		PX4_ERR("vehicle_angular_acceleration callback registration failed!");
+		return false;
+	}
+
+	if (!_vehicle_acceleration_sub.registerCallback()) {
+		PX4_ERR("vehicle_acceleration callback registration failed!");
+		return false;
+	}
+
+	return true;
+}
+
+void
+VehicleModelEstimator::parameters_updated()
+{
+	// vehicle inertia matrix
+	const float inertia[3][3] = {
+		{_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()},
+		{_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()},
+		{_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()}
+	};
+	_I = matrix::Matrix3f(inertia);
+
+	// vehicle mass
+	_mass = _param_vm_mass.get();
+
+	// low pass filters
+	if (fabsf(_lpf_ang_accel.get_cutoff_freq() - _param_vm_est_cutoff.get()) > 0.01f) {
+		_lpf_ang_accel.set_cutoff_frequency(_loop_update_rate_hz_ang_accel, _param_vm_est_cutoff.get());
+		_lpf_ang_accel.reset(_ang_accel_filtered);
+	}
+
+	if (fabsf(_lpf_torque_sp.get_cutoff_freq() - _param_vm_est_cutoff.get()) > 0.01f) {
+		_lpf_torque_sp.set_cutoff_frequency(_loop_update_rate_hz_torque_sp, _param_vm_est_cutoff.get());
+		_lpf_torque_sp.reset(_torque_sp_filtered);
+	}
+
+	if (fabsf(_lpf_lin_accel.get_cutoff_freq() - _param_vm_est_cutoff.get()) > 0.01f) {
+		_lpf_lin_accel.set_cutoff_frequency(_loop_update_rate_hz_lin_accel, _param_vm_est_cutoff.get());
+		_lpf_lin_accel.reset(_lin_accel_filtered);
+	}
+
+	if (fabsf(_lpf_thrust_sp.get_cutoff_freq() - _param_vm_est_cutoff.get()) > 0.01f) {
+		_lpf_thrust_sp.set_cutoff_frequency(_loop_update_rate_hz_thrust_sp, _param_vm_est_cutoff.get());
+		_lpf_thrust_sp.reset(_thrust_sp_filtered);
+	}
+}
+
+void
+VehicleModelEstimator::Run()
+{
+	if (should_exit()) {
+		_vehicle_angular_acceleration_sub.unregisterCallback();
+		_vehicle_acceleration_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
+		parameters_updated();
+	}
+
+	// check for updates in other topics
+	if (_vehicle_land_detected_sub.updated()) {
+		vehicle_land_detected_s vehicle_land_detected;
+
+		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
+			_landed = vehicle_land_detected.landed;
+			_maybe_landed = vehicle_land_detected.maybe_landed;
+		}
+	}
+
+	if (_vehicle_control_mode_sub.updated()) {
+		vehicle_control_mode_s vehicle_control_mode;
+
+		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
+			_armed = vehicle_control_mode.flag_armed;
+		}
+	}
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	/* run inertia estimator on angular acceleration and torque setpoint changes */
+	bool inertia_update = false;
+	vehicle_angular_acceleration_s angular_acceleration;
+	vehicle_torque_setpoint_s torque_setpoint;
+
+	if (_vehicle_angular_acceleration_sub.update(&angular_acceleration)) {
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt_ang_accel = math::constrain(((angular_acceleration.timestamp_sample - _timestamp_ang_accel) / 1e6f), 0.0002f, 0.02f);
+		_timestamp_ang_accel = angular_acceleration.timestamp_sample;
+
+		// Calculate loop update rate
+		// (while disarmed or at least a few times beacause updating the filter is expensive)
+		if (!_armed || (now - _task_start) < 3300000) {
+			_dt_accumulator_ang_accel += dt_ang_accel;
+			++_loop_counter_ang_accel;
+
+			if (_dt_accumulator_ang_accel > 1.0f) {
+				const float loop_update_rate = (float)_loop_counter_ang_accel / _dt_accumulator_ang_accel;
+				_loop_update_rate_hz_ang_accel = _loop_update_rate_hz_ang_accel * 0.5f + loop_update_rate * 0.5f;
+				_dt_accumulator_ang_accel = 0.0f;
+				_loop_counter_ang_accel = 0;
+
+				if (fabsf(_lpf_ang_accel.get_sample_freq() - _loop_update_rate_hz_ang_accel) > 1.0f) {
+					_lpf_ang_accel.set_cutoff_frequency(_loop_update_rate_hz_ang_accel, _param_vm_est_cutoff.get());
+				}
+			}
+		}
+
+		// Apply low pass filter
+		Vector3f ang_accel_filtered_new(_lpf_ang_accel.apply(Vector3f(angular_acceleration.xyz)));
+
+		// Time derivative
+		_ang_accel_filtered_diff = (ang_accel_filtered_new - _ang_accel_filtered) / dt_ang_accel;
+		_ang_accel_filtered = ang_accel_filtered_new;
+
+		// Indicate that new data is available to estimate the vehicle inertia
+		inertia_update = true;
+	}
+
+	if (_vehicle_torque_setpoint_sub.update(&torque_setpoint)) {
+
+		// Buffer incoming data
+		_torque_sp_buffer.push(torque_setpoint);
+		torque_setpoint = _torque_sp_buffer.get_oldest();
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt_torque_sp = math::constrain(((torque_setpoint.timestamp_sample - _timestamp_torque_sp) / 1e6f), 0.0002f, 0.02f);
+		_timestamp_torque_sp = torque_setpoint.timestamp_sample;
+
+		// Calculate loop update rate
+		// (while disarmed or at least a few times beacause updating the filter is expensive)
+		if (!_armed || (now - _task_start) < 3300000) {
+			_dt_accumulator_torque_sp += dt_torque_sp;
+			++_loop_counter_torque_sp;
+
+			if (_dt_accumulator_torque_sp > 1.0f) {
+				const float loop_update_rate = (float)_loop_counter_torque_sp / _dt_accumulator_torque_sp;
+				_loop_update_rate_hz_torque_sp = _loop_update_rate_hz_torque_sp * 0.5f + loop_update_rate * 0.5f;
+				_dt_accumulator_torque_sp = 0.0f;
+				_loop_counter_torque_sp = 0;
+
+				if (fabsf(_lpf_torque_sp.get_sample_freq() - _loop_update_rate_hz_torque_sp) > 1.0f) {
+					_lpf_torque_sp.set_cutoff_frequency(_loop_update_rate_hz_torque_sp, _param_vm_est_cutoff.get());
+				}
+			}
+		}
+
+		// Compute required buffer size
+		if (!_armed || (now - _task_start) < 3300000 || _torque_sp_buffer.get_length() < 2) {
+			int _torque_sp_buffer_length = (_param_vm_est_delay.get() * _loop_update_rate_hz_torque_sp) + 1;
+
+			if (_torque_sp_buffer.get_length() != _torque_sp_buffer_length) {
+				if(!_torque_sp_buffer.allocate(_torque_sp_buffer_length)) {
+					PX4_ERR("Torque setpoint buffer allocation failed!");
+				}
+			}
+		}
+
+		// Apply low pass filter
+		Vector3f torque_sp_filtered_new(_lpf_torque_sp.apply(Vector3f(torque_setpoint.xyz)));
+
+		// Time derivative
+		_torque_sp_filtered_diff = (torque_sp_filtered_new - _torque_sp_filtered) / dt_torque_sp;
+		_torque_sp_filtered = torque_sp_filtered_new;
+
+		// Indicate that new data is available to estimate the vehicle inertia
+		inertia_update = true;
+	}
+
+	/* run mass estimator on linear acceleration and thrust setpoint changes */
+	bool mass_update = false;
+	vehicle_acceleration_s acceleration;
+	vehicle_thrust_setpoint_s thrust_setpoint;
+
+	if (_vehicle_acceleration_sub.update(&acceleration)) {
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt_lin_accel = math::constrain(((acceleration.timestamp_sample - _timestamp_lin_accel) / 1e6f), 0.0002f, 0.02f);
+		_timestamp_lin_accel = acceleration.timestamp_sample;
+
+		// Calculate loop update rate
+		// (while disarmed or at least a few times beacause updating the filter is expensive)
+		if (!_armed || (now - _task_start) < 3300000) {
+			_dt_accumulator_lin_accel += dt_lin_accel;
+			++_loop_counter_lin_accel;
+
+			if (_dt_accumulator_lin_accel > 1.0f) {
+				const float loop_update_rate = (float)_loop_counter_lin_accel / _dt_accumulator_lin_accel;
+				_loop_update_rate_hz_lin_accel = _loop_update_rate_hz_lin_accel * 0.5f + loop_update_rate * 0.5f;
+				_dt_accumulator_lin_accel = 0.0f;
+				_loop_counter_lin_accel = 0;
+
+				if (fabsf(_lpf_lin_accel.get_sample_freq() - _loop_update_rate_hz_lin_accel) > 1.0f) {
+					_lpf_lin_accel.set_cutoff_frequency(_loop_update_rate_hz_lin_accel, _param_vm_est_cutoff.get());
+				}
+			}
+		}
+
+		// Compute required buffer size
+		if (!_armed || (now - _task_start) < 3300000 || _thrust_sp_buffer.get_length() < 2) {
+			int _thrust_sp_buffer_length = (_param_vm_est_delay.get() * _loop_update_rate_hz_thrust_sp) + 1;
+
+			if (_thrust_sp_buffer.get_length() != _thrust_sp_buffer_length) {
+				if(!_thrust_sp_buffer.allocate(_thrust_sp_buffer_length)) {
+					PX4_ERR("Thrust setpoint buffer allocation failed!");
+				}
+			}
+		}
+
+		// Apply low pass filter
+		Vector3f lin_accel_filtered_new(_lpf_lin_accel.apply(Vector3f(acceleration.xyz)));
+
+		// Time derivative
+		_lin_accel_filtered_diff = (lin_accel_filtered_new - _lin_accel_filtered) / dt_lin_accel;
+		_lin_accel_filtered = lin_accel_filtered_new;
+
+		// Indicate that new data is available to estimate the vehicle mass
+		mass_update = true;
+	}
+
+	if (_vehicle_thrust_setpoint_sub.update(&thrust_setpoint)) {
+
+		// Buffer incoming data
+		_thrust_sp_buffer.push(thrust_setpoint);
+		thrust_setpoint = _thrust_sp_buffer.get_oldest();
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt_thrust_sp = math::constrain(((thrust_setpoint.timestamp_sample - _timestamp_thrust_sp) / 1e6f), 0.0002f, 0.02f);
+		_timestamp_thrust_sp = thrust_setpoint.timestamp_sample;
+
+		// Calculate loop update rate
+		// (while disarmed or at least a few times beacause updating the filter is expensive)
+		if (!_armed || (now - _task_start) < 3300000) {
+			_dt_accumulator_thrust_sp += dt_thrust_sp;
+			++_loop_counter_thrust_sp;
+
+			if (_dt_accumulator_thrust_sp > 1.0f) {
+				const float loop_update_rate = (float)_loop_counter_thrust_sp / _dt_accumulator_thrust_sp;
+				_loop_update_rate_hz_thrust_sp = _loop_update_rate_hz_thrust_sp * 0.5f + loop_update_rate * 0.5f;
+				_dt_accumulator_thrust_sp = 0.0f;
+				_loop_counter_thrust_sp = 0;
+
+				if (fabsf(_lpf_thrust_sp.get_sample_freq() - _loop_update_rate_hz_thrust_sp) > 1.0f) {
+					_lpf_thrust_sp.set_cutoff_frequency(_loop_update_rate_hz_thrust_sp, _param_vm_est_cutoff.get());
+				}
+			}
+		}
+
+		// Apply low pass filter
+		Vector3f thrust_sp_filtered_new(_lpf_thrust_sp.apply(Vector3f(thrust_setpoint.xyz)));
+
+		// Time derivative
+		_thrust_sp_filtered_diff = (thrust_sp_filtered_new - _thrust_sp_filtered) / dt_thrust_sp;
+		_thrust_sp_filtered = thrust_sp_filtered_new;
+
+		// Indicate that new data is available to estimate the vehicle mass
+		mass_update = true;
+	}
+
+	// Update inertia estimate only if we have new data and we are flying
+	if (inertia_update && _armed && !_landed && !_maybe_landed) {
+		// Predict angular jerk from time derivative of torque setpoint and estimated inertia
+		Matrix<float, 3, 1> jerk_pred = _I_est_inv * _torque_sp_filtered_diff;
+
+		// Compute prediction error
+		matrix::Matrix<float, 3, 1> error = _ang_accel_filtered_diff - jerk_pred;
+
+		// Update estimate
+		_I_est_inv += error * _torque_sp_filtered_diff.T() * _param_vm_est_gain.get();
+
+		// Sanity check
+		bool reset_needed = false;
+		for (size_t i = 0; i < 3; i++) {
+			for (size_t j = 0; j < 3; j++) {
+				if (not PX4_ISFINITE(_I_est_inv(i, j))) {
+					reset_needed = true;
+				}
+			}
+		}
+
+		if (reset_needed) {
+			_I_est_inv = inv(_I);
+		}
+
+		// Enforce positive values on diagonal
+		for (size_t i = 0; i < 3; i++) {
+			if (_I_est_inv(i, i) < FLT_EPSILON) {
+				_I_est_inv(i, i) = FLT_EPSILON;
+			}
+		}
+
+		// Enforce symmetry
+		_I_est_inv(1, 0) = 0.5f * (_I_est_inv(0, 1) + _I_est_inv(1, 0));
+		_I_est_inv(0, 1) = _I_est_inv(1, 0);
+		_I_est_inv(2, 0) = 0.5f * (_I_est_inv(0, 2) + _I_est_inv(2, 0));
+		_I_est_inv(0, 2) = _I_est_inv(2, 0);
+		_I_est_inv(2, 1) = 0.5f * (_I_est_inv(1, 2) + _I_est_inv(2, 1));
+		_I_est_inv(1, 2) = _I_est_inv(2, 1);
+	}
+
+	// Update mass estimate only if we have new data and we are flying
+	if (mass_update && _armed && !_landed && !_maybe_landed) {
+		// Predict jerk from time derivative of thrust setpoint and estimated mass
+		Vector3f jerk_pred = _mass_est_inv * _thrust_sp_filtered_diff;
+
+		// Compute prediction error
+		Vector3f error = _lin_accel_filtered_diff - jerk_pred;
+
+		// Update estimate
+		_mass_est_inv += error.dot(_torque_sp_filtered_diff) * _param_vm_est_gain.get();
+
+		// Sanity check
+		if (not PX4_ISFINITE(_mass_est_inv)) {
+			_mass_est_inv = 1.0f / _mass;
+		}
+
+		// Enforce positive value
+		if (_mass_est_inv < FLT_EPSILON) {
+			_mass_est_inv = FLT_EPSILON;
+		}
+
+		if ((1.0f / _mass_est_inv) < FLT_EPSILON) {
+			_mass_est_inv = 1.0f / _mass;
+		}
+	}
+
+	// Publish estimated model
+	if ((now - _prev_publish_time) > (1e6f / _param_vm_est_pubrate.get())) {
+		_prev_publish_time = now;
+
+		// Compute inertia matrix and mass
+		Matrix3f I = _I;
+		float mass = _mass;
+		if (_param_vm_est_en.get()) {
+			// Invert mass estimation
+			mass = 1.0f / _mass_est_inv;
+
+			// Try to invert inertia estimation
+			bool invertible = inv(_I_est_inv, I);
+
+			if (not invertible) {
+				// Fallback to default value
+				I = _I;
+				_I_est_inv = inv(_I);
+			}
+		} else {
+			// reset estimation
+			_I_est_inv = inv(_I);
+			_mass_est_inv = 1.0f / _mass;
+		}
+
+		// Publish
+		vehicle_model_s vehicle_model;
+		vehicle_model.timestamp = hrt_absolute_time();
+		vehicle_model.mass = mass;
+		I.copyTo(vehicle_model.inertia);
+		_vehicle_model_pub.publish(vehicle_model);
+	}
+
+	perf_end(_loop_perf);
+}
+
+int VehicleModelEstimator::task_spawn(int argc, char *argv[])
+{
+	VehicleModelEstimator *instance = new VehicleModelEstimator();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int VehicleModelEstimator::print_status()
+{
+	PX4_INFO("Running");
+
+	perf_print_counter(_loop_perf);
+
+	return 0;
+}
+
+int VehicleModelEstimator::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int VehicleModelEstimator::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This implements the estimation of vehicle model (mass & inertia).
+To estimate the vehicle inertia, the estimator low-pass filters and
+computes the time derivative of the vehicle angular acceleration
+and of the vehicle torque setpoint, then applies a least-mean-square.
+Similarly, to estimate the vehicle mass, the estimator low-pass
+filters and computes the time derivative of the vehicle acceleration
+and of the vehicle thrust setpoint, then applies a least-mean-square.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+/**
+ * Multicopter rate control app start / stop handling function
+ */
+extern "C" __EXPORT int vehicle_model_estimator_main(int argc, char *argv[]);
+
+int vehicle_model_estimator_main(int argc, char *argv[])
+{
+	return VehicleModelEstimator::main(argc, argv);
+}

src/modules/vehicle_model_estimator/VehicleModelEstimator.hpp

@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2019 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file VehicleModelEstimator.hpp
+ *
+ * Vehicle model estimator.
+ *
+ * To estimate the vehicle inertia, the estimator low-pass filters and
+ * computes the time derivative of the vehicle angular acceleration
+ * and of the vehicle torque setpoint, then applies a least-mean-square.
+ * Similarly, to estimate the vehicle mass, the estimator low-pass
+ * filters and computes the time derivative of the vehicle acceleration
+ * and of the vehicle thrust setpoint, then applies a least-mean-square.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/ecl/EKF/ekf.h>	// included for RingBuffer
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_acceleration.h>
+#include <uORB/topics/vehicle_angular_acceleration.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_model.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
+
+
+class VehicleModelEstimator : public ModuleBase<VehicleModelEstimator>, public ModuleParams, public px4::WorkItem
+{
+public:
+	VehicleModelEstimator();
+
+	virtual ~VehicleModelEstimator();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	void Run() override;
+
+	bool init();
+
+private:
+
+	/**
+	 * initialize some vectors/matrices from parameters
+	 */
+	void		parameters_updated();
+
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};			/**< parameter updates subscription */
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
+	uORB::Subscription _vehicle_torque_setpoint_sub{ORB_ID(vehicle_torque_setpoint)}; 	/**< vehicle torque setpoint subscription */
+	uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; 	/**< vehicle thrust setpoint subscription */
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};	  	/**< vehicle land detected subscription */
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_angular_acceleration_sub{this, ORB_ID(vehicle_angular_acceleration)};
+	uORB::SubscriptionCallbackWorkItem _vehicle_acceleration_sub{this, ORB_ID(vehicle_acceleration)};
+
+	uORB::Publication<vehicle_model_s>		   _vehicle_model_pub{ORB_ID(vehicle_model)};				/**< vehicle model publication */
+
+	bool _armed{false};
+	bool _landed{true};
+	bool _maybe_landed{true};
+
+	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
+
+	static constexpr const float initial_update_rate_hz = 1000.f; /**< loop update rate used for initialization */
+	hrt_abstime _task_start{hrt_absolute_time()};
+	hrt_abstime _prev_publish_time{0};
+
+	// Inertia estimation data
+	matrix::SquareMatrix3f _I;  		/**< Inertia matrix entered via parameters */
+	matrix::SquareMatrix3f _I_est_inv;      /**< Inverse of estimated inertia matrix */
+
+	// Angular acceleration data
+	math::LowPassFilter2pVector3f _lpf_ang_accel{0.f, 0.f}; 	/**< low-pass filters for angular acceleration */
+	matrix::Vector3f _ang_accel_filtered;				/**< low-pass filtered angular acceleration */
+	matrix::Vector3f _ang_accel_filtered_diff;			/**< time derivative of the low-pass filtered angular acceleration */
+	hrt_abstime _timestamp_ang_accel{0};
+	float _loop_update_rate_hz_ang_accel{initial_update_rate_hz};
+	int _loop_counter_ang_accel{0};
+	float _dt_accumulator_ang_accel{0.0f};
+
+	// Torque setpoint data
+	RingBuffer<vehicle_torque_setpoint_s> _torque_sp_buffer;
+	math::LowPassFilter2pVector3f _lpf_torque_sp{0.f, 0.f}; 	/**< low-pass filters for torque setpoint */
+	matrix::Vector3f _torque_sp_filtered;				/**< low-pass filtered torque setpoint */
+	matrix::Vector3f _torque_sp_filtered_diff;			/**< time derivative of the low-pass filtered torque setpoint */
+	hrt_abstime _timestamp_torque_sp{0};
+	float _loop_update_rate_hz_torque_sp{initial_update_rate_hz};
+	int _loop_counter_torque_sp{0};
+	float _dt_accumulator_torque_sp{0.0f};
+
+	// Mass estimation data
+	float _mass{0.0f};		/**< Mass entered via parameters */
+	float _mass_est_inv{1.0f};	/**< Inverse of estimated mass */
+
+	// Linear acceleration data
+	math::LowPassFilter2pVector3f _lpf_lin_accel{0.f, 0.f}; 	/**< low-pass filters for linear acceleration */
+	matrix::Vector3f _lin_accel_filtered;				/**< low-pass filtered linear acceleration */
+	matrix::Vector3f _lin_accel_filtered_diff;			/**< time derivative of the low-pass filtered linear acceleration */
+	hrt_abstime _timestamp_lin_accel{0};
+	float _loop_update_rate_hz_lin_accel{initial_update_rate_hz};
+	int _loop_counter_lin_accel{0};
+	float _dt_accumulator_lin_accel{0.0f};
+
+	// Thrust setpoint data
+	RingBuffer<vehicle_thrust_setpoint_s> _thrust_sp_buffer;
+	math::LowPassFilter2pVector3f _lpf_thrust_sp{0.f, 0.f}; 	/**< low-pass filters for thrust setpoint */
+	matrix::Vector3f _thrust_sp_filtered;				/**< low-pass filtered thrust setpoint */
+	matrix::Vector3f _thrust_sp_filtered_diff;			/**< time derivative of the low-pass filtered thrust setpoint */
+	hrt_abstime _timestamp_thrust_sp{0};
+	float _loop_update_rate_hz_thrust_sp{initial_update_rate_hz};
+	int _loop_counter_thrust_sp{0};
+	float _dt_accumulator_thrust_sp{0.0f};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::VM_MASS>) _param_vm_mass,			/**< vehicle mass */
+
+		(ParamFloat<px4::params::VM_INERTIA_XX>) _param_vm_inertia_xx,		/**< vehicle inertia */
+		(ParamFloat<px4::params::VM_INERTIA_YY>) _param_vm_inertia_yy,
+		(ParamFloat<px4::params::VM_INERTIA_ZZ>) _param_vm_inertia_zz,
+		(ParamFloat<px4::params::VM_INERTIA_XY>) _param_vm_inertia_xy,
+		(ParamFloat<px4::params::VM_INERTIA_XZ>) _param_vm_inertia_xz,
+		(ParamFloat<px4::params::VM_INERTIA_YZ>) _param_vm_inertia_yz,
+
+		(ParamInt<px4::params::VM_EST_EN>) _param_vm_est_en,
+		(ParamFloat<px4::params::VM_EST_PUBRATE>) _param_vm_est_pubrate,
+		(ParamFloat<px4::params::VM_EST_CUTOFF>) _param_vm_est_cutoff,
+		(ParamFloat<px4::params::VM_EST_GAIN>) _param_vm_est_gain,
+		(ParamFloat<px4::params::VM_EST_DELAY>) _param_vm_est_delay
+	)
+};
+

src/modules/vehicle_model_estimator/vehicle_model_estimator_params.c

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2019 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_model_estimator_params.c
+ * Parameters for the estimation of the vehicle model.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+
+/**
+ * Enables the vehicle model estimation
+ *
+ * @boolean
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_INT32(VM_EST_EN, 1);
+
+/**
+ * Estimated vehicle model publishing rate
+ *
+ * The estimator will always run at maximum rate, updating its
+ * estimate at full rate.
+ * This parameter may be used to limit the rate at which the
+ * resulting estimation is published.
+ *
+ * @unit Hz
+ * @min 0
+ * @max 1000
+ * @decimal 0
+ * @increment 1
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_EST_PUBRATE, 10.0f);
+
+/**
+ * Estimation gain
+ *
+ * A larger value will lead to faster update of the estimated model
+ * but an excessively large value may cause faulty estimation.
+ *
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_EST_GAIN, 0.1f);
+
+/**
+ * Vehicle model estimator cutoff frequency
+ *
+ * Determines the cutoff frequency of the 2nd order low pass filter
+ * applied on control setpoints and sensor data before estimation.
+ *
+ * @min 0
+ * @max 30
+ * @decimal 2
+ * @increment 1
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_EST_CUTOFF, 5.0f);
+
+/**
+ * Vehicle model estimator actuator delay
+ *
+ * Determines the delay applied to the torque and thrust setpoints
+ * in order to take into account the time required for the
+ * setpoints to take effect.
+ * This delay may vary depending on
+ * - whether the setpoints go through an IO board
+ * - the latency of the protocol used to communicate with actuators
+ * - the intrinsic latency of actuators
+ *
+ * @unit s
+ * @min 0
+ * @max 0.5
+ * @decimal 4
+ * @increment 1
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_EST_DELAY, 0.080f);