From 33e7ec38820db376d3bf3d20b5be0020c85a9f22 Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Mon, 8 Aug 2022 16:11:36 +0200 Subject: [PATCH] added prior update and posterior update --- .../FixedwingShearEstimator.cpp | 229 ++++++++++++++++-- .../FixedwingShearEstimator.hpp | 12 +- .../fw_dyn_soar_estimator_params.c | 30 +-- 3 files changed, 238 insertions(+), 33 deletions(-) diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp index 217c3c8b5e..fdf040c0b8 100644 --- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp +++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp @@ -47,7 +47,7 @@ using matrix::wrap_pi; FixedwingShearEstimator::FixedwingShearEstimator() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::test1), - _soaring_estimator_shear_pub(ORB_ID(soaring_estimator_shear)) + _soaring_estimator_shear_pub(ORB_ID(soaring_estimator_shear)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { // limit to 10 Hz @@ -66,12 +66,41 @@ FixedwingShearEstimator::~FixedwingShearEstimator() bool FixedwingShearEstimator::init() { - if (!_vehicle_angular_velocity_sub.registerCallback()) { + if (!_soaring_controller_wind_sub.registerCallback()) { PX4_ERR("vehicle position callback registration failed!"); return false; } PX4_INFO("Starting FW_DYN_SOAR_ESTIMATOR"); return true; + + // init horizontal wind field + for (uint i=0; i<6; i++){ + _X_prior_horizontal(i) = 0.f; + _X_posterior_horizontal(i) = 0.f; + for (uint j=0; j<6; j++){ + _A_horizontal(i,j) = 0.f; + } + } + _P_prior_horizontal = _Q_horizontal; + _P_posterior_horizontal = _Q_horizontal; + + // init vertical wind field + for (uint i=0; i<_dim_vertical; i++){ + _X_prior_vertical(i) = 0.f; + _X_posterior_vertical(i) = 0.f; + for (uint j=0; j<_dim_vertical; j++){ + _A_vertical(i,j) = 0.f; + } + } + _P_prior_vertical = _Q_vertical; + _P_posterior_vertical = _Q_vertical; + + // init time + _last_run = hrt_absolute_time(); + + // init reset counter + _reset_counter = 0; + } int @@ -90,11 +119,11 @@ FixedwingShearEstimator::parameters_update() _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++){ + for (int i=0;i<(int)_dim_vertical;i++){ _Q_vertical(i,i) = powf(_param_sigma_q_vel.get(),2); } - _R_vertical = powf(_param_sigma_r_vel.get(),2); + _R_vertical(0,0) = powf(_param_sigma_r_vel.get(),2); return PX4_OK; } @@ -105,17 +134,17 @@ 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; + for (uint i=0;i<6;i++){ + _X_prior_horizontal(i) = 0.0f; + _X_posterior_horizontal(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; + for (uint i=0;i<_dim_vertical;i++){ + _X_prior_vertical(i) = 0.0f; + _X_posterior_vertical(i) = 0.0f; _P_prior_vertical(i,i) = 1.0f; _P_posterior_vertical(i,i) = 1.0f; } @@ -124,17 +153,116 @@ FixedwingShearEstimator::reset_filter() void FixedwingShearEstimator::perform_prior_update() { + // get time since last run + float dt = (hrt_absolute_time() - _last_run)/1000000.f; + _last_run = hrt_absolute_time(); + // perform prior update assuming trivial dynamics of the wind field (mean field stays the same) + _X_prior_horizontal = _X_posterior_horizontal; + _X_prior_vertical = _X_posterior_vertical; + _P_prior_horizontal = _P_posterior_horizontal + dt*_Q_horizontal; + _P_prior_vertical = _P_posterior_vertical + dt*_Q_vertical; } void -FixedwingShearEstimator::perform_posterior_update() +FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind) { - + // + bool error = false; + + // first fill the horizontal observation matrix: + float vx = _X_prior_horizontal(0); + float vy = _X_prior_horizontal(1); + float h = _X_prior_horizontal(4); + float a = _X_prior_horizontal(5); + _H_horizontal(0,0) = 1.f/(1.f + expf(-(height-h)*a)); + _H_horizontal(1,1) = 1.f/(1.f + expf(-(height-h)*a)); + _H_horizontal(0,2) = 1.f; + _H_horizontal(1,3) = 1.f; + _H_horizontal(0,4) = -((a*vx*expf(-(height-h)*a)))/powf(1.f + expf(-(height-h)*a),2); + _H_horizontal(1,4) = -((a*vy*expf(-(height-h)*a)))/powf(1.f + expf(-(height-h)*a),2); + _H_horizontal(0,5) = -((vx*(h-height))*expf(-(height-h)*a))/powf(1.f + expf(-(height-h)*a),2); + _H_horizontal(1,5) = -((vx*(h-height))*expf(-(height-h)*a))/powf(1.f + expf(-(height-h)*a),2); + + // then fill the vertical observation matrix + for (uint i=0;i<_dim_vertical;i++){ + _H_vertical(0,i) = powf(height,i); + } + + // compute Kalman gain matrix for horizontal wind states + Matrix tmp1_horizontal = _P_prior_horizontal*_H_horizontal.T(); + Matrix tmp2_horizontal = _H_horizontal*_P_prior_horizontal*_H_horizontal.T() + _R_horizontal; + Matrix inv_horizontal; + inv_horizontal(0,0) = tmp2_horizontal(1,1); + inv_horizontal(0,1) = -tmp2_horizontal(0,1); + inv_horizontal(1,1) = tmp2_horizontal(0,0); + inv_horizontal(1,0) = -tmp2_horizontal(1,0); + float determinant_horizontal = tmp2_horizontal(0,0)*tmp2_horizontal(1,1) - tmp2_horizontal(1,0)*tmp2_horizontal(0,1); + if (fabs(determinant_horizontal)>0.000001f){ + inv_horizontal /= determinant_horizontal; + _K_horizontal = tmp1_horizontal*inv_horizontal; + } + else{ + PX4_ERR("singular horizontal matrix, resetting filter"); + error = true; + } + + // then fill the vertical observation matrix + for (uint i=0;i<_dim_vertical;i++){ + _H_vertical(0,i) = powf(height,i); + } + + // compute Kalman gain matrix for vertical wind states + Matrix tmp1_vertical = _P_prior_vertical*_H_vertical.T(); + Matrix tmp2_vertical = _H_vertical*_P_prior_vertical*_H_vertical.T() + _R_vertical; + Matrix inv_vertical; + float determinant_vertical = tmp2_vertical(0,0); + if (fabs(determinant_vertical)>0.000001f){ + inv_vertical(0,0) = 1.f / determinant_vertical; + _K_vertical = tmp1_vertical*inv_vertical; + } + else{ + PX4_ERR("singular vertical matrix, resetting filter"); + error = true; + } + + if (error) { + reset_filter(); + _reset_counter += 1; + } + else { + // perform horizontal update + Vector z_expected_horizontal = (Vector) (_H_horizontal*_X_prior_horizontal); + Vector wind_horizontal; + Matrix identity_1; + wind_horizontal(0) = wind(0); + wind_horizontal(1) = wind(1); + identity_1.setIdentity(); + _X_posterior_horizontal = _X_prior_horizontal + _K_horizontal*(wind_horizontal - z_expected_horizontal); + _P_posterior_horizontal = (identity_1 - _K_horizontal*_H_horizontal)*_P_prior_horizontal; + + // perform vertical update + Vector z_expected_vertical = (Vector) (_H_vertical*_X_prior_vertical); + Vector wind_vertical; + Matrix identity_2; + wind_vertical(0) = wind(2); + identity_2.setIdentity(); + _X_posterior_vertical = _X_prior_vertical + _K_vertical*(wind_vertical - z_expected_vertical); + _P_posterior_vertical = (identity_2 - _K_vertical*_H_vertical)*_P_prior_vertical; + } + + // find the correct sign of params (parametrization is not unique) + if (_X_posterior_horizontal(5)<0.f) { + _X_posterior_horizontal(0) *= -1.f; + _X_posterior_horizontal(1) *= -1.f; + _X_posterior_horizontal(5) *= -1.f; + } + + } void -FixedwingPositionINDIControl::Run() +FixedwingShearEstimator::Run() { if (should_exit()) { _soaring_controller_wind_sub.unregisterCallback(); @@ -145,7 +273,7 @@ FixedwingPositionINDIControl::Run() perf_begin(_loop_perf); // only run controller if wind info changed - if (_soaring_controller_wind_sub.update(&soaring_controller_wind)) + if (_soaring_controller_wind_sub.update(&_soaring_controller_wind)) { // only update parameters if they changed bool params_updated = _parameter_update_sub.updated(); @@ -162,8 +290,8 @@ FixedwingPositionINDIControl::Run() } // get current measurement - _current_wind = Vector3f(soaring_controller_wind.wind_estimate_filtered); - _current_height = Vector3f(soaring_controller_wind.position)(2); + _current_wind = Vector3f(_soaring_controller_wind.wind_estimate_filtered); + _current_height = Vector3f(_soaring_controller_wind.position)(2); // prior update perform_prior_update(); @@ -175,9 +303,78 @@ FixedwingPositionINDIControl::Run() // maybe reset filters... // publish shear params + // ======================================== + // publish controller position in ENU frame + // ======================================== + _soaring_estimator_shear.timestamp = hrt_absolute_time(); + _soaring_estimator_shear.vx = _X_posterior_horizontal(0); + _soaring_estimator_shear.vy = _X_posterior_horizontal(1); + _soaring_estimator_shear.bx = _X_posterior_horizontal(2); + _soaring_estimator_shear.bx = _X_posterior_horizontal(3); + _soaring_estimator_shear.h = _X_posterior_horizontal(4); + _soaring_estimator_shear.a = _X_posterior_horizontal(5); + _soaring_estimator_shear.reset_counter = _reset_counter; + _soaring_estimator_shear_pub.publish(_soaring_estimator_shear); } } +int FixedwingShearEstimator::task_spawn(int argc, char *argv[]) +{ + FixedwingShearEstimator *instance = new FixedwingShearEstimator(); + + 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 FixedwingShearEstimator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int FixedwingShearEstimator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +fw_dyn_soar_estimator is the fixed wing shear estimator for dynamic soaring. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("fw_dyn_soar_estimator", "estimator"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int fw_dyn_soar_estimator_main(int argc, char *argv[]) +{ + return FixedwingShearEstimator::main(argc, argv); +} + + + diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp index b3c2e89ca7..1fb553fe8a 100644 --- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp +++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp @@ -96,17 +96,23 @@ private: void perform_posterior_update(float height, Vector3f wind); bool check_plausibility(); void publish_estimate(); + void status_publish(); // control variables + hrt_abstime _last_run{0}; + uint _reset_counter = {}; const static size_t _dim_vertical = 2; // order of vertical approximation function for vertical wind Vector _X_prior_horizontal= {}; Matrix _P_prior_horizontal = {}; Vector _X_posterior_horizontal= {}; - Matrix _P_prosterior_horizontal = {}; + Matrix _P_posterior_horizontal = {}; Matrix _Q_horizontal = {}; Matrix _R_horizontal = {}; Matrix _H_horizontal = {}; + Matrix _A_horizontal = {}; + Matrix _K_horizontal = {}; + Vector _X_prior_vertical= {}; Matrix _P_prior_vertical = {}; @@ -114,8 +120,10 @@ private: Matrix _P_posterior_vertical = {}; Vector _X_vertical = {}; // params of vertical wind Matrix _Q_vertical = {}; - float _R_vertical = {}; + Matrix _R_vertical = {}; Matrix _H_vertical = {}; + Matrix _A_vertical = {}; + Matrix _K_vertical = {}; // measurement variables Vector3f _current_wind = {}; diff --git a/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c b/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c index 39d0fe4353..1e3eb32aad 100644 --- a/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c +++ b/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c @@ -16,13 +16,13 @@ * This is the std dev of the wind velocity in each direction * * @unit kg - * @min 0.01 + * @min 0.0 * @max 10 - * @decimal 2 - * @increment 0.01 + * @decimal 6 + * @increment 0.000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 1.f); +PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.0001f); /** * Standard deviation of vertical shear position @@ -30,13 +30,13 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 1.f); * This is the std dev of the shear vertical position * * @unit kg - * @min 0.01 + * @min 0.0 * @max 10 - * @decimal 2 - * @increment 0.01 + * @decimal 6 + * @increment 0.000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 1.f); +PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.001f); /** * Standard deviation of velicity state in shear model @@ -44,13 +44,13 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 1.f); * This is the std dev of the shear strenght param * * @unit kg - * @min 0.01 + * @min 0.0 * @max 10 - * @decimal 2 - * @increment 0.01 + * @decimal 6 + * @increment 0.000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 1.f); +PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.00003f); /** * Standard deviation of velicity measurement (wind) @@ -58,10 +58,10 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 1.f); * This is the std dev of the wind pseudomeasurement passed to the EKF * * @unit kg - * @min 0.01 + * @min 0.0 * @max 10 - * @decimal 2 - * @increment 0.01 + * @decimal 6 + * @increment 0.000001 * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 1.f); \ No newline at end of file