improve bias estimator indoor, bug fixes in arming locks

*add enable/disable handlers for estimator
*map lock and mode switch service calls made only at events
*allow estimator to recover gracefully from on/off events

Author: ajshank

autopilot_handler/apm_handler/src/mavros_translate.cpp

@@ -109,27 +109,29 @@ void mavrosStateCallback( const mavros_msgs::State::ConstPtr &msg )
   {
     vehicle_armed_ = true;
     lockreq.request.data = true;
+    map_lock.call( lockreq );
   }
   else if( msg->armed == false && vehicle_armed_ )
   {
     vehicle_armed_ = false;
     lockreq.request.data = false;
+    map_lock.call( lockreq );
   }
-  map_lock.call( lockreq );
 
   /* call bias compensation service when switching in/out of computer */
   std_srvs::SetBool biasreq;
   if( msg->mode == "CMODE(25)" && !in_comp_mode_ )
   {
     in_comp_mode_ = true;
     biasreq.request.data = true;
+    bias_comp.call( biasreq );
   }
   else if( msg->mode != "CMODE(25)" && in_comp_mode_ )
   {
     in_comp_mode_ = false;
     biasreq.request.data = false;
+    bias_comp.call( biasreq );
   }
-  bias_comp.call( biasreq );
 }
 
 

freyja_controller.launch

@@ -44,7 +44,7 @@
 	  <param name="controller_rate"   type="int"    value="45"  />
   	<param name="total_mass"        type="double" value="$(arg total_mass)"/>
   	<param name="use_stricter_gains" type="bool"  value="false" />
-  	<param name="estimator_rate"    type="int"    value="10" />
+  	<param name="estimator_rate"    type="int"    value="50" />
   	<param name="bias_compensation" type="string" value="$(arg bias_compensation)" />
   	<param name="mass_estimation"   type="bool"   value="true" />
   	<param name="mass_correction"   type="bool"   value="false" />

lqg_controller/include/bias_estimator.h

@@ -54,6 +54,7 @@ class BiasEstimator
 
   /* parameters */
   int estimator_rate_, estimator_period_us_;
+  bool estimator_off_;
 
   /* prevent state propagation when other callbacks are happening */
   std::mutex state_prop_mutex_;
@@ -96,11 +97,16 @@ class BiasEstimator
     void getEstimatedBiases( Eigen::Matrix<double, 3, 1> & );
 
     /* set and reset accessors */
-    inline void markEnabled() { t_estimator_on_ = ClockTime::now(); }
-    inline void clearBiases()
+    inline void enable()
+    {
+      t_estimator_on_ = ClockTime::now();
+      estimator_off_ = false;
+    }
+    inline void disable()
     {
       best_estimate_.tail<3>() << 0.0, 0.0, 0.0;
       estimator_output_shaping_ = 0.0;
+      estimator_off_ = true;
     }
 
     /* Time constant calc */

lqg_controller/include/lqr_control_bias.h

@@ -68,6 +68,7 @@ class LQRController
   /* Bias estimator reference object */
   BiasEstimator &bias_est_;
   bool bias_compensation_req_;
+  bool bias_compensation_off_;
   Eigen::Matrix<double, 3, 1> f_biases_;
   std::thread bias_handler_thread_;
 

lqg_controller/src/bias_estimator.cpp

@@ -42,7 +42,7 @@ BiasEstimator::BiasEstimator() : nh_(), priv_nh_("~")
   
     Use thread libraries instead:
   */
-
+  estimator_off_ = false;
   state_propagation_alive_ = true;
   n_stprops_since_update_ = 0;
   last_prop_t_ = std::chrono::high_resolution_clock::now();
@@ -80,10 +80,10 @@ void BiasEstimator::initEstimatorSystem()
 
   /* Numbers pulled out of a magical hat only available to the deserving few */
   proc_noise_Q_ << 0.1*IDEN3x3, ZERO3x3, ZERO3x3,
-                   ZERO3x3, 0.1*IDEN3x3, ZERO3x3,
-                   ZERO3x3, ZERO3x3, 0.5*IDEN3x3;
-  meas_noise_R_ << 0.25*IDEN3x3, ZERO3x3,
-                   ZERO3x3, 0.5*IDEN3x3;
+                   ZERO3x3, 0.2*IDEN3x3, ZERO3x3,
+                   ZERO3x3, ZERO3x3, 0.8*IDEN3x3;
+  meas_noise_R_ << 0.2*IDEN3x3, ZERO3x3,
+                   ZERO3x3, 0.3*IDEN3x3;
 
   meas_matrix_C_ << IDEN3x3, ZERO3x3, ZERO3x3,
                     ZERO3x3, IDEN3x3, ZERO3x3;
@@ -131,17 +131,22 @@ void BiasEstimator::state_propagation( )
     {
       /* don't use stale ctrl_input_u_ for some reason */
       n_stprops_since_update_++;
-      if( n_stprops_since_update_ > 3 )
-        ctrl_input_u_ << 0.0, 0.0, 0.0;
-
-      /* propagate state forward */
-      best_estimate_ = sys_A_ * best_estimate_ +
-                       sys_B_ * (ctrl_input_u_.cwiseProduct(input_shaping));
-      state_cov_P_ = sys_A_*state_cov_P_*sys_A_t_ + proc_noise_Q_;
-
-      /* only clip estimated bias accelerations */
-      best_estimate_.block<3,1>(6,0) = BIAS_LIM_ABS.cwiseMin(
+      if( estimator_off_ || n_stprops_since_update_ > 3 )
+      {
+        ctrl_input_u_ << 0.0, 0.0, 0.0;   // clear ctrl until further update
+        best_estimate_ = best_estimate_;  // do not propagate
+      }
+      else
+      {
+        /* propagate state forward */
+        best_estimate_ = sys_A_ * best_estimate_ +
+                         sys_B_ * (ctrl_input_u_.cwiseProduct(input_shaping));
+        state_cov_P_ = sys_A_*state_cov_P_*sys_A_t_ + proc_noise_Q_;
+
+        /* only clip estimated bias accelerations */
+        best_estimate_.block<3,1>(6,0) = BIAS_LIM_ABS.cwiseMin(
                       best_estimate_.block<3,1>(6,0).cwiseMax(-BIAS_LIM_ABS) );
+      }
 
       /* fill state information: this will be unrolled by gcc */
       for( int idx=0; idx < nStates; idx++ )

lqg_controller/src/lqr_control_bias.cpp

@@ -64,6 +64,8 @@ LQRController::LQRController(BiasEstimator &b) : nh_(),
   else if( _bcomp == "auto" || _bcomp == "off" )
     bias_compensation_req_ = false;               // off, or on by service call
 
+  bias_compensation_off_ = (_bcomp == "off")? true : false;   // always off
+  
   f_biases_ << 0.0, 0.0, 0.0;
 
 
@@ -112,16 +114,22 @@ void LQRController::initLqrSystem()
 
 bool LQRController::biasEnableServer( BoolServReq& rq, BoolServRsp& rp )
 {
+  if( bias_compensation_off_ )
+  {
+    ROS_WARN( "LQR: Bias compensation remains off throughout." );
+    return false;       // service unsuccessful
+  }
+  
   bias_compensation_req_ = rq.data;
   if( bias_compensation_req_ )
   {
     ROS_WARN( "LQR: Bias compensation active!" );
-    bias_est_.markEnabled();
+    bias_est_.enable();
   }
   else
   {
     ROS_WARN( "LQR: Bias compensation inactive!" );
-    bias_est_.clearBiases();
+    bias_est_.disable();
   }
   return true;  // service successful
 }
@@ -158,7 +166,7 @@ void LQRController::stateCallback( const freyja_msgs::CurrentState::ConstPtr &ms
     rsmtx.lock();
     reduced_state_.head<6>() = current_state.head<6>() - reference_state_.head<6>();
     /* yaw-error is done differently */
-    reduced_state_(6) = calcYawError( reduced_state_(6), reference_state_(6) );
+    reduced_state_(6) = calcYawError( current_state_(6), reference_state_(6) );
     rsmtx.unlock();
   }