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

tuned shear estimator covariances

Browse Source
This commit is contained in:
Marvin Harms
2022-08-18 11:42:10 +02:00
parent f5a8b9ac2d
commit 73eab1710e
2 changed files with 6 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
+1 -6
View File
@@ -201,8 +201,6 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
// first fill the horizontal observation matrix:
float vx = _X_prior_horizontal(0);
float vy = _X_prior_horizontal(1);
//float bx = _X_prior_horizontal(1);
//float by = _X_prior_horizontal(3);
float h = _X_prior_horizontal(4);
float a = _X_prior_horizontal(5);
@@ -262,7 +260,6 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
Matrix<float, 6, 6> identity_1;
z_expected_horizontal(0) = _X_prior_horizontal(0)/(1.f+expf(-_X_prior_horizontal(5)*(_current_height-_X_prior_horizontal(4)))) + _X_prior_horizontal(2);
z_expected_horizontal(1) = _X_prior_horizontal(1)/(1.f+expf(-_X_prior_horizontal(5)*(_current_height-_X_prior_horizontal(4)))) + _X_prior_horizontal(3);
z_expected_horizontal.print();
wind_horizontal(0) = _current_wind(0);
wind_horizontal(1) = _current_wind(1);
identity_1.setIdentity();
@@ -335,11 +332,9 @@ FixedwingShearEstimator::Run()
// posterior update
perform_posterior_update(_current_height, _current_wind);
// check if filter diverges
// maybe reset filters...
if (sqrtf(_P_posterior_horizontal(4,4))>=10.f) {
if (sqrtf(_P_posterior_horizontal(4,4))>=6.f||sqrtf(_P_posterior_horizontal(5,5))>=0.2f) {
PX4_WARN("large height uncertainty, resetting filter");
reset_filter();
}
src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c
+5 -5
View File
@@ -22,7 +22,7 @@
* @increment 0.000001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.01f);
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.3f);
/**
* Standard deviation of vertical shear position
@@ -36,7 +36,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.01f);
* @increment 0.000001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.1f);
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.3f);
/**
* Standard deviation of velicity state in shear model
@@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.1f);
* @increment 0.000001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.003f);
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.03f);
/**
* Standard deviation of velicity measurement (wind)
@@ -64,7 +64,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.003f);
* @increment 0.000001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 4.f);
PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 3.f);
/**
* Initial guess of the vertical shear position
@@ -78,4 +78,4 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 4.f);
* @increment 0.1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_INIT_H, 100.f);
PARAM_DEFINE_FLOAT(DS_INIT_H, 110.f);