fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-10 10:28:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

added feasibility check

Browse Source
This commit is contained in:
Marvin Harms
2022-08-19 11:53:52 +02:00
parent 73eab1710e
commit 99c04bb12b
5 changed files with 55 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/soaring_estimator_shear.msg
+1 -1
View File
@@ -16,5 +16,5 @@ float32 sigma_by # covariance of by
float32 sigma_h # covariance of h
float32 sigma_a # covariance of a
bool params_healthy # plausibility check
bool soaring_feasible # plausibility check
uint64 reset_counter # filter reset counter
src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv
+3 -3
View File
@@ -1,3 +1,3 @@
34.908707,2802.143443,-12010.045206,24897.004025,-31652.414085,24086.196989,-4942.033085,-13772.898892,20576.241530,-12668.675896,-4555.658999,21674.320937,-30001.814698,25832.757640,-13941.536701,3638.740016
-1.920001,7042.626425,-38404.376575,103774.671218,-174237.276934,184373.717471,-89448.645248,-70134.302861,177608.190474,-136819.046222,-30572.012263,196546.815165,-245393.925168,174924.844259,-73416.451006,14256.585131
90.837687,134.620712,-674.566268,1031.769726,671.319554,-5057.527415,8440.006743,-4910.847468,-5701.223344,13715.392464,-8181.508398,-9147.528585,23538.196976,-23336.500927,12535.088204,-3052.472443
33.279737,2355.965009,-9853.856406,19962.238258,-24975.068263,19045.215851,-4528.565470,-10036.742015,16387.989801,-11464.538539,-2278.904099,17753.913430,-26381.541512,23421.451272,-12798.876432,3352.799155
-1.655556,6955.844338,-37836.458976,101707.858193,-169407.588331,177059.774008,-83213.665962,-70079.333339,169854.280775,-127407.321325,-32264.742913,186788.856870,-229810.067680,162394.940106,-67738.519051,13090.706363
90.256827,695.796077,-3566.592598,8070.766654,-9651.849655,4159.119473,5301.738775,-9269.047782,2572.483886,7898.748320,-9780.034074,-66.886050,11734.271409,-14380.432089,8489.722642,-2195.005657
1 34.908707 33.279737 2802.143443 2355.965009 -12010.045206 -9853.856406 24897.004025 19962.238258 -31652.414085 -24975.068263 24086.196989 19045.215851 -4942.033085 -4528.565470 -13772.898892 -10036.742015 20576.241530 16387.989801 -12668.675896 -11464.538539 -4555.658999 -2278.904099 21674.320937 17753.913430 -30001.814698 -26381.541512 25832.757640 23421.451272 -13941.536701 -12798.876432 3638.740016 3352.799155
2 -1.920001 -1.655556 7042.626425 6955.844338 -38404.376575 -37836.458976 103774.671218 101707.858193 -174237.276934 -169407.588331 184373.717471 177059.774008 -89448.645248 -83213.665962 -70134.302861 -70079.333339 177608.190474 169854.280775 -136819.046222 -127407.321325 -30572.012263 -32264.742913 196546.815165 186788.856870 -245393.925168 -229810.067680 174924.844259 162394.940106 -73416.451006 -67738.519051 14256.585131 13090.706363
3 90.837687 90.256827 134.620712 695.796077 -674.566268 -3566.592598 1031.769726 8070.766654 671.319554 -9651.849655 -5057.527415 4159.119473 8440.006743 5301.738775 -4910.847468 -9269.047782 -5701.223344 2572.483886 13715.392464 7898.748320 -8181.508398 -9780.034074 -9147.528585 -66.886050 23538.196976 11734.271409 -23336.500927 -14380.432089 12535.088204 8489.722642 -3052.472443 -2195.005657
src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
+48 -11
View File
@@ -87,8 +87,8 @@ FixedwingShearEstimator::init()
_A_horizontal(i,j) = 0.f;
}
}
_P_prior_horizontal = _Q_horizontal;
_P_posterior_horizontal = _Q_horizontal;
_P_prior_horizontal = 3.f*_Q_horizontal;
_P_posterior_horizontal = 3.f*_Q_horizontal;
// init vertical wind field
for (uint i=0; i<_dim_vertical; i++){
@@ -98,8 +98,8 @@ FixedwingShearEstimator::init()
_A_vertical(i,j) = 0.f;
}
}
_P_prior_vertical = _Q_vertical;
_P_posterior_vertical = _Q_vertical;
_P_prior_vertical = 3.f*_Q_vertical;
_P_posterior_vertical = 3.f*_Q_vertical;
//
@@ -156,16 +156,16 @@ FixedwingShearEstimator::reset_filter()
_X_prior_horizontal(i) = 0.0f;
_X_posterior_horizontal(i) = 0.0f;
}
_P_prior_horizontal = _Q_horizontal;
_P_posterior_horizontal = _Q_horizontal;
_P_prior_horizontal = 3.f*_Q_horizontal;
_P_posterior_horizontal = 3.f*_Q_horizontal;
// reset vertical wind state
for (uint i=0;i<_dim_vertical;i++){
_X_prior_vertical(i) = 0.0f;
_X_posterior_vertical(i) = 0.0f;
}
_P_prior_vertical = _Q_vertical;
_P_posterior_vertical = _Q_vertical;
_P_prior_vertical = 3.f*_Q_vertical;
_P_posterior_vertical = 3.f*_Q_vertical;
// set height to enable convergence
_X_prior_horizontal(4) = _init_height;
@@ -181,8 +181,8 @@ FixedwingShearEstimator::perform_prior_update()
// get time since last run
float dt = (float)(hrt_absolute_time() - _last_run)/1000000.f;
_last_run = hrt_absolute_time();
if (dt>10.f) {
dt = 10.f;
if (dt>1.f) {
dt = 1.f;
}
// perform prior update assuming trivial dynamics of the wind field (mean field stays the same)
@@ -334,10 +334,11 @@ FixedwingShearEstimator::Run()
// check if filter diverges
// maybe reset filters...
if (sqrtf(_P_posterior_horizontal(4,4))>=6.f||sqrtf(_P_posterior_horizontal(5,5))>=0.2f) {
if (sqrtf(_P_posterior_horizontal(4,4))>=5.f||sqrtf(_P_posterior_horizontal(5,5))>=0.3f) {
PX4_WARN("large height uncertainty, resetting filter");
reset_filter();
}
}
// publish shear params
@@ -357,6 +358,7 @@ FixedwingShearEstimator::Run()
_soaring_estimator_shear.sigma_by = sqrtf(_P_posterior_horizontal(3,3))*_unit_v;
_soaring_estimator_shear.sigma_h = sqrtf(_P_posterior_horizontal(4,4))*_unit_h;
_soaring_estimator_shear.sigma_a = sqrtf(_P_posterior_horizontal(5,5))*_unit_a;
_soaring_estimator_shear.soaring_feasible = check_feasibility();
_soaring_estimator_shear.reset_counter = _reset_counter;
_soaring_estimator_shear_pub.publish(_soaring_estimator_shear);
}
@@ -364,6 +366,41 @@ FixedwingShearEstimator::Run()
}
bool FixedwingShearEstimator::check_feasibility()
{
// ================================================================
// simple check, if dynamic soaring is feasible in these conditions
// ================================================================
float shear_x = _X_posterior_horizontal(0) - _X_posterior_horizontal(2);
float shear_y = _X_posterior_horizontal(1) - _X_posterior_horizontal(3);
float shear_strength = _X_posterior_horizontal(5);
//float heading = atan2f(shear_x, shear_y);
//float heading_stdev = 0.f;
// require shear strength above 10 m/s
float shear = sqrtf(powf(shear_x,2) + powf(shear_y,2));
if (shear<10.f) {
return false;
}
// require shear strength above 0.3 1/s
if (shear_strength<0.3f) {
return false;
}
// require covariance certainty below 2 m/s (use propagation of variance)
float shear_stdev = sqrtf(powf(shear_x/shear,2)*_P_posterior_horizontal(0,0) +
powf(shear_x/shear,2)*_P_posterior_horizontal(2,2) +
powf(shear_y/shear,2)*_P_posterior_horizontal(1,1) +
powf(shear_y/shear,2)*_P_posterior_horizontal(3,3));
if (shear_stdev>3.f) {
return false;
}
return true;
}
int FixedwingShearEstimator::task_spawn(int argc, char *argv[])
{
src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp
+1 -1
View File
@@ -95,7 +95,7 @@ private:
void reset_filter();
void perform_prior_update();
void perform_posterior_update(float height, Vector3f wind);
bool check_plausibility();
bool check_feasibility();
void publish_estimate();
void status_publish();
src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c
+2 -2
View File
@@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.3f);
* @increment 0.000001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.03f);
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.02f);
/**
* Standard deviation of velicity measurement (wind)
@@ -78,4 +78,4 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 3.f);
* @increment 0.1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_INIT_H, 110.f);
PARAM_DEFINE_FLOAT(DS_INIT_H, 90.f);