Index: trunk/extremum_seeking/esc_approx/include/esc_approx/approx_esc_1d.h
===================================================================
--- trunk/extremum_seeking/esc_approx/include/esc_approx/approx_esc_1d.h	(revision 11)
+++ trunk/extremum_seeking/esc_approx/include/esc_approx/approx_esc_1d.h	(revision 13)
@@ -37,4 +37,5 @@
 	std::vector<double> monitor();
 	std::vector<std::string> monitorNames();
+	void reset();
 
 };
Index: trunk/extremum_seeking/esc_approx/include/esc_approx/approx_esc_2d.h
===================================================================
--- trunk/extremum_seeking/esc_approx/include/esc_approx/approx_esc_2d.h	(revision 11)
+++ trunk/extremum_seeking/esc_approx/include/esc_approx/approx_esc_2d.h	(revision 13)
@@ -40,4 +40,5 @@
 	std::vector<double> monitor();
 	std::vector<std::string> monitorNames();
+	void reset();
 
 };
Index: trunk/extremum_seeking/esc_approx/src/approx_esc_1d.cpp
===================================================================
--- trunk/extremum_seeking/esc_approx/src/approx_esc_1d.cpp	(revision 11)
+++ trunk/extremum_seeking/esc_approx/src/approx_esc_1d.cpp	(revision 13)
@@ -35,9 +35,9 @@
 	k_grad_ = k_grad;
 	init_vel_ = init_vel;
-	sample_ = 0;
 	sampling_ = sampling;
-	ptr_ = 0;
 	states_.resize(data_size);
 	obj_vals_.resize(data_size);
+	sample_ = 0;
+	ptr_ = 0;
 	initialized_ = true;
 }
@@ -111,2 +111,7 @@
 	return std::vector<std::string>();
 }
+
+void ApproxESC1D::reset(){
+	sample_ = 0;
+	ptr_ = 0;
+}
Index: trunk/extremum_seeking/esc_approx/src/approx_esc_2d.cpp
===================================================================
--- trunk/extremum_seeking/esc_approx/src/approx_esc_2d.cpp	(revision 11)
+++ trunk/extremum_seeking/esc_approx/src/approx_esc_2d.cpp	(revision 13)
@@ -33,8 +33,8 @@
 	k_grad_ = k_grad;
 	init_vel_ = init_vel;
+	sampling_ = sampling;
 	sample_ = 0;
-	sampling_ = sampling;
 	ptr_ = 0;
-	states_ = Eigen::MatrixXf::Zero(2,data_size);
+	states_ = Eigen::MatrixXf::Zero(2,data_size_);
 	obj_vals_ = Eigen::VectorXf::Zero(data_size_);
 	state_curr_.resize(2);
@@ -133,2 +133,9 @@
 	return std::vector<std::string>();
 }
+
+void ApproxESC2D::reset(){
+	sample_ = 0;
+	ptr_ = 0;
+	states_ = Eigen::MatrixXf::Zero(2,data_size_);
+	obj_vals_ = Eigen::VectorXf::Zero(data_size_);
+}
Index: trunk/extremum_seeking/esc_common/include/esc_common/esc.h
===================================================================
--- trunk/extremum_seeking/esc_common/include/esc_common/esc.h	(revision 11)
+++ trunk/extremum_seeking/esc_common/include/esc_common/esc.h	(revision 13)
@@ -15,31 +15,53 @@
 #include <string>
 #define PI 3.141592654
+
+/// Superclass for extremum seeking control algorithms.
 class ESC
 {
-public:
-
-	enum inputType { inputStateValue, inputValue };
-	enum outputType { outputVelocity, outputPosition };
+  public:
+    /// Controller input type
+    enum inputType
+    {
+      inputStateValue, ///< State-value input.
+      inputValue       ///< Value input.
+    };
+	
+    /// Controller output type.
+    enum outputType
+    {
+      outputVelocity,  ///< Velocity reference output.
+      outputPosition   ///< Position reference output.
+    };
 
 public:
-	virtual ~ESC() { }
+    virtual ~ESC() { }
 
-	virtual std::vector<std::string> monitorNames() { return std::vector<std::string>(); }
-	virtual std::vector<double> monitor() { return std::vector<double>(); }
+    /// Get internal monitor variable names.
+    virtual std::vector<std::string> monitorNames() { return std::vector<std::string>(); }
+	
+    /// Get internal monitor variables.
+    virtual std::vector<double> monitor() { return std::vector<double>(); }
 
-	virtual inputType getInputType() = 0;
-	virtual outputType getOutputType() = 0;
+    /// Get controller input type.
+    virtual inputType getInputType() = 0;
+	
+    /// Get controller output type.
+    virtual outputType getOutputType() = 0;
 
-	virtual std::vector<double> step(std::vector<double> state, double obj_val)
-	{
-		return step(obj_val);
-	}
+    /// Control step function for value-input control algorithms.
+    virtual std::vector<double> step(std::vector<double> state, double obj_val)
+    {
+      return step(obj_val);
+    }
 
-	virtual std::vector<double> step(double obj_val)
-	{
-		return std::vector<double>();
-	}
+    /// Control step function for state-input control algorithms.
+    virtual std::vector<double> step(double obj_val)
+    {
+      return std::vector<double>();
+    }
+
+    /// Reset control algorithm to initial conditions.
+    virtual void reset() = 0;
 };
 
-
 #endif /* ESC_H_ */
Index: trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_1d.h
===================================================================
--- trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_1d.h	(revision 11)
+++ trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_1d.h	(revision 13)
@@ -47,4 +47,5 @@
 	double minPeakDetect(double e_minus);
 	double aSwitch(double e);
+	void reset();
 
 };
Index: trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_2d.h
===================================================================
--- trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_2d.h	(revision 11)
+++ trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_2d.h	(revision 13)
@@ -25,5 +25,5 @@
 
 protected:
-	double A_,M_,ddelta1_,ddelta2_,ddelta3_,A,delta_,B_, mpd_init_, w_switch_old_, a_switch1_old_, a_switch2_old_, a_switch3_old_,yr_,period_;
+	double A_,M_,ddelta1_,ddelta2_,ddelta3_,delta_,B_, mpd_init_, w_switch_old_, a_switch1_old_, a_switch2_old_, a_switch3_old_,yr_,period_;
 	double min_peak_,w_switch_;
 	std::vector<double> vel_ref_;
@@ -42,7 +42,8 @@
 	std::vector<double> monitor();
 	std::vector<std::string> monitorNames();
+	void reset();
 protected:
-	double wSwitch(double e_minus);
-	double minPeakDetect(double e_minus);
+	double wSwitch(double e);
+	double minPeakDetect(double e);
 	double aSwitch1(double e);
 	double aSwitch2(double e);
Index: trunk/extremum_seeking/esc_nn/src/nn_esc_1d.cpp
===================================================================
--- trunk/extremum_seeking/esc_nn/src/nn_esc_1d.cpp	(revision 11)
+++ trunk/extremum_seeking/esc_nn/src/nn_esc_1d.cpp	(revision 13)
@@ -134,2 +134,12 @@
 
 }
+
+void NNESC1D::reset(){
+	w_switch_old_ = 0;
+	a_switch_old_ = A;
+	yr_ = 0;
+	min_peak_ = 0;
+	vel_ref_ = 0;
+	w_switch_ = 0;
+	min_peak_detect_init_ = false;
+}
Index: trunk/extremum_seeking/esc_nn/src/nn_esc_2d.cpp
===================================================================
--- trunk/extremum_seeking/esc_nn/src/nn_esc_2d.cpp	(revision 11)
+++ trunk/extremum_seeking/esc_nn/src/nn_esc_2d.cpp	(revision 13)
@@ -20,5 +20,5 @@
 	ddelta1_ = 0;
 	ddelta2_ = 0;
-	ddelta2_ = 0;
+	ddelta3_ = 0;
 	delta_ = 0;
 	B_ = 0;
@@ -51,4 +51,8 @@
 	monitor_vals.push_back(min_peak_);
 	monitor_vals.push_back(w_switch_);
+	monitor_vals.push_back(yr_+ddelta1_);
+	monitor_vals.push_back(yr_+ddelta2_);
+	monitor_vals.push_back(yr_+ddelta3_);
+	monitor_vals.push_back(yr_+delta_);
 
 	return monitor_vals;
@@ -61,4 +65,8 @@
 	monitor_names.push_back("minimum peak detector output");
 	monitor_names.push_back("w switch value");
+	monitor_names.push_back("threshold value 1");
+	monitor_names.push_back("threshold value 2");
+	monitor_names.push_back("threshold value 3");
+	monitor_names.push_back("threshold value 4");
 
 	return monitor_names;
@@ -75,10 +83,10 @@
 	ddelta1_ = ddelta1;
 	ddelta2_ = ddelta2;
-	ddelta2_ = ddelta3;
+	ddelta3_ = ddelta3;
 	delta_ = delta;
 	period_ = period;
 	w_switch_old_ = 0;
 	a_switch1_old_ = A_;
-	a_switch2_old_ = A_;
+	a_switch2_old_ = 0;
 	a_switch3_old_ = 0;
 	vel_ref_.resize(2);
@@ -99,22 +107,30 @@
 
 	if(!min_peak_detect_init_){
-		yr_ = obj_val;
+		min_peak_ = obj_val;
+		yr_ = min_peak_;
 		min_peak_detect_init_ = true;
 	}
 
 	double e = yr_ - obj_val;
-	vel_ref_[1] = aSwitch1(e)+aSwitch2(e);
-	vel_ref_[0] = aSwitch2(e)+aSwitch3(e);
-	min_peak_ = minPeakDetect(-e);
-	w_switch_ = wSwitch(-e);
+	double s[3];
+	s[0] = aSwitch1(e);
+	s[1] = aSwitch2(e);
+	s[2] = aSwitch3(e);
+
+	vel_ref_[1] = s[0]+s[1];
+	vel_ref_[0] = s[1]+s[2];
+
+	min_peak_ = minPeakDetect(e);
+	w_switch_ = wSwitch(e);
 	yr_ = yr_ + (w_switch_+min_peak_)*period_;
+
 	return vel_ref_;
 }
-double NNESC2D::wSwitch(double e_minus){
-	if(e_minus<-delta_){
+double NNESC2D::wSwitch(double e){
+	if(e>delta_){
 		w_switch_old_ = 0;
 		return 0;
 	}
-	else if(e_minus>delta_){
+	else if(e<-delta_){
 		w_switch_old_ = B_;
 		return B_;
@@ -125,6 +141,6 @@
 }
 
-double NNESC2D::minPeakDetect(double e_minus){
-	if(e_minus>0)
+double NNESC2D::minPeakDetect(double e){
+	if(e<=0)
 		return 0;
 	else
@@ -146,11 +162,13 @@
 
 double NNESC2D::aSwitch2(double e){
+
 	if( e < -ddelta2_ ){
-		a_switch2_old_ = 0;
-		return 0;
-	}
-	else if(e>ddelta2_){
 		a_switch2_old_ = A_;
 		return A_;
+
+	}
+	else if(e>ddelta2_){
+		a_switch2_old_ = 0;
+		return 0;
 	}
 	else
@@ -170,2 +188,17 @@
 		return a_switch3_old_;
 }
+
+void NNESC2D::reset(){
+	w_switch_old_ = 0;
+	a_switch1_old_ = A_;
+	a_switch2_old_ = 0;
+	a_switch3_old_ = 0;
+	vel_ref_.resize(2);
+	vel_ref_[0] = 0;
+	vel_ref_[1] = 0;
+	yr_ = 0;
+	min_peak_ = 0;
+	w_switch_ = 0;
+	min_peak_detect_init_ = false;
+	initialized_ = true;
+}
Index: trunk/extremum_seeking/esc_sm/CMakeLists.txt
===================================================================
--- trunk/extremum_seeking/esc_sm/CMakeLists.txt	(revision 11)
+++ trunk/extremum_seeking/esc_sm/CMakeLists.txt	(revision 13)
Index: trunk/extremum_seeking/esc_sm/include/esc_sm/sm_esc_1d.h
===================================================================
--- trunk/extremum_seeking/esc_sm/include/esc_sm/sm_esc_1d.h	(revision 11)
+++ trunk/extremum_seeking/esc_sm/include/esc_sm/sm_esc_1d.h	(revision 13)
@@ -36,4 +36,5 @@
 	std::vector<double> monitor();
 	std::vector<std::string> monitorNames();
+	void reset();
 protected:
 	int sign(double value);
Index: trunk/extremum_seeking/esc_sm/src/sm_esc_1d.cpp
===================================================================
--- trunk/extremum_seeking/esc_sm/src/sm_esc_1d.cpp	(revision 11)
+++ trunk/extremum_seeking/esc_sm/src/sm_esc_1d.cpp	(revision 13)
@@ -87,2 +87,6 @@
 }
 
+void SMESC1D::reset(){
+	driving_input_ = 0;
+	driving_input_init_ = false;
+}
Index: trunk/extremum_seeking/esc_test/include/esc_test/esc_test.h
===================================================================
--- trunk/extremum_seeking/esc_test/include/esc_test/esc_test.h	(revision 11)
+++ trunk/extremum_seeking/esc_test/include/esc_test/esc_test.h	(revision 13)
@@ -11,4 +11,5 @@
     
     ESCSystem *system_;
+    std::vector<float> vel_, pos_;
     
   protected:
Index: trunk/extremum_seeking/esc_test/include/esc_test/function.h
===================================================================
--- trunk/extremum_seeking/esc_test/include/esc_test/function.h	(revision 11)
+++ trunk/extremum_seeking/esc_test/include/esc_test/function.h	(revision 13)
@@ -9,11 +9,11 @@
 };
 
-class Gauss : public ESCFunction
+class Gauss1D : public ESCFunction
 {
   private:
-    float a_, b_, c_;
+    float a_, b_, c_, d_;
 
   public:
-    Gauss(float a=1, float b=0, float c=1) : a_(a), b_(b), c_(c) { }
+    Gauss1D(float a=0, float b=1, float c=0, float d=1) : a_(a), b_(b), c_(c), d_(d) { }
     
     std::vector<float> init()
@@ -29,7 +29,54 @@
         throw std::runtime_error("invalid state size");
         
-      return a_*std::exp(-(state[0]-b_)*(state[0]-b_)/(2*c_*c_));
+      return a_ + b_*std::exp(-(state[0]-c_)*(state[0]-c_)/(2*d_*d_));
     }
 };
+
+class Gauss2D : public ESCFunction
+{
+  private:
+    float a_, b_;
+    std::vector<float> c_, d_;
+
+  public:
+    Gauss2D(float a, float b, std::vector<float> c, std::vector<float> d)
+    {
+      if (c.empty())
+      {
+        c_.push_back(0);
+        c_.push_back(0);
+      }
+      else
+        c_ = c;
+        
+      if (d.empty())
+      {
+        d_.push_back(1);
+        d_.push_back(1);
+      }
+      else
+        d_ = d;
+    }
+    
+    std::vector<float> init()
+    {
+      std::vector<float> state;
+      state.push_back(0);
+      state.push_back(0);
+      return state;
+    }
+    
+    float value(const std::vector<float> &state) const
+    {
+      if (state.size() != 1)
+        throw std::runtime_error("invalid state size");
+      
+      double exponent = (state[0]-c_[0])*(state[0]-c_[0])/(2*d_[0]*d_[0]) + 
+                        (state[1]-c_[1])*(state[1]-c_[1])/(2*d_[1]*d_[1]);
+      
+      return a_ + b_*std::exp(-exponent);
+    }
+};
+
 
 class ESCSystem
Index: trunk/extremum_seeking/esc_test/launch/approx_esc_1d.launch
===================================================================
--- trunk/extremum_seeking/esc_test/launch/approx_esc_1d.launch	(revision 13)
+++ trunk/extremum_seeking/esc_test/launch/approx_esc_1d.launch	(revision 13)
@@ -0,0 +1,19 @@
+<launch>
+  <!-- Approximation based ESC Node (1D) -->
+	<node name="approx_esc_1d" pkg="esc_approx" type="esc_1d" output="screen">
+		<param name="data_size" type="int" value="10" />
+		<param name="poly_degree" type="int" value="3" />
+		<param name="k_grad" type="double" value="0.001" />
+		<param name="init_vel" type="double" value="0.00001" />
+		<param name="sampling" type="int" value="1" />
+		<param name="monitor" type="bool" value="false" />
+		<param name="period" type="double" value="0.01" />
+
+		<remap from="/approx_esc_1d/esc_in" to="/esc_test/esc_in"/>
+		<remap from="/approx_esc_1d/vel_ref" to="/esc_test/vel_ref"/>
+	</node> 
+
+	<node name="esc_test" pkg="esc_test" type="esc_test" output="screen"/>
+
+	<node name="plot" pkg="rxtools" type="rxplot" output="screen" args="/esc_test/obj_val/data /esc_test/vel_ref/data[0] /esc_test/esc_in/state[0]"/>
+</launch>
Index: trunk/extremum_seeking/esc_test/launch/nn_esc_1d.launch
===================================================================
--- trunk/extremum_seeking/esc_test/launch/nn_esc_1d.launch	(revision 13)
+++ trunk/extremum_seeking/esc_test/launch/nn_esc_1d.launch	(revision 13)
@@ -0,0 +1,19 @@
+<launch>
+  <!-- Neural Network ESC Node (1D) -->
+	<node name="nn_esc_1d" pkg="esc_nn" type="esc_1d" output="screen">
+		<param name="A" type="double" value="0.002" />
+		<param name="B" type="double" value="12" />
+		<param name="M" type="double" value="5" />
+		<param name="ddelta" type="double" value="0.05" />
+		<param name="delta" type="double" value="0.1" />
+		<param name="period" type="double" value="0.01" />
+		<param name="monitor" type="bool" value="true" />
+
+		<remap from="/nn_esc_1d/obj_val" to="/esc_test/obj_val"/>
+		<remap from="/nn_esc_1d/vel_ref" to="/esc_test/vel_ref"/>
+	</node>
+
+	<node name="esc_test" pkg="esc_test" type="esc_test" output="screen"/>
+
+	<node name="plot" pkg="rxtools" type="rxplot" output="screen" args="/esc_test/obj_val/data,/nn_esc_1d/monitor_out/values[0] /esc_test/vel_ref/data[0] /esc_test/esc_in/state[0]"/>
+</launch>
Index: trunk/extremum_seeking/esc_test/launch/perturb_esc_1d.launch
===================================================================
--- trunk/extremum_seeking/esc_test/launch/perturb_esc_1d.launch	(revision 13)
+++ trunk/extremum_seeking/esc_test/launch/perturb_esc_1d.launch	(revision 13)
@@ -0,0 +1,22 @@
+<launch>
+  <!-- Perturbation based ESC Node -->
+	<node name="perturb_esc" pkg="esc_perturb" type="esc" output="screen">
+		<param name="sin_amp" type="double" value="0.05" />
+		<param name="sin_freq" type="double" value="15" />
+		<param name="corr_gain" type="double" value="-1000" />
+		<param name="high_pass_pole" type="double" value="5" />
+		<param name="low_pass_pole" type="double" value="1" />
+		<param name="comp_pole" type="double" value="700" />
+		<param name="comp_zero" type="double" value="10" />
+		<param name="monitor" type="bool" value="true" />
+		<param name="period" type="double" value="0.01" />
+
+		<remap from="/perturb_esc/esc_in" to="/esc_test/esc_in"/>
+		<remap from="/perturb_esc/pos_ref" to="/esc_test/pos_ref"/>
+	</node>
+
+	<node name="esc_test" pkg="esc_test" type="esc_test" output="screen"/>
+
+	<node name="plot" pkg="rxtools" type="rxplot" output="screen" args="/esc_test/obj_val/data /esc_test/pos_ref/data[0] /esc_test/esc_in/state[0]"/>
+</launch>
+
Index: trunk/extremum_seeking/esc_test/launch/sm_esc_1d.launch
===================================================================
--- trunk/extremum_seeking/esc_test/launch/sm_esc_1d.launch	(revision 13)
+++ trunk/extremum_seeking/esc_test/launch/sm_esc_1d.launch	(revision 13)
@@ -0,0 +1,18 @@
+<launch>
+  <!-- Sliding mode ESC Node (1D) -->
+	<node name="sm_esc_1d" pkg="esc_sm" type="esc_1d" output="screen">
+		<param name="rho" type="double" value="0.01" />
+		<param name="k" type="double" value="0.002" />
+		<param name="alpha" type="double" value="0.1" />
+		<param name="period" type="double" value="0.01" />
+		<param name="monitor" type="bool" value="true" />
+
+		<remap from="/sm_esc_1d/obj_val" to="/esc_test/obj_val"/>
+		<remap from="/sm_esc_1d/vel_ref" to="/esc_test/vel_ref"/>
+	</node> 
+
+	<node name="esc_test" pkg="esc_test" type="esc_test" output="screen"/>
+
+	<node name="plot" pkg="rxtools" type="rxplot" output="screen" args="/esc_test/obj_val/data /esc_test/vel_ref/data[0] /esc_test/esc_in/state[0]"/>
+</launch>
+
Index: trunk/extremum_seeking/esc_test/src/esc_test.cpp
===================================================================
--- trunk/extremum_seeking/esc_test/src/esc_test.cpp	(revision 11)
+++ trunk/extremum_seeking/esc_test/src/esc_test.cpp	(revision 13)
@@ -7,12 +7,12 @@
 void ESCTest::velocityCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
 {
-  system_->step(msg->data);
-  publish();
+  vel_ = msg->data;
+  pos_.clear();
 }
 
 void ESCTest::positionCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
 {
-  system_->set(msg->data);
-  publish();
+  pos_ = msg->data;
+  vel_.clear();
 }
 
@@ -39,6 +39,4 @@
   vel_sub_ = nh_.subscribe("vel_ref", 1, &ESCTest::velocityCallback, this);
   pos_sub_ = nh_.subscribe("pos_ref", 1, &ESCTest::positionCallback, this);
-
-  publish();  
 }
 
@@ -47,5 +45,16 @@
   ROS_INFO("Spinning");
 
-  ros::spin();
+  ros::Rate loop_rate(100);
+  while (nh_.ok())
+  {
+    if (!vel_.empty())
+      system_->step(vel_);
+    else if (!pos_.empty())
+      system_->set(pos_);
+    publish();
+
+    loop_rate.sleep();
+    ros::spinOnce();
+  }  
 }
 
@@ -54,7 +63,21 @@
   ros::init(argc, argv, "esc_test");
   
+  ESCFunction *function;
+  
+  if (argc > 1 && strcmp(argv[1], "-2") == 0)
+  {
+    std::vector<float> mean, sigma;
+    mean.push_back(0.5);
+    mean.push_back(0.2);
+    sigma.push_back(0.15);
+    sigma.push_back(0.15);
+    function = new Gauss2D(2, -1, mean, sigma);
+  }
+  else
+    function = new Gauss1D(2.5, -1.5, 0.4, 0.27/sqrt(2.0));
+  
   ESCTest test;
   
-  test.init(new ESCSystem(new Gauss(-1, 2, 1)));
+  test.init(new ESCSystem(function));
   test.spin();