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