added shear estimator module, not working yet

ROMFS/px4fmu_common/init.d/rc.fw_apps

@@ -16,6 +16,7 @@ ekf2 start &
 fw_att_control start
 fw_pos_control_l1 start
 fw_dyn_soar_control start
+fw_dyn_soar_estimator start
 airspeed_selector start
 #
 # Start Land Detector.

boards/px4/fmu-v5/default.cmake

@@ -68,6 +68,7 @@ px4_add_board(
 		flight_mode_manager
 		fw_att_control
 		fw_dyn_soar_control
+		fw_dyn_soar_estimator
 		fw_pos_control_l1
 		gyro_calibration
 		gyro_fft

boards/px4/sitl/default.cmake

@@ -37,6 +37,7 @@ px4_add_board(
 		fw_att_control
 		fw_pos_control_l1
 		fw_dyn_soar_control
+		fw_dyn_soar_estimator
 		gyro_calibration
 		gyro_fft
 		land_detector

boards/px4/sitl/test.cmake

@@ -37,6 +37,7 @@ px4_add_board(
 		fw_att_control
 		fw_pos_control_l1
 		fw_dyn_soar_control
+		fw_dyn_soar_estimator
 		gyro_calibration
 		gyro_fft
 		land_detector

msg/CMakeLists.txt

@@ -145,6 +145,7 @@ set(msg_files
 	soaring_controller_position.msg
 	soaring_controller_status.msg
 	soaring_controller_wind.msg
+	soaring_estimator_shear.msg
 	system_power.msg
 	takeoff_status.msg
 	task_stack_info.msg

msg/soaring_controller_wind.msg

@@ -4,3 +4,4 @@ uint64 timestamp			# time since system start (microseconds)
 
 float32[3] wind_estimate                            # WIND ESTIMATE IN ENU FRAME
 float32[3] wind_estimate_filtered                   # LP-FILTERED WIND ESTIMATE IN ENU FRAME, USED BY INDI CONTROLLER
+float32[3] position                                 # position of the current estimate in the soaring frame

msg/soaring_estimator_shear.msg

@@ -0,0 +1,13 @@
+# SOARING ESTIMATOR WIND ESTIMATE, USED FOR SELECTING THE CORRECT TRAJECTORY
+
+uint64 timestamp			# time since system start (microseconds)
+
+float32 vx                  # maximum wind in x-direction
+float32 vy                  # maximum wind in y-direction
+float32 bx                  # wind offset in x-direction
+float32 by                  # wind offset in y-direction
+float32 h                   # vertical position of shear layer in soaring frame
+float32 a                   # shear strength
+
+bool params_healthy         # plausibility check
+uint64 reset_counter        # filter reset counter

src/modules/fw_dyn_soar_estimator/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015-2017 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.
+#
+############################################################################
+
+#add_subdirectory(launchdetection)
+
+px4_add_module(
+	MODULE modules__fw_dyn_soar_estimator
+	MAIN fw_dyn_soar_estimator
+	STACK_MAIN 1024
+	SRCS
+		FixedwingShearEstimator.cpp
+		FixedwingShearEstimator.hpp
+	DEPENDS
+	)

src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp

@@ -0,0 +1,183 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "FixedwingShearEstimator.hpp"
+
+using math::constrain;
+using math::max;
+using math::min;
+using math::radians;
+using matrix::Matrix;
+using matrix::Vector3f;
+using matrix::Vector;
+using matrix::wrap_pi;
+
+
+
+FixedwingShearEstimator::FixedwingShearEstimator() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::test1),
+    _soaring_estimator_shear_pub(ORB_ID(soaring_estimator_shear))
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	// limit to 10 Hz
+	_soaring_controller_wind_sub.set_interval_ms(100.f);
+
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+FixedwingShearEstimator::~FixedwingShearEstimator()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+FixedwingShearEstimator::init()
+{
+	if (!_vehicle_angular_velocity_sub.registerCallback()) {
+		PX4_ERR("vehicle position callback registration failed!");
+		return false;
+	}
+    PX4_INFO("Starting FW_DYN_SOAR_ESTIMATOR");
+	return true;
+}
+
+int
+FixedwingShearEstimator::parameters_update()
+{
+	updateParams();
+
+	// update params...
+    _Q_horizontal(0,0) = powf(_param_sigma_q_vel.get(),2);
+    _Q_horizontal(1,1) = powf(_param_sigma_q_vel.get(),2);
+    _Q_horizontal(2,2) = powf(_param_sigma_q_vel.get(),2);
+    _Q_horizontal(3,3) = powf(_param_sigma_q_vel.get(),2);
+    _Q_horizontal(4,4) = powf(_param_sigma_q_h.get(),2);
+    _Q_horizontal(5,5) = powf(_param_sigma_q_a.get(),2);
+
+    _R_horizontal(0,0) = powf(_param_sigma_r_vel.get(),2);
+    _R_horizontal(1,1) = powf(_param_sigma_r_vel.get(),2);
+
+    for (int i=0;i<_dim_vertical;i++){
+        _Q_vertical(i,i) = powf(_param_sigma_q_vel.get(),2);
+    }
+
+    _R_vertical = powf(_param_sigma_r_vel.get(),2);
+
+	return PX4_OK;
+}
+
+void
+FixedwingShearEstimator::reset_filter()
+{
+    // reset all states of the filter to some initial guess.
+
+    // reset horizontal wind state
+    for (int i=0;i<6;i++){
+        _X_prior_horizontal(i,i) = 0.0f;
+        _X_posterior_horizontal(i,i) = 0.0f;
+        _P_prior_horizontal(i,i) = 1.0f;
+        _P_posterior_horizontal(i,i) = 1.0f;
+    }
+
+    // reset vertical wind state
+    for (int i=0;i<_dim_vertical;i++){
+        _X_prior_vertical(i,i) = 0.0f;
+        _X_posterior_vertical(i,i) = 0.0f;
+        _P_prior_vertical(i,i) = 1.0f;
+        _P_posterior_vertical(i,i) = 1.0f;
+    }
+}
+
+void
+FixedwingShearEstimator::perform_prior_update()
+{
+
+}
+
+void
+FixedwingShearEstimator::perform_posterior_update()
+{
+    
+}
+
+void
+FixedwingPositionINDIControl::Run()
+{
+    if (should_exit()) {
+		_soaring_controller_wind_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+    perf_begin(_loop_perf);
+
+    // only run controller if wind info changed
+	if (_soaring_controller_wind_sub.update(&soaring_controller_wind))
+    {   
+        // only update parameters if they changed
+		bool params_updated = _parameter_update_sub.updated();
+
+		// check for parameter updates
+		if (params_updated) {
+			// clear update
+			parameter_update_s pupdate;
+			_parameter_update_sub.copy(&pupdate);
+
+			// update parameters from storage
+			updateParams();
+			parameters_update();
+		}
+
+        // get current measurement
+        _current_wind = Vector3f(soaring_controller_wind.wind_estimate_filtered);
+        _current_height = Vector3f(soaring_controller_wind.position)(2);
+
+        // prior update
+        perform_prior_update();
+
+        // posterior update
+        perform_posterior_update(_current_height, _current_wind);
+
+        // check if filter diverges 
+        // maybe reset filters...
+
+        // publish shear params
+    }
+
+
+}
+
+

src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp

@@ -0,0 +1,127 @@
+/**
+ * Implementation of a sigmoidal shear EKF for estimating the shear parameters of the wind.
+ * The estimate is then used by the dynmic soaring controller in "fw_dyn_soar_control".
+ *
+ * @author Marvin Harms <marv@teleport.ch>
+ */
+
+// use inclusion guards
+#ifndef FIXEDWINGSHEARESTIMATOR_HPP_
+#define FIXEDWINGSHEARESTIMATOR_HPP_
+
+#include <float.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/px4_config.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/WorkItem.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/soaring_controller_wind.h>
+#include <uORB/topics/soaring_estimator_shear.h>
+#include <uORB/uORB.h>
+
+using namespace time_literals;
+
+using matrix::Dcmf;
+using matrix::Quatf;
+using matrix::Vector;
+using matrix::Matrix;
+using matrix::Matrix3f;
+using matrix::Vector3f;
+
+
+class FixedwingShearEstimator final : public ModuleBase<FixedwingShearEstimator>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	FixedwingShearEstimator();
+	~FixedwingShearEstimator() override;
+
+	/** @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);
+
+	bool init();
+
+private:
+	void Run() override;
+
+	orb_advert_t	_mavlink_log_pub{nullptr};
+
+	// make the main task run, whenever a new body rate becomes available
+	uORB::SubscriptionCallbackWorkItem _soaring_controller_wind_sub{this, ORB_ID(soaring_controller_wind)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+    // Subscriptions
+    //uORB::Subscription _soaring_controller_wind_sub{ORB_ID(soaring_controller_wind)};
+	
+    // Publishers
+	uORB::Publication<soaring_estimator_shear_s> _soaring_estimator_shear_pub{ORB_ID(soaring_estimator_shear)};
+
+    // Message structs
+	soaring_estimator_shear_s	_soaring_estimator_shear{};	///< soaring controller pos 
+	soaring_controller_wind_s	_soaring_controller_wind{};	///< soaring controller wind
+
+	// parameter struct
+	DEFINE_PARAMETERS(
+		// aircraft params
+		(ParamFloat<px4::params::DS_SIGMA_Q_V>) _param_sigma_q_vel,
+        (ParamFloat<px4::params::DS_SIGMA_Q_H>) _param_sigma_q_h,
+        (ParamFloat<px4::params::DS_SIGMA_Q_A>) _param_sigma_q_a,
+        (ParamFloat<px4::params::DS_SIGMA_R_V>) _param_sigma_r_vel
+	)
+
+
+	perf_counter_t	_loop_perf;				///< loop performance counter
+	
+	// Update our local parameter cache.
+	int		parameters_update();
+    void    reset_filter();
+    void    perform_prior_update();
+    void    perform_posterior_update(float height, Vector3f wind);
+    bool    check_plausibility();
+    void    publish_estimate();
+
+    // control variables
+    const static size_t _dim_vertical = 2;  // order of vertical approximation function for vertical wind
+
+	Vector<float, 6> _X_prior_horizontal= {};
+    Matrix<float, 6, 6> _P_prior_horizontal = {};
+    Vector<float, 6> _X_posterior_horizontal= {};	
+    Matrix<float, 6, 6> _P_prosterior_horizontal = {};
+    Matrix<float, 6, 6> _Q_horizontal = {};
+    Matrix<float, 2, 2> _R_horizontal = {};
+    Matrix<float, 2, 6> _H_horizontal = {};
+
+    Vector<float, _dim_vertical> _X_prior_vertical= {};
+    Matrix<float, _dim_vertical, _dim_vertical> _P_prior_vertical = {};
+    Vector<float, _dim_vertical> _X_posterior_vertical= {};
+    Matrix<float, _dim_vertical, _dim_vertical> _P_posterior_vertical = {};
+    Vector<float, _dim_vertical> _X_vertical = {};			// params of vertical wind
+    Matrix<float, _dim_vertical, _dim_vertical> _Q_vertical = {};
+    float _R_vertical = {};
+    Matrix<float, 1, _dim_vertical> _H_vertical = {};
+
+    // measurement variables
+    Vector3f _current_wind = {};
+    float _current_height = {};
+
+};
+
+
+#endif // FIXEDWINGSHEARESTIMATOR_HPP_

src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c

@@ -0,0 +1,67 @@
+/**
+ * @file fw_dyn_soar_estimator_params.c
+ *
+ * Parameters defined by the INDI position controller
+ *
+ * @author Marvin Harms <marv@teleport.ch>
+ */
+
+/*
+ * Controller parameters, accessible via MAVLink
+ */
+
+/**
+ * Standard deviation of velicity state in shear model
+ * 
+ * This is the std dev of the wind velocity in each direction
+ * 
+ * @unit kg
+ * @min 0.01
+ * @max 10
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 1.f);
+
+/**
+ * Standard deviation of vertical shear position
+ * 
+ * This is the std dev of the shear vertical position
+ * 
+ * @unit kg
+ * @min 0.01
+ * @max 10
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 1.f);
+
+/**
+ * Standard deviation of velicity state in shear model
+ * 
+ * This is the std dev of the shear strenght param
+ * 
+ * @unit kg
+ * @min 0.01
+ * @max 10
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 1.f);
+
+/**
+ * Standard deviation of velicity measurement (wind)
+ * 
+ * This is the std dev of the wind pseudomeasurement passed to the EKF
+ * 
+ * @unit kg
+ * @min 0.01
+ * @max 10
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 1.f);