add dynamic mass correction, support automatic bias estimation

*mass estimation is enabled, correction is off by default
*bias estimation param type change to string: "auto" default
*add parameters to launch file

Author: ajshank

freyja_controller.launch

@@ -9,26 +9,26 @@
 
   <!-- allowed autopilots: "asctec" or "arducopter" --> 
   <!-- must provide vicon_topic -->
-  <arg name="autopilot" default="arducopter"/>
+  <arg name="autopilot"       default="arducopter"/>
   <arg name="vicon_topic"/>
   <!-- use "device:baudrate" for apm, "device" for asctec -->
-  <arg name="comm_port" default="/dev/ttyUSB0:57600"/>
-  <arg name="total_mass" default="0.95"/>
-  <arg name="thrust_scaler" default="200.0"/>
+  <arg name="comm_port"       default="/dev/ttyUSB0:57600"/>
+  <arg name="total_mass"      default="0.95"/>
+  <arg name="thrust_scaler"   default="200.0"/>
 
   <!-- true if providing your own trajectory source -->
   <arg name="use_external_trajectory" default="false"/>
   <arg name="use_velctrl_only" default="false"/>
-  <arg name="bias_compensation" default="false" />
+  <arg name="bias_compensation" default="auto" />
   <arg name="start_rosbag" default="false"/>
 
 
   <!-- state source can be: "apm", "asctec", "vicon" or "onboard_camera" -->
   <!-- implemented filters: "median", "gauss" and "lwma" -->
   <node name="state_manager" pkg="state_manager" type="state_manager_node">
-    <param name="state_source" type="string" value="vicon" />
-    <param name="filter_type" type="string" value="median" />
-    <param name="vicon_topic" type="string" value="$(arg vicon_topic)" />
+    <param name="state_source"  type="string"   value="vicon" />
+    <param name="filter_type"   type="string"   value="median" />
+    <param name="vicon_topic"   type="string"   value="$(arg vicon_topic)" />
   </node>
 
   <!-- an example trajectory node for reference -->
@@ -41,17 +41,19 @@
   <!-- control node (see velctrl flag above) -->
   <node name="lqg_controller" pkg="lqg_control" type="lqg_control_node"
   			output="screen" if="$(eval not use_velctrl_only)">
-	  <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="70" />
-  	<param name="bias_compensation" type="bool" value="$(arg bias_compensation)" />
+	  <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="bias_compensation" type="string" value="$(arg bias_compensation)" />
+  	<param name="mass_estimation"   type="bool"   value="true" />
+  	<param name="mass_correction"   type="bool"   value="false" />
   </node>
 
   <node name="lqr_vel_controller" pkg="lqr_control" type="lqr_vel_ctrl_node"
   			if="$(eval use_velctrl_only)">
-    <param name="controller_rate" type="int" value="30" />
-    <param name="total_mass" type="double" value="$(arg total_mass)" />
+    <param name="controller_rate"   type="int"    value="30" />
+    <param name="total_mass"        type="double" value="$(arg total_mass)" />
   </node>
 
   <!-- autopilot communication node -->

lqg_controller/include/lqr_control_bias.h

@@ -17,6 +17,7 @@
 #include <ros/ros.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <std_srvs/SetBool.h>
+#include <std_msgs/Float32.h>
 
 #include <freyja_msgs/CurrentState.h>
 #include <freyja_msgs/CtrlCommand.h>
@@ -70,6 +71,10 @@ class LQRController
   Eigen::Matrix<double, 3, 1> f_biases_;
   std::thread bias_handler_thread_;
 
+  /*Dynamic mass estimation and compensation */
+  bool enable_dyn_mass_correction_;
+  bool enable_dyn_mass_estimation_;
+  
   public:
     LQRController( BiasEstimator & );
     void initLqrSystem();
@@ -91,4 +96,8 @@ class LQRController
 
     /* helper function to calculate yaw error */
     inline double calcYawError( const double&, const double& ) __attribute__((always_inline));
+    
+    /* estimate actual mass in flight */
+    void estimateMass( const Eigen::Matrix<double, 4, 1> &, ros::Time & );
+    ros::Publisher est_mass_pub_;
 };

lqg_controller/src/lqr_control_bias.cpp

@@ -44,6 +44,7 @@ LQRController::LQRController(BiasEstimator &b) : nh_(),
                     ( "/rpyt_command", 1, true );
   controller_debug_pub_ = nh_.advertise <freyja_msgs::ControllerDebug>
                     ( "/controller_debug", 1, true );
+  est_mass_pub_ = nh_.advertise<std_msgs::Float32>( "/freyja_estimated_mass", 1, true );
 
   /* Timer to run the LQR controller perdiodically */
   float controller_period = 1.0/controller_rate_;
@@ -56,9 +57,25 @@ LQRController::LQRController(BiasEstimator &b) : nh_(),
   have_reference_update_ = false;  
 
   /* Bias compensation parameters */
-  bool _bcomp = true;
-  priv_nh_.param( "bias_compensation", bias_compensation_req_, _bcomp );
+  std::string _bcomp = "auto";
+  priv_nh_.param( "bias_compensation", _bcomp, _bcomp );
+  if( _bcomp == "on" )
+    bias_compensation_req_ = true;                // always on (be careful!!)
+  else if( _bcomp == "auto" || _bcomp == "off" )
+    bias_compensation_req_ = false;               // off, or on by service call
+  
   f_biases_ << 0.0, 0.0, 0.0;
+  
+  
+  /* Mass estimation */
+  enable_dyn_mass_correction_ = false;
+  priv_nh_.param( "mass_correction", enable_dyn_mass_correction_, bool(false) );
+  priv_nh_.param( "mass_estimation", enable_dyn_mass_estimation_, bool(true) );
+  if( enable_dyn_mass_correction_ )
+  {
+    enable_dyn_mass_estimation_ = true;
+    ROS_WARN( "LQR: Mass correction active at init! This is discouraged." );
+  }
 }
 
 void LQRController::initLqrSystem()
@@ -240,6 +257,43 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
   /* Tell bias estimator about new control input */
   if( bias_compensation_req_ )
     bias_est_.setControlInput( control_input );
+    
+  /* Update the total flying mass if requested */
+  if( enable_dyn_mass_estimation_ )
+    estimateMass( control_input, tnow );
+}
+
+void LQRController::estimateMass( const Eigen::Matrix<double, 4, 1> &c, ros::Time &t )
+{
+  /* Experimental module that estimates the true mass of the system in air.
+    It uses the provided mass and observes the deviation from expected output
+    of the controller - and attributes *all* of that error to an incorrect
+    mass parameter. This is recommended only if mass may be off by ~200-300g.
+    Errors larger than that can induce major second-order oscillations, and are
+    usually better addressed elsewhere in the architecture (or get a better scale).
+  */
+  static float prev_estimated_mass = total_mass_;
+  static std_msgs::Float32 estmass;
+  static ros::Time t_last = ros::Time::now();
+
+  if( (t-t_last).toSec() < 3.0 )
+    return;
+
+  float ctrl_effort = c(2) + 9.81;
+  float estimated_mass = total_mass_*(9.81 - ctrl_effort)/9.81;
+  // basic low-pass filter to prevent wild jitter
+  estimated_mass = (prev_estimated_mass + estimated_mass)/2.0;
+  
+  estmass.data = estimated_mass;
+  est_mass_pub_.publish( estmass );
+  
+  /* if correction is allowed- clip between min and max */
+  if( enable_dyn_mass_correction_ )
+    total_mass_ = std::min( 2.0f, std::max( 0.8f, estimated_mass ) );
+  
+  // book-keeping
+  t_last = t;
+  prev_estimated_mass = estimated_mass;
 }
 
 int main( int argc, char** argv )