working soaring estimator shear

src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp

@@ -70,7 +70,8 @@ FixedwingShearEstimator::init()
 		return false;
 	}
     PX4_INFO("Starting FW_DYN_SOAR_ESTIMATOR");
-	return true;
+
+    parameters_update();
 
     // init horizontal wind field
     for (uint i=0; i<6; i++){
@@ -94,16 +95,16 @@ FixedwingShearEstimator::init()
     _P_prior_vertical = _Q_vertical;
     _P_posterior_vertical = _Q_vertical;
 
+    //
+    _X_prior_horizontal(4) = _init_height;
+    _X_posterior_horizontal(4) = _init_height;
+
     // init time
     _last_run = hrt_absolute_time();
 
     // init reset counter
     _reset_counter = 0;
 
-    //
-    _X_prior_horizontal(4) = 110.f;
-    _X_posterior_horizontal(4) = 110.f;
-
     return true;
 }
 
@@ -129,6 +130,8 @@ FixedwingShearEstimator::parameters_update()
 
     _R_vertical(0,0) = powf(_param_sigma_r_vel.get(),2);
 
+    _init_height = _param_init_h.get();
+
 	return PX4_OK;
 }
 
@@ -152,14 +155,24 @@ FixedwingShearEstimator::reset_filter()
         _P_prior_vertical(i,i) = 1.0f;
         _P_posterior_vertical(i,i) = 1.0f;
     }
+
+    // set height to enable convergence
+    _X_prior_horizontal(4) = _init_height;
+    _X_posterior_horizontal(4) = _init_height;
+
+    // increment counter
+    _reset_counter += 1;
 }
 
 void
 FixedwingShearEstimator::perform_prior_update()
 {
     // get time since last run
-    float dt = (hrt_absolute_time() - _last_run)/1000000;
+    float dt = (float)(hrt_absolute_time() - _last_run)/1000000.f;
     _last_run = hrt_absolute_time();
+    if (dt>10.f) {
+        dt = 10.f;
+    }
 
     // perform prior update assuming trivial dynamics of the wind field (mean field stays the same)
     _X_prior_horizontal = _X_posterior_horizontal;
@@ -232,7 +245,6 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
 
     if (error) {
         reset_filter();
-        _reset_counter += 1;
     }
     else {
         // perform horizontal update
@@ -256,12 +268,13 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
     }
 
     // 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;
     }
-
+    */
 
 }
 
@@ -292,7 +305,6 @@ FixedwingShearEstimator::Run()
 			updateParams();
 			parameters_update();
 		}
-
         // get current measurement
         _current_wind = Vector3f(_soaring_controller_wind.wind_estimate_filtered);
         _current_height = Vector3f(_soaring_controller_wind.position)(2);
@@ -305,6 +317,7 @@ FixedwingShearEstimator::Run()
 
         // check if filter diverges 
         // maybe reset filters...
+        //reset_filter();
 
         // publish shear params
         // ========================================

src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp

@@ -83,7 +83,8 @@ private:
 		(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
+        (ParamFloat<px4::params::DS_SIGMA_R_V>) _param_sigma_r_vel,
+        (ParamFloat<px4::params::DS_INIT_H>) _param_init_h
 	)
 
 
@@ -129,6 +130,9 @@ private:
     Vector3f _current_wind = {};
     float _current_height = {};
 
+    // helper variables
+    float _init_height = {};
+
 };
 
 

src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c

@@ -15,53 +15,67 @@
  * 
  * This is the std dev of the wind velocity in each direction
  * 
- * @unit kg
+ * @unit m
  * @min 0.0
  * @max 10
  * @decimal 6
  * @increment 0.000001
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.001f);
+PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.1f);
 
 /**
  * Standard deviation of vertical shear position
  * 
  * This is the std dev of the shear vertical position
  * 
- * @unit kg
+ * @unit m
  * @min 0.0
  * @max 10
  * @decimal 6
  * @increment 0.000001
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.01f);
+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
+ * @unit m
  * @min 0.0
  * @max 10
  * @decimal 6
  * @increment 0.000001
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.0003f);
+PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.03f);
 
 /**
  * Standard deviation of velicity measurement (wind)
  * 
  * This is the std dev of the wind pseudomeasurement passed to the EKF
  * 
- * @unit kg
+ * @unit m
  * @min 0.0
  * @max 10
  * @decimal 6
  * @increment 0.000001
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 1.f);
+PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 1.f);
+
+/**
+ * Initial guess  of the vertical shear position
+ * 
+ * This is the filter initial value of the shear vertical position
+ * 
+ * @unit m
+ * @min -200
+ * @max 200
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_INIT_H, 60.f);