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();