fixed vertical shear to constant assumption

msg/soaring_estimator_shear.msg

@@ -16,5 +16,9 @@ float32 sigma_by            # covariance of by
 float32 sigma_h             # covariance of h
 float32 sigma_a             # covariance of a
 
+float32 coeff_0             # offset vertical wind
+float32 coeff_1             # linear coeff vertical wind
+float32 coeff_2             # quadratic coeff vertical wind
+
 bool soaring_feasible       # plausibility check
 uint64 reset_counter        # filter reset counter

src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp

@@ -216,7 +216,7 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
 
     // then fill the vertical observation matrix
     for (uint i=0;i<_dim_vertical;i++){
-        _H_vertical(0,i) = powf(height,i);
+        _H_vertical(0,i) = powf(height-h,i);
     }
 
     // compute Kalman gain matrix for horizontal wind states
@@ -358,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.coeff_0 = _X_posterior_vertical(0);
         _soaring_estimator_shear.soaring_feasible = check_feasibility();
         _soaring_estimator_shear.reset_counter = _reset_counter;
         _soaring_estimator_shear_pub.publish(_soaring_estimator_shear);

src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp

@@ -102,7 +102,7 @@ private:
     // 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
+    const static size_t _dim_vertical = 1;  // order of vertical approximation function for vertical wind
 
 	Vector<float, 6> _X_prior_horizontal= {};
     Matrix<float, 6, 6> _P_prior_horizontal = {};